I would like to take the inverse of a nxn matrix to use in my GraphSlam.
The issues that I encountered:
.inverse() Eigen-library (3.1.2) doesn't allow zero values, returns NaN
The LAPACK (3.4.2) library doesn't allow to use a zero determinant, but allows zero values (used example code from Computing the inverse of a matrix using lapack in C)
Seldon library (5.1.2) wouldn't compile for some reason
Did anyone successfully implemented an n x n matrix inversion code that allows negative, zero-values and a determinant of zero? Any good library (C++) recommendations?
I try to calculate the omega in the following for GraphSlam:
http://www.acastano.com/others/udacity/cs_373_autonomous_car.html
Simple example:
[ 1 -1 0 0 ]
[ -1 2 -1 0 ]
[ 0 -1 1 0 ]
[ 0 0 0 0 ]
Real example would be 170x170 and contain 0's, negative values, bigger positive values.
Given simple example is used to debug the code.
I can calculate this in matlab (Moore-Penrose pseudoinverse) but for some reason I'm not able to program this in C++.
A = [1 -1 0 0; -1 2 -1 0; 0 -1 1 0; 0 0 0 0]
B = pinv(A)
B=
[0.56 -0.12 -0.44 0]
[-0.12 0.22 -0.11 0]
[-0.44 -0.11 0.56 0]
[0 0 0 0]
For my application I can (temporarily) remove the dimension with zero's.
So I am going to remove the 4th column and the 4th row.
I can also do that for my 170x170 matrix, the 4x4 was just an example.
A:
[ 1 -1 0 ]
[ -1 2 -1 ]
[ 0 -1 1 ]
So removing the 4th column and the 4th row wouldn’t bring a zero determinant.
But I can still have a zero determinant if my matrix is as above.
This when the sum of each row or each column is zero. (Which I will have all the time in GraphSlam)
The LAPACK-solution (Moore-Penrose Inverse based) worked if the determinant was not zero (used example code from Computing the inverse of a matrix using lapack in C). But failed as a "pseudoinverse" with a determinant of zero.
SOLUTION: (all credits to Frank Reininghaus), using SVD(singular value decomposition)
http://sourceware.org/ml/gsl-discuss/2008-q2/msg00013.html
Works with:
Zero values (even full 0 rows and full 0 columns)
Negative values
Determinant of zero
A^-1:
[0.56 -0.12 -0.44]
[-0.12 0.22 -0.11]
[-0.44 -0.11 0.56]
If all you want is to solve problem of the form Ax=B (or equivalently compute products of the form A^-1 * b), then I recommend you not to compute the inverse or pseudo-inverse of A, but directly solve for Ax=b using an appropriate rank-revealing solver. For instance, using Eigen:
x = A.colPivHouseholderQr().solve(b);
x = A.jacobiSvd(ComputeThinU|ComputeThinV).solve(b);
Your Matlab command does not calculate the inverse in your case because the matrix has determinat zero. The pinv commmand calculates the Moore-Penrose pseudoinverse. pinv(A) has some of, but not all, the properties of inv(A).
So you are not doing the same thing in C++ and in Matlab!
Previous
As in my comment. Now as answer. You must make sure that you invert invertible matrices. That means
det A != 0
Your example matrix has determinant equals zero. This is not an invertible matrix. I hope you don't try on this one!
For example a given matrix has determinant zero if there is a full row or column of zero entries.
Are you sure it's because of the zero/negative values, and not because your matrix is non-invertible?
A matrix only has an inverse if its determinant is nonzero (mathworld link), and the matrix example you posted in the question has a zero determinant and so it has no inverse.
That should explain why those libraries do not allow you to take the inverse of the matrix given, but I can't say if the same reasoning holds for your full size 170x170 matrix.
If your matrixes is kind of covariance or weight matrices you can use "generalized cholesky inversion" instead of SVD. The results will be more acceptable for practical use
Related
I am trying to find transformation matrices between different coordinate frames. In order to rotate, we basically multiply the rotation matrices and append the translation vector to obtain the final homogeneous matrix.
Here I have attached a snippet of my code where tf_matrix and output are Eigen::Transform variables.
tf_matrix.setIdentity();
tf_matrix.rotate( output.rotation() );
tf_matrix.translate( output.translation() );
When I look at their outputs, it seems like it is generating the rotation and translation matrix into 4x4 matrices and multiplying it instead of appending the translation vector
Output:
//This is rotation matrix
output.rotation()
1 0 0
0 0.0707372 -0.997495
0 0.997495 0.0707372
//translation vector
output.translation()
0.3
0.3
0.3
//After applying rotate() and translate() tf_matrix.transform.matrix() looks like the below
1 0 0 0.3
0 0.0707372 -0.997495 -0.278027
0 0.997495 0.0707372 0.32047
0 0 0 1
//Printing just the tf_matrix.transform.rotation()
1 0 0
0 0.0707372 -0.997495
0 0.997495 0.0707372
//Printing just the tf_matrix.transform.translation()
0.3
-0.278027
0.32047
//Ideally it should look like the below
1 0 0 0.3
0 0.0707372 -0.997495 0.3
0 0.997495 0.0707372 0.3
0 0 0 1
What did I try
I tried to generate a simple 4x4 identity Eigen::Trnasform and append it to the output matrix after the rotation, but the value 1 of the identity matrix gets added
I also tried, multiply tf_matrix.col(3) += output_matrix.col(3) , but it faces similar issues as above.
I am not sure how to go about rotation because my understanding is that I need to just multiply the 3x3 rotation matrix and append/add the 3x3 translation vector to the final column of this matrix. It seems like Eigen should be able to handle this without me writing extra code. But, this rotate, translate clearly doesn't give the right answers.
Could you please point out what am I missing if any or if there's a better way to go about it.
The order of operations is reversed from what you seem to expect: see here. Suppose you have a coordinate in R3 that you want to translate (matrix Mt) and then rotate (matrix Mr), you might expect to write Vec3 = Vec3 * Mt * Mr. Many game-engines and math libraries (eg Ogre, XNA, CRYENGINE, Unity, I believe) use this order of operations. However, Eigen requires Vec3 = Mr * Mt * Vec3; in Eigen, the coordinate being passed through is a column vector, in game engines, it is a row vector. Correspondingly, the matrices in the two different forms are transposes of one another.
To solve you problem:
tf_matrix.setIdentity();
tf_matrix = output.rotation() * tf_matrix;
tf_matrix = translate * tf_matrix;
or
tf_matrix = translate * output.rotation();
The pretranslate() and premultiply() methods can also be used to do this.
boost::number::ublas contains the M::size_type lu_factorize(M& m) function. Its name suggests that it performs the LU decomposition of a given matrix m, i.e. should produce two matrices that m = L*U. There seems to be no documentation provided for this function.
It is easy to deduce that it returns 0 to indicate successful decomposition, and a non-zero value when the matrix is singular. However, it is completely unclear where is the result. Taking the matrix by reference suggests that it works in-place, however it should produce two matrices (L and U) not one. So what does it do?
There is no documentation in boost, but looking at the documentation of SciPy's lu_factor one can see, that it's not uncommon to return one result for the LU decomposition.
This is enough, because in a typical approach to LU decomposition, L's diagonal consists of ones only, as presented in this answer from Mathematics, for example.
So, it is possible to fit both L and U into one matrix, putting L in result's lower part, omitting the diagonal (which is assumed to contain only ones), and U in the upper part. For example, for a 3x3 problem the result is:
u11 u12 u13
m = l21 u22 u23
l31 l32 u33
which implies:
1 0 0
L = l21 1 0
l31 l32 1
and
u11 u12 u13
U = 0 u22 u23
0 0 u33
Inspecting boost's void lu_substitute(const M& m, vector_expression<E>& e) function, from the same namespace seems to confirm this. It solves the equation LUx = e, where both L and U are contained in its m argument in two steps.
First solve Lz = e for z, where z = Ux, using lower part of m:
inplace_solve(m, e, unit_lower_tag ());
then, having computed z = Ux (with e modified in place), Ux = e can be solved, using upper part of m:
inplace_solve(m, e, upper_tag ());
inplace_solve is mentioned in the documentation, and it:
Solves a system of linear equations with triangular form, i.e. A is triangular.
So everything seems to make sense.
The boost doesn't have document of LU factorization (a lower triangular matrix L and upper triangular matrix U), but the source code shared with the public.
If the code is hard to follow, please check the webpage by Nick Higham. It had an detailed explanation. Here are an example from the link:
Let's say we need to solve Ax = b.
(1) Make LU from input matrix, A
[3 -1 1 1]
[-1 3 1 -1] ->
[-1 -1 3 1]
[1 1 1 3]
Low
[1 0 0 0]
[-1/3 1 0 0]
[-1/3 -1/2 1 0]
[1/3 1/2 0 1]
Upper
[3 -1 1 1]
[0 8/3 4/3 -2/3]
[0 0 4 1]
[0 0 0 3]
This example looks straight forward to human but algorithm wise could be numerous steps. This is why LU Factorization came. Methodically, Relation with Gaussian Elimination, Schur Complements, and Block Implementations are some.
(2) Solve the triangular systems Ly = b and Ux = y, since then b = L(Ux).
I try to use eig_pair to get Eigen decomposition for pair of general dense square matrices A and B of the same size, such that A*eigvec = B*eigvec*diagmat(eigval), but the result doesn't match the Matlab function eig. for example:
A= [1,2;3,4] B=[2,4;5,8]
in Matlab:
[u,v] = eig(A,B)
result:
u =
-1.0000 -0.0000
0.5000 -1.0000
v =
1.0000 0
0 0.5000
in armadillo:
eig_pair(v,u,A,B)
result:
u:
9.9301e-016 -1.0000e+000
1.0000e+000 5.0000e-001
v:
0.5000
1.0000
My question is: how to get the values of u and v that match the results in Matlab?
Looking forwards to your reply!!!
Eigenvectors are not unique. If u is an eigenvector, so is m * u for all m != 0. Furthermore, the order that eig returns eigenvectors in Matlab is arbitrary. (I don't know what order Armadillo returns eigenvectors.) You could try and create a canonical order for the eigenvectors by sorting the eigenvalues, but that is problematic if you have complex eigenvalues. (Recall that real matrices can have complex eigenvalues.)
Thus, (-1.0000, 0.5000) (first column of u in Matlab) is the same eigenvector as ( -1.0000e+000, 5.0000e-001) (second column of u in Armadillo). Similarly, (-0.0000, -1.0000) is equivalent to (9.9301e-016, 1.0000e+000) when you scale by -1 and account for floating point errors. Note that there may be numerical precision errors which would cause the floating point values to compare not equal even if mathematically the numbers are equal.
If you want a canonical representation of eigenvectors, you could rescale them to have norm 1, and also multiply by -1 if the sign of the first element is negative. Of course, if the first element in the eigenvector is close to 0, this is again problematic since the value might have ended up just barely on the wrong side of zero due to numerical reasons. So come to think of it, it might be better to ensure that the largest element (after normalization)--rather than the first--is positive.
I am trying to use OpenCV's estimateAffine3D() function to get the affine transformation between two sets of coplanar points in 3D. If I hold one variable constant, I find there is a constant error in the translation component of that variable.
My test code is:
std::vector<cv::Point3f> first, second;
std::vector<uchar> inliers;
cv::Mat aff(3,4,CV_64F);
for (int i = 0; i <6; i++)
{
first.push_back(cv::Point3f(i,i%3,1));
second.push_back(cv::Point3f(i,i%3,1));
}
int ret = cv::estimateAffine3D(first, second, aff, inliers);
std::cout << aff << std::endl;
The output I expect is:
[1 0 0 0]
[0 1 0 0]
[0 0 1 0]
Edit: My expectation is incorrect. The matrix does not decompose into [R|t] for the case of constant z-coordinates.
but what I get (with some rounding for readability) is:
[1 0 0 0]
[0 1 0 0]
[0 0 0.5 0.5]
Is there a way to fix this behavior? Is there a function which does the same on sets of 2D points?
No matter how I run your code I get fine output. For example when I run it exactly as you posted it I get.
[1,0,0 ,0]
[0,1,0 ,0]
[0,0,.5,.5]
which is correct because the 4th element of a homogeneous coordinate is assumed to be 1. When I run it with 2 as the z value I get
[1,0,0 ,0]
[0,1,0 ,0]
[0,0,.8,.4]
which also works (.8*2+.4 = 2). Are you sure you didn't just read aff(2,2) wrong?
The key problem is:
Your purpose is to estimate the rotation and translation between two sets of 3D points, but the OpenCV function estimateAffine3D() is not for that purpose. As its name suggests, this function is to compute the affine transformation between two sets of 3D points. When computing the affine transformation, the constraints on the rotation matrix is not considered. Of course, the result is not correct. To obtain the rotation and translation, you need to implement the SVD based algorithm.You may search "absolute orientation" in google. This is a classic and closed-form algorithm.
I have 2D transformations stored in a plain ol' 3x3 Matrix. How can I re-format that one to a Matrix I can shove over to OpenGL in order to transform orthogonal shapes, polygons and suchlike.
How do I have to put the values so the transformations are preserved?
(On an unrelated note, is there a fast way to invert a 3x3 Matrix?)
Some explanation about transformation matrices: All the columns, except the last one, describe the orientation of a new coordinate system in the base of the current coordinate system. So the first column is the X vector of the new coordinate system, as seen from the current, the second is the new Y vector and the 3rd is the new Z. So far this only covers the rotation. The last column is used for the relative offset. The last row and the bottom most right value are used for the homogenous transformations. It's best to leave the last row 0, ..., 0, 1
In your case you're missing the Z values, so we just insert a identity transform there, so that incoming values are left as they are.
Say this is your original matrix:
xx xy tx
yx yy ty
0 0 1
This matrix is missing the Z transformation. Inserting identity means: Leave Z as is, and don't mix with the rest. So ·z = z· = 0, except zz = 1. This gives you the following matrix
↓
xx xy 0 tx
yx yy 0 ty
0 0 1 0 ←
0 0 0 1
You can apply that onto the current OpenGL matrix stack with glMultMatrix if OpenGL version is below 3 core profile. Be aware that OpenGL numbers the matrix in column major order i.e, the indices in the array go like this (hexadecimal digits)
0 4 8 c
1 5 9 d
2 6 a e
3 7 b f
This contrary to the usual C notation which is
0 1 2 3
4 5 6 7
8 9 a b
c d e f
With OpenGL-3 core and later you've to do matrix management and manipulation yourself, anyway.
EDIT for second part of question
If by inverting one means finding the matrix M^-1 for a given matrix M, so that M^1 * M = M * M^1 = 1. For 3×3 matrices the determinant inversion method requires less operations than Gauss-Jordan elemination and is thus the most efficient way to do it. Already for 4×4 matrices determinant inversion is slower than every other method. http://www.sosmath.com/matrix/inverse/inverse.html
If you know that your matrix is orthonormal, then you may just transpose the upper left part except bottom row and rightmost column, and negate the sign of the rightmost column, except the very bottom right element. This exploits the fact that for orthonormal matrices M^-1 = M^T.
Just add the fourth row and column. For example given
2 3 3
3 2 4
0 0 1
Create the following
2 3 3 0
3 2 4 0
0 0 1 0
0 0 0 1
The transformation still occurs on the x-y plane even though it is now in three-space.