I try to transpose from one MatrixX* into another (not quadratic but with correct dimensions). However the best I could find is the Transpose< Derived > ::transpose() function.
Is there even a call which puts the result into an already allocated Matrix instead of allocating a new one?
EDIT:
Actually I was using Eigen::Map on top of the Matrix.
typedef Eigen::Matrix<std::uint8_t, Eigen::Dynamic, Eigen::Dynamic> matrix_type;
typedef Eigen::Map<matrix_type> map_type;
const map_type src ( src_ptr , width , height );
map_type dest( dest_ptr, height, width );
map.transposeInPlace();
Using transposeInPlace() triggers an assert in Derived& DenseBase<Derived>
::lazyAssign(const DenseBase<OtherDerived>& other).
Try to use transposeInPlace() function
Here is the documentation: http://eigen.tuxfamily.org/dox/TutorialMatrixArithmetic.html
For in-place transposition, as for instance in a = a.transpose(),
simply use the transposeInPlace() function:
MatrixXf a(2,3); a << 1, 2, 3, 4, 5, 6;
cout << "Here is the initial matrix a:\n" << a << endl;
a.transposeInPlace();
cout << "and after being transposed:\n" << a << endl;
UPDATE: As Zeta mentioned in comment, matrix object should be resizable - this is always true for all MatrixX* objects.
Using Eigen::Map on top of a Matrix indeed results in an assert, since it seems like transposeInPlace is not possible yet for Eigen::Map (AKA a bug).
Luckily for me, using regular ::transpose was fine, since Eigen uses late assigning of data.
Related
I have a publish-subscribe type of a node that receives pose information (position and orientation) from the subscribed data stream and it should compute the inverse and publish out.
In order to do so I'm creating a 4-by-4 homogeneous transformation matrix from the original pose data.
Inverse it using the Eigen C++ template library, convert the transformation matrix back to position and orientation form and publish it.
When I plotted the published data stream I noticed some noise so I ended up publishing the original data too for comparison, here is what I did:
convert original_pose to TF matrix, named as original_TF
convert original_TF back to pose, named as original_pose_
publish original_pose_
inverse original_TF assign to inverted_TF
convert inverted_TF to pose, named as inverted_pose_
publish inverted_pose_
When I plot the X, Y, Z position fields, I'm seeing a significant amount of noise (spikes and notches in the visual below) in the inverted pose data. Since I'm using the same functions to convert the original pose to TF and back, I know that those equations aren't the source of the noise.
Blue is the original, whereas red is the inverted.
Here is the code. Really nothing extraordinary.
bool inverse_matrix(std::vector<std::vector<double> > & input, std::vector<std::vector<double> > & output)
{
// TODO: Currently only supports 4-by-4 matrices, I can make this configurable.
// see https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html
Eigen::Matrix4d input_matrix;
Eigen::Matrix4d output_matrix;
Eigen::VectorXcd input_eivals;
Eigen::VectorXcd output_eivals;
input_matrix << input[0][0], input[0][1], input[0][2], input[0][3],
input[1][0], input[1][1], input[1][2], input[1][3],
input[2][0], input[2][1], input[2][2], input[2][3],
input[3][0], input[3][1], input[3][2], input[3][3];
cout << "Here is the matrix input:\n" << input_matrix << endl;
input_eivals = input_matrix.eigenvalues();
cout << "The eigenvalues of the input_eivals are:" << endl << input_eivals << endl;
if(input_matrix.determinant() == 0) { return false; }
output_matrix = input_matrix.inverse();
cout << "Here is the matrix output:\n" << output_matrix << endl;
output_eivals = output_matrix.eigenvalues();
cout << "The eigenvalues of the output_eivals are:" << endl << output_eivals << endl;
// Copy output_matrix to output
for (int i = 0; i < 16; ++i)
{
int in = i/4;
int im = i%4;
output[in][im] = output_matrix(in, im);
}
return true;
}
-- Edit 1 --
I printed out the eigenvalues of the input and output matrices of the inverse_matrix function.
Here is the matrix input:
0.99916 -0.00155684 -0.0409514 0.505506
0.00342358 -0.992614 0.121267 0.19625
-0.0408377 -0.121305 -0.991775 1.64257
0 0 0 1
The eigenvalues of the input_eivals are:
(1,0)
(-0.992614,0.121312)
(-0.992614,-0.121312)
(1,0)
Here is the matrix output:
0.99916 0.00342358 -0.0408377 -0.438674
-0.00155684 -0.992614 -0.121305 0.39484
-0.0409514 0.121267 -0.991775 1.62597
-0 -0 0 1
The eigenvalues of the output_eivals are:
(1,0)
(-0.992614,0.121312)
(-0.992614,-0.121312)
(1,0)
-- Edit 2 --
I don't quite understand what you are plotting. Is it original_pose.{X,Y,Z} and inverted_pose.{X,Y,Z}? Then the "spikes" will really depend on the orientation-part of the matrix.
I am plotting original_pose_{position.x, position.y, position.z} and inverted_pose_{position.x, position.y, position.z} where the complete data that's published is <variable_name>{position.x, position.y, position.z, orientation.w, orientation.x, orientation.y, orientation.z}.
Can you elaborate on "the "spikes" will really depend on the orientation-part of the matrix."?
Also, how is your description related to the code-snippet? (I don't see any matching variable names).
I've identified that the source of the noise is the inversion, which is the item number 4 in my description: inverse original_TF assign to inverted_TF. To relate one another, I'm calling the function as follows:
isSuccess = inverse_matrix(original_TF, inverted_TF);
How do you store "poses" (is that the vector<vector> in your snippet)?
Yes, I'm storing them in 2-dimensional vectors of type double.
At any point, do you use Eigen::Transform to store transformations, or just plain Eigen::Matrix4d?
No, I'm only using Eigen::Matrix4d locally in the inverse_matrix function to be able to make use of the Eigen library for computation.
I have a polyline that I need to offset by a constant. Imagine a polyline representing the centre line of a highway, I need to offset/parallel this centre line 50 units (to the left and -50 units (to the right) to create lanes.
What function can I use to perform this offset/parallel transation? I believe I should use a MatrixXd or ArrayXd to store the polyline points? But maybe there is a better object to use to store these? Should I use the method transpose() to achieve my parallel operation? Note the polyline points are 2d not 3d.
That really depends what else you're going to be doing with the points. You can use a Matrix2Xd or MatrixX2d as well, if you want to limit the number of rows/columns. I don't know the effects on a polyline but if you just want to add a constant vector to each point, you can do a rowwise or colwise add:
#include <iostream>
#include <Eigen/Core>
using namespace Eigen;
int main()
{
MatrixXd mat(5, 2);
VectorXd vec(2);
vec << 10., 20;
mat.setRandom();
std::cout << mat << "\n\n";
mat.rowwise() += vec.transpose();
std::cout << mat << "\n\n";
return 0;
}
You have to calculate the first derivative (tangent) for each point of your polyline. Only then any parallel shift wrt this tangent at a certain point makes sense.
I wish to solve the eigenvalues for a large sparse symmetric matrix (typically of the order of 20,000 - 60,000). I have tried leveraging the SymEigsShiftSolver/SymEigsSolver but it takes considerable amount of time to produce my eigenvalues. Is there a way for a speedup?? I have attached a demo code of what I am aiming to do. Am I missing something??
int main()
{
Eigen::MatrixXd A = Eigen::MatrixXd::Random(10000, 10000);
Eigen::MatrixXd M = A.transpose() * A;
// Matrix operation objects
DenseGenMatProd<double> op_largest(M);
DenseSymShiftSolve<double> op_smallest(M);
// Construct solver object, requesting the smallest 10 eigenvalues
SymEigsShiftSolver< double, LARGEST_MAGN, DenseSymShiftSolve<double> >
eigs_smallest(&op_smallest, 10, 30, 0.0);
eigs_smallest.init();
eigs_smallest.compute();
std::cout << "Smallest 10 Eigenvalues :\n" <<
eigs_smallest.eigenvalues() << std::endl;
return 0;
}
You code example is fusing dense matrices. First thing is to properly assemble a Eigen::SparseMatrix<double> as detailed there. Then, on Spectra side, use the respective SparseSymShiftSolve helper.
Also, do not forget to compile with compiler optimization ON. This is important for such header-only lib.
I am using CImg for Image processing assignment. After calling transpose() with the following code
cout << image_subsample.width() << ","<<image_subsample.height() << "transpose:" << image_subsample.transpose().width() <<"," <<image_subsample.transpose().height() << endl;
The output is
1200,1transpose:1200,1200
the expected output of transpose is
1200,1transpose:1,1200
Am I missing something?
You actually transpose your matrix twice in your example, so the output you get is logical.
Either use get_transpose() or better store your transposed matrix somewhere before displaying its size.
CImg<> transp = img.get_transpose();
fprintf(stderr,"%d,%d",transp.width(),transp.height());
I have a fairly simple question: how to take one row of cv::Mat and get all the data in std::vector? The cv::Mat contains doubles (it can be any simple datatype for the purpose of the question).
Going through OpenCV documentation is just very confusing, unless I bookmark the page I can not find a documentation page twice by Googling, there's just to much of it and not easy to navigate.
I have found the cv::Mat::at(..) to access the Matrix element, but I remember from C OpenCV that there were at least 3 different ways to access elements, all of them used for different purposes... Can't remember what was used for which :/
So, while copying the Matrix element-by-element will surely work, I am looking for a way that is more efficient and, if possible, a bit more elegant than a for loop for each row.
It should be as simple as:
m.row(row_idx).copyTo(v);
Where m is cv::Mat having CV_64F depth and v is std::vector<double>
Data in OpenCV matrices is laid out in row-major order, so that each row is guaranteed to be contiguous. That means that you can interpret the data in a row as a plain C array. The following example comes directly from the documentation:
// compute sum of positive matrix elements
// (assuming that M is double-precision matrix)
double sum=0;
for(int i = 0; i < M.rows; i++)
{
const double* Mi = M.ptr<double>(i);
for(int j = 0; j < M.cols; j++)
sum += std::max(Mi[j], 0.);
}
Therefore the most efficient way is to pass the plain pointer to std::vector:
// Pointer to the i-th row
const double* p = mat.ptr<double>(i);
// Copy data to a vector. Note that (p + mat.cols) points to the
// end of the row.
std::vector<double> vec(p, p + mat.cols);
This is certainly faster than using the iterators returned by begin() and end(), since those involve extra computation to support gaps between rows.
From the documentation at here, you can get a specific row through cv::Mat::row, which will return a new cv::Mat, over which you can iterator with cv::Mat::begin and cv::Mat::end. As such, the following should work:
cv::Mat m/*= initialize */;
// ... do whatever...
cv::Mat first_row(m.row(0));
std::vector<double> v(first_row.begin<double>(), first_row.end<double>());
Note that I don't know any OpenCV, but googling "OpenCV mat" led directly to the basic types documentation and according to that, this should work fine.
The matrix iterators are random-access iterators, so they can be passed to any STL algorithm, including std::sort() .
This is also from the documentiation, so you could actually do this without a copy:
cv::Mat m/*= initialize */;
// ... do whatever...
// first row begin end
std::vector<double> v(m.begin<double>(), m.begin<double>() + m.size().width);
To access more than the first row, I'd recommend the first snippet, since it will be a lot cleaner that way and there doesn't seem to be any heavy copying since the data types seem to be reference-counted.
You can also use cv::Rect
m(cv::Rect(0, 0, 1, m.cols))
will give you first row.
matrix(cv::Rect(x0, y0, len_x, len_y);
means that you will get sub_matrix from matrix whose upper left corner is (x0,y0) and size is (len_x, len_y). (row,col)
I think this works,
an example :
Mat Input(480, 720, CV_64F, Scalar(100));
cropping the 1st row of the matrix:
Rect roi(Point(0, 0), Size(720, 1));
then:
std::vector<std::vector<double> > vector_of_rows;
vector_of_rows.push_back(Input(roi));