Finding eigenvectors of covariance matrix to create 3D bounding sphere - c++

I'm currently in the process of writing a function to find an "exact" bounding-sphere for a set of points in 3D space. I think I have a decent understanding of the process so far, but I've gotten stuck.
Here's what I'm working with:
A) Points in 3D space
B) 3x3 covariance matrix stored in a 4x4 matrix class (referenced by cells m0,m1,m2,m3,m4,ect; instead of rows and cols)
I've found the 3 eigenvalues for the covariance matrix of the points, and I've set up a function to convert a matrix to reduced row echelon form (rref) via Gaussian elimination.
I've tested both of those functions against figures in examples I've found online, and they appear to be working correctly.
The next step is to find the eigenvectors using the equation:
(M - λ*I)*V
... where M is the covariance matrix, λ is one of the eigenvalues, I is the identity matrix, and V is the eigenvector.
However, I don't seem to be constructing the 4x3 matrix correctly before rref'ing it, as the far right column where the eigenvector components should be calculated are 0 before and after running rref. I understand why they are zero after (without any constants, the simplest solution to a linear system of equations is all coefficients of zero), but I'm at a loss as to what to put there.
Here's the function so far:
Vect eigenVector(const Matrix &M, const float eval) {
Matrix A = Matrix(M);
A -= Matrix(IDENTITY)*eval;
A.rref();
return Vect(A[m3],A[m7],A[m11]);
}
The 3x3 covariance matrix is passed as M, and the eigenvalue as eval. Matrix(IDENTITY) returns an identity matrix. m3,m7, and m11 correspond to the far-right column of a 4x3 matrix.
Here's the example 3x3 matrix (stored in a 4x4 matrix class) I'm using to test the functions:
Matrix(1.5f, 0.5f, 0.75f, 0,
0.5f, 0.5f, 0.25f, 0,
0.75f, 0.25f, 0.5f, 0,
0, 0, 0, 0);
I'm correctly (?) getting the eigenvalues of 2.097, 0.3055, 0.09756 from my other function.
eigenVector() above correctly subtracts the passed eigenvalue from the diagonal (0,0 1,1 2,2)
Matrix A after rref():
[(1, 0, 0, -0),
(-0, 1, 0, -0),
(-0, -0, 1, -0),
(0, 0, 0, -2.09694)]
For the rref() function, I'm using a translated python function found here:
http://elonen.iki.fi/code/misc-notes/python-gaussj/index.html
What should the matrix I pass to rref() look like to get an eigenvector out?
Thanks

(M - λI)V is not an equation, it's just an expression. However, (M - λI)V = 0 is. And it's the equation that relates eigenvectors to eigenvalues.
So assuming your rref function works, I would imagine that you create an augmented matrix as [(M - λI) | 0], where 0 denotes a zero-vector. This sounds like what you're doing already, so I would have to assume that your rref function is broken. Or alternatively, it doesn't know how to handle 4x4 matrices (as opposed to 4x3 matrices, which is what it would expect for an augmented matrix).

Ah, with a few more hours of grueling research, I've managed to solve my problem.
The issue is that there is no "one" set of eigenvectors but rather an infinite number with varying magnitudes.
The method I chose was to use a REF (row echelon form) instead of RREF, leaving enough information in the matrix to allow me to substitute in an arbitrary value for z, and work backwards to solve for y and x. I then normalized the vector to get a unit eigenvector, which should work for my purposes.
My final code:
Vect eigenVector(const Matrix &M, const float eVal) {
Matrix A = Matrix(M);
A -= Matrix(IDENTITY)*eVal;
A.ref();
float K = 16; // Arbitrary value
float J = -K*A[m6]; // Substitute in K to find J
float I = -K*A[m2]-J*A[m1]; // Substitute in K and J to find I
Vect eVec = Vect(I,J,K);
eVec.norm(); // Normalize eigenvector
return eVec;
}
The only oddity is that the eigenvectors come out facing in the opposite direction than I expected (they were negated!), but that's a moot problem.

Related

Have a 3D object look at a 3D object matrix?

What I have currently is causing my 3D object to become flat. But it is looking at my target.
Vector4 up;
newMatrix.SetIdentity();
up.set_x(0);
up.set_y(1);
up.set_z(0);
Vector4 zaxis = player_->transform().GetTranslation() - spider_->transform().GetTranslation();
zaxis.Normalise();
Vector4 xaxis = CrossProduct(up, zaxis);
xaxis.Normalise();
Vector4 yaxis = CrossProduct(zaxis, xaxis);
newMatrix.set_m(0, 0, xaxis.x()); newMatrix.set_m(0, 1, xaxis.y()); newMatrix.set_m(0, 2, xaxis.z());
newMatrix.set_m(1, 0, yaxis.x()); newMatrix.set_m(1, 1, yaxis.y()); newMatrix.set_m(1, 2, yaxis.z());
newMatrix.set_m(2, 0, zaxis.x()); newMatrix.set_m(2, 1, zaxis.y()); newMatrix.set_m(2, 2, zaxis.z());
Excuse the method for putting values into the matrix, I'm working with what my framework gives me.
Vector4 Game::CrossProduct(Vector4 v1, Vector4 v2)
{
Vector4 crossProduct;
crossProduct.set_x((v1.y() * v2.z()) - (v2.y() * v2.z()));
crossProduct.set_y((v1.z() * v2.x()) - (v1.z() * v2.x()));
crossProduct.set_z((v1.x() * v2.y()) - (v1.x() * v2.y()));
return crossProduct;
}
What am I doing wrong here?
Note that I have added the forth line before with the 1 in the corner before just in case, with no change.
you got problem when (0,1,0) is close to parallel to the direction you want to look at. Than the cross product will fail leading in one or two basis vectors to be zero which could lead to 2D appearance. But that would happen only if your objects are offset only in y axis between each other. To avoid that you can test dot product between the up and view direction and if the abs of result is bigger than 0.7 use (1,0,0) instead (as a up or right or whatever...).
Also as We know nothing about your notations we can not confirm your setting the matrix is correct (it might be or may be not, it could be transposed etc.) for more info take a look at:
Understanding 4x4 homogenous transform matrices
But most likely your matrix holds also the perspective transform and by setting its cells you are overriding it completely leading to no perspective hence 2D output. In such case you should multiply original perspective matrix with your new matrix and use the result.

Irrlicht: draw 2D image in 3D space based on four corner coordinates

I would like to create a function to position a free-floating 2D raster image in space with the Irrlicht engine. The inspiration for this is the function rgl::show2d in the R package rgl. An example implementation in R can be found here.
The input data should be limited to the path to the image and a table with the four corner coordinates of the respective plot rectangle.
My first, pretty primitive and finally unsuccessful approach to realize this with irrlicht:
Create a cube:
ISceneNode * picturenode = scenemgr->addCubeSceneNode();
Flatten one side:
picturenode->setScale(vector3df(1, 0.001, 1));
Add image as texture:
picturenode->setMaterialTexture(0, driver->getTexture("path/to/image.png"));
Place flattened cube at the center position of the four corner coordinates. I just calculate the mean coordinates on all three axes with a small function position_calc().
vector3df position = position_calc(rcdf); picturenode->setPosition(position);
Determine the object rotation by calculating the normal of the plane defined by the four corner coordinates, normalizing the result and trying to somehow translate the resulting vector to rotation angles.
vector3df normal = normal_calc(rcdf);
vector3df angles = (normal.normalize()).getSphericalCoordinateAngles();
picturenode->setRotation(angles);
This solution doesn't produce the expected result. The rotation calculation is wrong. With this approach I'm also not able to scale the image correctly to it's corner coordinates.
How can I fix my workflow? Or is there a much better way to achieve this with Irrlicht that I'm not aware of?
Edit: Thanks to #spug I believe I'm almost there. I tried to implement his method 2, because quaternions are already available in Irrlicht. Here's what I came up with to calculate the rotation:
#include <Rcpp.h>
#include <irrlicht.h>
#include <math.h>
using namespace Rcpp;
core::vector3df rotation_calc(DataFrame rcdf) {
NumericVector x = rcdf["x"];
NumericVector y = rcdf["y"];
NumericVector z = rcdf["z"];
// Z-axis
core::vector3df zaxis(0, 0, 1);
// resulting image's normal
core::vector3df normal = normal_calc(rcdf);
// calculate the rotation from the original image's normal (i.e. the Z-axis)
// to the resulting image's normal => quaternion P.
core::quaternion p;
p.rotationFromTo(zaxis, normal);
// take the midpoint of AB from the diagram in method 1, and rotate it with
// the quaternion P => vector U.
core::vector3df MAB(0, 0.5, 0);
core::quaternion m(MAB.X, MAB.Y, MAB.Z, 0);
core::quaternion rot = p * m * p.makeInverse();
core::vector3df u(rot.X, rot.Y, rot.Z);
// calculate the rotation from U to the midpoint of DE => quaternion Q
core::vector3df MDE(
(x(0) + x(1)) / 2,
(y(0) + y(1)) / 2,
(z(0) + z(1)) / 2
);
core::quaternion q;
q.rotationFromTo(u, MDE);
// multiply in the order Q * P, and convert to Euler angles
core::quaternion f = q * p;
core::vector3df euler;
f.toEuler(euler);
// to degrees
core::vector3df degrees(
euler.X * (180.0 / M_PI),
euler.Y * (180.0 / M_PI),
euler.Z * (180.0 / M_PI)
);
Rcout << "degrees: " << degrees.X << ", " << degrees.Y << ", " << degrees.Z << std::endl;
return degrees;
}
The result is almost correct, but the rotation on one axis is wrong. Is there a way to fix this or is my implementation inherently flawed?
That's what the result looks like now. The points mark the expected corner points.
I've thought of two ways to do this; neither are very graceful - not helped by Irrlicht restricting us to spherical polars.
NB. the below assumes rcdf is centered at the origin; this is to make the rotation calculation a bit more straightforward. Easy to fix though:
Compute the center point (the translational offset) of rcdf
Subtract this from all the points of rcdf
Perform the procedures below
Add the offset back to the result points.
Pre-requisite: scaling
This is easy; simply calculate the ratios of width and height in your rcdf to your original image, then call setScaling.
Method 1: matrix inversion
For this we need an external library which supports 3x3 matrices, since Irrlicht only has 4x4 (I believe).
We need to solve the matrix equation which rotates the image from X-Y to rcdf. For this we need 3 points in each frame of reference. Two of these we can immediately set to adjacent corners of the image; the third must point out of the plane of the image (since we need data in all three dimensions to form a complete basis) - so to calculate it, simply multiply the normal of each image by some offset constant (say 1).
(Note the points on the original image have been scaled)
The equation to solve is therefore:
(Using column notation). The Eigen library offers an implementation for 3x3 matrices and inverse.
Then convert this matrix to spherical polar angles: https://www.learnopencv.com/rotation-matrix-to-euler-angles/
Method 2:
To calculate the quaternion to rotate from direction vector A to B: Finding quaternion representing the rotation from one vector to another
Calculate the rotation from the original image's normal (i.e. the Z-axis) to rcdf's normal => quaternion P.
Take the midpoint of AB from the diagram in method 1, and rotate it with the quaternion P (http://www.geeks3d.com/20141201/how-to-rotate-a-vertex-by-a-quaternion-in-glsl/) => vector U.
Calculate the rotation from U to the midpoint of DE => quaternion Q
Multiply in the order Q * P, and convert to Euler angles: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
(Not sure if Irrlicht has support for quaternions)

Drawing Euler Angles rotational model on a 2d image

I'm currently attempting to draw a 3d representation of euler angles within a 2d image (no opengl or 3d graphic windows). The image output can be similar to as below.
Essentially I am looking for research or an algorithm which can take a Rotation Matrix or a set of Euler angles and then output them onto a 2d image, like above. This will be implemented in a C++ application that uses OpenCV. It will be used to output annotation information on a OpenCV window based on the state of the object.
I think I'm over thinking this because I should be able to decompose the unit vectors from a rotation matrix and then extract their x,y components and draw a line in cartesian space from (0,0). Am i correct in this thinking?
EDIT: I'm looking for an Orthographic Projection. You can assume the image above has the correct camera/viewing angle.
Any help would be appreciated.
Thanks,
EDIT: The example source code can now be found in my repo.
Header: https://bitbucket.org/jluzwick/tennisspindetector/src/6261524425e8d80772a58fdda76921edb53b4d18/include/projection_matrix.h?at=master
Class Definitions: https://bitbucket.org/jluzwick/tennisspindetector/src/6261524425e8d80772a58fdda76921edb53b4d18/src/projection_matrix.cpp?at=master
It's not the best code but it works and shows the steps necessary to get the projection matrix described in the accepted answer.
Also here is a youtube vid of the projection matrix in action (along with scale and translation added): http://www.youtube.com/watch?v=mSgTFBFb_68
Here are my two cents. Hope it helps.
If I understand correctly, you want to rotate 3D system of coordinates and then project it orthogonally onto a given 2D plane (2D plane is defined with respect to the original, unrotated 3D system of coordinates).
"Rotating and projecting 3D system of coordinates" is "rotating three 3D basis vectors and projecting them orthogonally onto a 2D plane so they become 2D vectors with respect to 2D basis of the plane". Let the original 3D vectors be unprimed and the resulting 2D vectors be primed. Let {e1, e2, e3} = {e1..3} be 3D orthonormal basis (which is given), and {e1', e2'} = {e1..2'} be 2D orthonormal basis (which we have to define). Essentially, we need to find such operator PR that PR * v = v'.
While we can talk a lot about linear algebra, operators and matrix representation, it'd be too long of a post. It'll suffice to say that :
For both 3D rotation and 3D->2D projection operators there are real matrix representations (linear transformations; 2D is subspace of 3D).
These are two transformations applied consequently, i.e. PR * v = P * R * v = v', so we need to find rotation matrix R and projection matrix P. Clearly, after we rotated v using R, we can project the result vector vR using P.
You have the rotation matrix R already, so we consider it is a given 3x3 matrix. So for simplicity we will talk about projecting vector vR = R * v.
Projection matrix P is a 2x3 matrix with i-th column being a projection of i-th 3D basis vector ei onto {e1..2'} basis.
Let's find P projection matrix such as a 3D vector vR is linearly transformed into 2D vector v' on a 2D plane with an orthonormal basis {e1..2'}.
A 2D plane can be easily defined by a vector normal to it. For example, from the figures in the OP, it seems that our 2D plane (the plane of the paper) has normal unit vector n = 1/sqrt(3) * ( 1, 1, 1 ). We need to find a 2D basis in the 2D plane defined by this n. Since any two linearly independent vectors lying in our 2D plane would form such basis, here are infinite number of such basis. From the problem's geometry and for the sake of simplicity, let's impose two additional conditions: first, the basis should be orthonormal; second, should be visually appealing (although, this is somewhat a subjective condition). As it can be easily seen, such basis is formed trivially in the primed system by setting e1' = ( 1, 0 )' = x'-axis (horizontal, positive direction from left to right) and e2' = ( 0, 1 )' = y'-axis (vertical, positive direction from bottom to top).
Let's now find this {e1', e2'} 2D basis in {e1..3} 3D basis.
Let's denote e1' and e2' as e1" and e2" in the original basis. Noting that in our case e1" has no e3-component (z-component), and using the fact that n dot e1" = 0, we get that e1' = ( 1, 0 )' -> e1" = ( -1/sqrt(2), 1/sqrt(2), 0 ) in the {e1..3} basis. Here, dot denotes dot-product.
Then e2" = n cross e1" = ( -1/sqrt(6), -1/sqrt(6), 2/sqrt(6) ). Here, cross denotes cross-product.
The 2x3 projection matrix P for the 2D plane defined by n = 1/sqrt(3) * ( 1, 1, 1 ) is then given by:
( -1/sqrt(2) 1/sqrt(2) 0 )
( -1/sqrt(6) -1/sqrt(6) 2/sqrt(6) )
where first, second and third columns are transformed {e1..3} 3D basis onto our 2D basis {e1..2'}, i.e. e1 = ( 1, 0, 0 ) from 3D basis has coordinates ( -1/sqrt(2), -1/sqrt(6) ) in our 2D basis, and so on.
To verify the result we can check few obvious cases:
n is orthogonal to our 2D plane, so there should be no projection. Indeed, P * n = P * ( 1, 1, 1 ) = 0.
e1, e2 and e3 should be transformed into their representation in {e1..2'}, namely corresponding column in P matrix. Indeed, P * e1 = P * ( 1, 0 ,0 ) = ( -1/sqrt(2), -1/sqrt(6) ) and so on.
To finalize the problem. We now constructed a projection matrix P from 3D into 2D for an arbitrarily chosen 2D plane. We now can project any vector, previously rotated by rotation matrix R, onto this plane. For example, rotated original basis {R * e1, R * e2, R * e3}. Moreover, we can multiply given P and R to get a rotation-projection transformation matrix PR = P * R.
P.S. C++ implementation is left as a homework exercise ;).
The rotation matrix will be easy to display,
A Rotation matrix can be constructed by using a normal, binormal and tangent.
You should be able to get them back out as follows:-
Bi-Normal (y') : matrix[0][0], matrix[0][1], matrix[0][2]
Normal (z') : matrix[1][0], matrix[1][1], matrix[1][2]
Tangent (x') : matrix[2][0], matrix[2][1], matrix[2][2]
Using a perspective transform you can the add perspective (x,y) = (x/z, y/z)
To acheive an orthographic project similar to that shown you will need to multiply by another fixed rotation matrix to move to the "camera" view (45° right and then up)
You can then multiply your end points x(1,0,0),y(0,1,0),z(0,0,1) and center(0,0,0) by the final matrix, use only the x,y coordinates.
center should always transform to 0,0,0
You can then scale these values to draw to you 2D canvas.

Which euler angle order does this code result in?

Using the mathematics library GLM, I use this code to combine the euler angle rotations to a rotation matrix.
#include <GLM/gtc/matrix_transform.hpp>
using namespace glm;
mat4 matrix = rotate(mat4(1), X, vec3(1, 0, 0))
* rotate(mat4(1), Y, vec3(0, 1, 0))
* rotate(mat4(1), Z, vec3(0, 0, 1));
Does this result in an euler angle sequenze of XYZ or ZYX? I am not sure since matrix multiplication behave not the same as scalar multiplications.
Remember that matrix calculation, in openGL, use a notation knows as vector column (http://en.wikipedia.org/wiki/Column_vector). So, any point transformation will be expressed by a system of linear equation, expressed in vector column notation like this:
[P'] = M.[P], where M = M1.M2.M3
This means that the first transformation that is applied to the points, expressed by vector [P] is M3, after that by M2 and at last by M1.
Answering your question, the resulting Euler angle will be ZXY, once Z rotation transformation is the last matrix that you write to form a matrix multiplication.

Get 3D coordinates from 2D image pixel if extrinsic and intrinsic parameters are known

I am doing camera calibration from tsai algo. I got intrensic and extrinsic matrix, but how can I reconstruct the 3D coordinates from that inormation?
1) I can use Gaussian Elimination for find X,Y,Z,W and then points will be X/W , Y/W , Z/W as homogeneous system.
2) I can use the
OpenCV documentation approach:
as I know u, v, R , t , I can compute X,Y,Z.
However both methods end up in different results that are not correct.
What am I'm doing wrong?
If you got extrinsic parameters then you got everything. That means that you can have Homography from the extrinsics (also called CameraPose). Pose is a 3x4 matrix, homography is a 3x3 matrix, H defined as
H = K*[r1, r2, t], //eqn 8.1, Hartley and Zisserman
with K being the camera intrinsic matrix, r1 and r2 being the first two columns of the rotation matrix, R; t is the translation vector.
Then normalize dividing everything by t3.
What happens to column r3, don't we use it? No, because it is redundant as it is the cross-product of the 2 first columns of pose.
Now that you have homography, project the points. Your 2d points are x,y. Add them a z=1, so they are now 3d. Project them as follows:
p = [x y 1];
projection = H * p; //project
projnorm = projection / p(z); //normalize
Hope this helps.
As nicely stated in the comments above, projecting 2D image coordinates into 3D "camera space" inherently requires making up the z coordinates, as this information is totally lost in the image. One solution is to assign a dummy value (z = 1) to each of the 2D image space points before projection as answered by Jav_Rock.
p = [x y 1];
projection = H * p; //project
projnorm = projection / p(z); //normalize
One interesting alternative to this dummy solution is to train a model to predict the depth of each point prior to reprojection into 3D camera-space. I tried this method and had a high degree of success using a Pytorch CNN trained on 3D bounding boxes from the KITTI dataset. Would be happy to provide code but it'd be a bit lengthy for posting here.