I'm trying to implement a function that takes two geometry vectors in 3D space and returns a rotation matrix that rotates the first vector to the second vector. My function currently uses Rodrigues' rotation formula to create a matrix, but my implementation of this formula gives the wrong answer for some inputs. I checked the math by hand for one test that gave an incorrect result, and my work gave the same result.
Here is the code for my function:
Matrix3d rotation_matrix(Vector3d vector0, Vector3d vector1)
{
vector0.normalize();
vector1.normalize();
// vector orthogonal to both inputs
Vector3d u = vector0.cross(vector1);
if (!u.norm())
{
if (vector0 == vector1)
return Matrix3d::Identity();
// return rotation matrix that represents 180 degree rotation
Matrix3d m1;
m1 << -1, 0, 0,
0,-1, 0,
0, 0, 1;
return m1;
}
/* For the angle between both inputs:
* 1) The sine is the magnitude of their cross product.
* 2) The cosine equals their dot product.
*/
// sine must be calculated using original cross product
double sine = u.norm();
double cosine = vector0.dot(vector1);
u.normalize();
double ux = u[0];
double uy = u[1];
double uz = u[2];
Matrix3d cross_product_matrix;
cross_product_matrix << 0, -uz, uy,
uz, 0,-ux,
-uy, ux, 0;
Matrix3d part1 = Matrix3d::Identity();
Matrix3d part2 = cross_product_matrix * sine;
Matrix3d part3 = cross_product_matrix*cross_product_matrix * (1 - cosine);
return part1 + part2 + part3;
}
I use the Eigen C++ library for linear algebra (available here):
http://eigen.tuxfamily.org/index.php?title=Main_Page
Any help is appreciated. Thanks.
A one liner version consists in using Eigen's Quaternion:
return Matrix3d(Quaterniond::FromTwoVectors(v0,v1));
If you want to rotate from one vector to another just use built in "Eigen::Quaternion::setFromTwoVectors"
http://eigen.tuxfamily.org/dox/classEigen_1_1QuaternionBase.html#ac35460294d855096e9b687cadf821452
It makes exactly what you need and implementation much faster. Then you can call
"Eigen::Quaternion::toRotationMatrix" , to convert to matrix. Both operations are comparably fast and probably faster than direct Rodrigues formula.
Related
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)
I am trying to follow an algebraic equation, and convert it to c++.
I am stuck on:
Calculate the radius as r = ||dp||
where dp is a vector, and:
dp = (dx,dy)
According to my google searching, the vertical bars in r = ||dp|| mean I need to normalize the vector.
I have:
std::vector<double> dpVector;
dpVector.push_back(dx);
dpVector.push_back(dy);
How should I be normalizing this so that it returns a double as 'r'?
||dp|| is the euclidean norm of the vector dp. Take a look at this link for a more complete explanation:
https://en.wikipedia.org/wiki/Euclidean_distance
The euclidean norm is computed as follow: ||dp|| = sqrt(dp.dp), where . represents the dot product.
In C++, this would equate to ||dp|| = std::sqrt(dx*dx + dy*dy). If dp had more dimensions, you would be better off using a linear algebra library for the dot product.
A normalized vector is one that has a length of 1, that is not what you want if you are looking for a length. Calculating the length is the first step for normalizing a vector, but I don't think you need the final step!
To calculate the length you need Pythagoras's Theorem. I'm not going to go into a full description but basically you take the square root of the square of both sides.
In other words multiply dx and dy by themselves, add them together, then square root the result.
r = std::sqrt(dx*dx + dy*dy);
If you really did want to normalize the vector then all you do as the final step is to divide dx and dy both by r. This gives a resulting unit vector of length 1.
You're probably looking for the euclidean norm which is the geometric length of the vector and a scalar value.
double r = std::sqrt(dx*dx + dy*dy);
In contrast to that, normalization of a vector represents the same direction with it length (its euclidean norm ;)) being set to 1. This is again a vector.
Fixed-dimensional vector objects (especially with low dimensionality) lend themselves to be represented as a class type.
A simple example:
namespace wse
{
struct v2d { double x, y; };
inline double dot(v2d const &a, v2d const &b)
{
return a.x*b.x + a.y*b.y;
}
inline double len(v2d const &v) { return std::sqrt(dot(v,v)); }
}
// ...
wse::v2d dp{2.4, 3.4};
// ... Euclidean norm:
auto r = len(dp);
// Normalized vector
wse::v2d normalized_dp{dp.x/r, dp.y/r};
I want to fit a plane to a 3D point cloud. I use a RANSAC approach, where I sample several points from the point cloud, calculate the plane, and store the plane with the smallest error. The error is the distance between the points and the plane. I want to do this in C++, using Eigen.
So far, I sample points from the point cloud and center the data. Now, I need to fit the plane to the samples points. I know I need to solve Mx = 0, but how do I do this? So far I have M (my samples), I want to know x (the plane) and this fit needs to be as close to 0 as possible.
I have no idea where to continue from here. All I have are my sampled points and I need more data.
From you question I assume that you are familiar with the Ransac algorithm, so I will spare you of lengthy talks.
In a first step, you sample three random points. You can use the Random class for that but picking them not truly random usually gives better results. To those points, you can simply fit a plane using Hyperplane::Through.
In the second step, you repetitively cross out some points with large Hyperplane::absDistance and perform a least-squares fit on the remaining ones. It may look like this:
Vector3f mu = mean(points);
Matrix3f covar = covariance(points, mu);
Vector3 normal = smallest_eigenvector(covar);
JacobiSVD<Matrix3f> svd(covariance, ComputeFullU);
Vector3f normal = svd.matrixU().col(2);
Hyperplane<float, 3> result(normal, mu);
Unfortunately, the functions mean and covariance are not built-in, but they are rather straightforward to code.
Recall that the equation for a plane passing through origin is Ax + By + Cz = 0, where (x, y, z) can be any point on the plane and (A, B, C) is the normal vector perpendicular to this plane.
The equation for a general plane (that may or may not pass through origin) is Ax + By + Cz + D = 0, where the additional coefficient D represents how far the plane is away from the origin, along the direction of the normal vector of the plane. [Note that in this equation (A, B, C) forms a unit normal vector.]
Now, we can apply a trick here and fit the plane using only provided point coordinates. Divide both sides by D and rearrange this term to the right-hand side. This leads to A/D x + B/D y + C/D z = -1. [Note that in this equation (A/D, B/D, C/D) forms a normal vector with length 1/D.]
We can set up a system of linear equations accordingly, and then solve it by an Eigen solver as follows.
// Example for 5 points
Eigen::Matrix<double, 5, 3> matA; // row: 5 points; column: xyz coordinates
Eigen::Matrix<double, 5, 1> matB = -1 * Eigen::Matrix<double, 5, 1>::Ones();
// Find the plane normal
Eigen::Vector3d normal = matA.colPivHouseholderQr().solve(matB);
// Check if the fitting is healthy
double D = 1 / normal.norm();
normal.normalize(); // normal is a unit vector from now on
bool planeValid = true;
for (int i = 0; i < 5; ++i) { // compare Ax + By + Cz + D with 0.2 (ideally Ax + By + Cz + D = 0)
if ( fabs( normal(0)*matA(i, 0) + normal(1)*matA(i, 1) + normal(2)*matA(i, 2) + D) > 0.2) {
planeValid = false; // 0.2 is an experimental threshold; can be tuned
break;
}
}
This method is equivalent to the typical SVD-based method, but much faster. It is suitable for use when points are known to be roughly in a plane shape. However, the SVD-based method is more numerically stable (when the plane is far far away from origin) and robust to outliers.
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.
So I am using a quaternion to create a segment of two points in 3D space, and trying to recompute a similar quaternion later on (one representing the same vector through space; I am aware that the segment's rotation around itself is undefined). I am creating the segment as such:
sf::Vector3<float> Start(0, 0, 0);
sf::Vector3<float> End = Start;
//Create a vector from the start to the end
sf::Vector3<float> Translation = Orientation.MultVect(sf::Vector3<float>(0, 1, 0));
//Add that vector onto the start position
End.x += Translation.x * Length;
End.y += Translation.y * Length;
End.z += Translation.z * Length;
Where Orientation::MultVect() looks like this:
sf::Vector3<float> Quaternion::MultVect(sf::Vector3<float> Vector)
{
//From http://www.idevgames.com/articles/quaternions
Quaternion VectorQuat = Quaternion();
VectorQuat.x = Vector.x;
VectorQuat.y = Vector.y;
VectorQuat.z = Vector.z;
VectorQuat.w = 0.0;
Quaternion Inverse = (*this);
Inverse.Invert();
Quaternion Result = Inverse * VectorQuat * (*this);
sf::Vector3<float> ResultVector;
ResultVector.x = Result.x;
ResultVector.y = Result.y;
ResultVector.z = Result.z;
return ResultVector;
}
Now this function seems to work rather well in other contexts, so I don't think the problem is here, but you never know. I should also mention that the point ends up where I expect it to, given the Quaternion I feed if (which I construct from Euler angles, sometimes with multiplication with other quaternions).
The problem appears, to me, to lie in recomputing the quaternion from Start and End. To do so, I use this function, which works well when orienting objects in the scene towards other objects (unless the objects in question are along the exact same Y axis, in which case I get quaternions with NaN values). Here is how I do that:
Quaternion Quaternion::FromLookVector(sf::Vector3<float> FromPoint, sf::Vector3<float> ToPoint)
{
///Based on this post:
///http://stackoverflow.com/questions/13014973/quaternion-rotate-to
//Get the normalized vector from origin position to ToPoint
sf::Vector3<double> VectorTo(ToPoint.x - FromPoint.x,
ToPoint.y - FromPoint.y,
ToPoint.z - FromPoint.z);
//Get the length of VectorTo
double VectorLength = sqrt(VectorTo.x*VectorTo.x +
VectorTo.y*VectorTo.y +
VectorTo.z*VectorTo.z);
//Normalize VectorTo
VectorTo.x /= -VectorLength;
VectorTo.y /= -VectorLength;
VectorTo.z /= -VectorLength;
//Define a unit up vector
sf::Vector3<double> VectorUp(0, -1, 0);
//The X axis is the cross product of both
//Get the cross product as the axis of rotation
sf::Vector3<double> AxisX(VectorTo.y*VectorUp.z - VectorTo.z*VectorUp.y,
VectorTo.z*VectorUp.x - VectorTo.x*VectorUp.z,
VectorTo.x*VectorUp.y - VectorTo.y*VectorUp.x);
//Normalize the axis
//Get the length of VectorTo
double AxisXLength = sqrt(AxisX.x*AxisX.x +
AxisX.y*AxisX.y +
AxisX.z*AxisX.z);
//Normalize VectorTo
AxisX.x /= AxisXLength;
AxisX.y /= AxisXLength;
AxisX.z /= AxisXLength;
//Get the adjusted Y vector
//Get the cross product of the other two axes
sf::Vector3<double> AxisY(VectorTo.y*AxisX.z - VectorTo.z*AxisX.y,
VectorTo.z*AxisX.x - VectorTo.x*AxisX.z,
VectorTo.x*AxisX.y - VectorTo.y*AxisX.x);
//Normalize the axis
//Get the length of VectorTo
double AxisYLength = sqrt(AxisY.x*AxisY.x +
AxisY.y*AxisY.y +
AxisY.z*AxisY.z);
//Normalize VectorTo
AxisY.x /= AxisYLength;
AxisY.y /= AxisYLength;
AxisY.z /= AxisYLength;
//A matrix representing the Thing's orientation
GLfloat RotationMatrix[16] = {(float)AxisX.x,
(float)AxisX.y,
(float)AxisX.z,
0,
(float)AxisY.x,
(float)AxisY.y,
(float)AxisY.z,
0,
(float)VectorTo.x,
(float)VectorTo.y,
(float)VectorTo.z,
0,
0,
0,
0,
1};
Quaternion LookQuat = Quaternion::FromMatrix(RotationMatrix);
//Reset the quaternion orientation
return LookQuat;
}
So when I compute the segments, I also check what their reconstructed values would be, like this:
sf::Vector3<float> Start(0, 0, 0);
sf::Vector3<float> End = Start;
//Create a vector from the start to the end
sf::Vector3<float> Translation = Orientation.MultVect(sf::Vector3<float>(0, 1, 0));
//Add that vector onto the start position
End.x += Translation.x * Length;
End.y += Translation.y * Length;
End.z += Translation.z * Length;
std::cout << "STATIC END (";
std::cout << End.x << ",";
std::cout << End.y << ",";
std::cout << End.z << ")\n";
///TEST
Quaternion Reconstructed = Quaternion::FromLookVector(Start, End);
Translation = Reconstructed.MultVect(sf::Vector3<float>(0, 1, 0));
sf::Vector3<float> TestEnd = Start;
TestEnd.x += Translation.x * Length;
TestEnd.y += Translation.y * Length;
TestEnd.z += Translation.z * Length;
std::cout << "RECONSTRUCTED END (";
std::cout << TestEnd.x << ",";
std::cout << TestEnd.y << ",";
std::cout << TestEnd.z << ")\n";
And the two don't match up. For example, if the static end point is (0,14.3998,0.0558498), then the recomputed point is (0,8.05585,-6.39976). The two should be identical, though. The undefined part of the rotation shouldn't change the position of the end point, only the roll (or Z-rotation, or whatever you want to call it), which, since this is a segment, doesn't matter.
Note that when I end up using this for things other than simple segments, the roll will matter, which is why I use an up vector to make sure the objects I place along these segments will always have their tops facing upwards as much as possible (objects looking straight up or down can have a special arbitrary roll determined separately, if need be). Another goal is creating multiple segments strung together, each rotating relative to the orientation of the one that came before it rather than rotating relative to global space.
So what am I doing wrong here? Why can't I recompute a second quaternion that performs the same translation as the first one?
I'm not entirely sure how you're calculating the 'rotating' quaternion between the two vectors, but I'm pretty sure it's very cumbersome. At least, if I understand you correctly, you have 'look' vectors which point in some direction, and the object 'looks' along that direction from the origin (0,0,0), correct?.
If the above is the case, it should not be too difficult. One thing I find quite peculiar though is that your quaternion - vector multiplication seems to be in reverse order. I have quaternion * vector defined as:
quat qt = *this * quat(0, vec.x, vec.y, vec.z) * inverse();
return vec3(qt.x, qt.y, qt.z);
In which the quat constructor is defined as quat(w, x, y, z) and the inverse() method returns a copy. Inverse is equal to conjugate, and it is defined as (w, -x, -y, -z). BUT, for this to be true your quaternions have to be normalized, only then will they actually represent an orientation (and only then will the inverse equal the conjugate). Then I have quaternion multiplication defined as follows:
// This describes A * B (not communative!)
w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z;
x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y;
y = A.w * B.y + A.y * B.w + A.z * B.x - A.x * B.z;
z = A.w * B.z + A.z * B.w + A.x * B.y - A.y * B.x;
With that out of the way, you want to be able to construct a quaternion from 'angle-axis'. Meaning it should take an axis of rotation, and an angle to rotate around that axis (in radians). I shall just give you that algorithm, as it doesn't make much sense intuitively:
// if axis is already unit length, remove the division
double halfAngle = angle * 0.5f; // In radians
double scale = sin(halfAngle) / axis.magnitude();
w = cos(halfAngle);
x = axis.x * scale;
y = axis.y * scale;
z = axis.z * scale;
So now we just have to calculate an axis to rotate around, and how much we want to rotate around it, in radians. At first sight this might seem complex, but it is just a case of understanding what is going on. You have two vectors, A and B. You want to calculate a quaternion which describes a rotation FROM A, TO B. To get the axis to rotate around, we just want a perpendicular axis to both, obviously this would be done by taking the cross product. If you're using a right handed coordinate system, it would be:
axis = A × B
If you're using a left handed coordinate system, I think you should just inverse the order, but don't take my word on that. Now to get the angle between the two vectors. This can very simply be done by taking the dot product. The only catch is that you have to normalize both vectors, so they have length 1, and won't alter the outcome of the dot product. This way the dot product will return the cosine of the angle, so to get the actual angle we can do:
angle = acos(normalize(A) * normalize(B))
The multiplication sign stands for the dot product of course. Now we just plug the axis and angle in the algorithm I gave you above, and we have a quaternion describing a 'rotation' from look vector A to look vector B. Now if the vectors point in the exact same direction, it would be unwise to apply the algorithm, as the axis will be (0,0,0). If you look at the algorithm I hope you see that will either try to divide by zero or simply output all zeros. So whenever I apply that algorithm I first check if the axis is not all zeros.
The formula you're currently using seems very strange and inefficient to me. I don't really understand why you're first computing a matrix either, computing a quaternion from a matrix is quite an expensive computation. In fact I believe computing the opposite, a matrix from a quaternion, is even faster.
Anyway, good luck getting it to work!