Search code examples
c++eigenpoint-clouds

Rotate a std::vector<Eigen::Vector3d> as a rigid transformation?

I have a few 3d points, stored in a std::vector<Eigen::Vector3d>. I need to rigidly rotate and translate these points, without changing their relationship to one another. As if moving the cloud as a whole.

Based on this question:

https://stackoverflow.com/questions/50507665/eigen-rotate-a-vector3d-with-a-quaternion

I have this code:

std::vector<Eigen::Vector3d> pts, ptsMoved;


Eigen::Quaterniond rotateBy = Eigen::Quaterniond(0.1,0.5,0.08,0.02);
Eigen::Vector3d translateBy(1, 2.5, 1.5);


for (int i = 0; i < pts.size(); i++)
{
    //transform point                   
    Vector3d rot = rotateBy * (pts[i] + translateBy);
    ptsMoved.push_back(rot);

}

When i view the points and compare them to the original points, however, I get this: (White are the original, green are the transformed).

points

What i expect, is the cloud as a whole to look the same, just in a different position and orientation. What i get, is a moved and rotated and scaled cloud, that looks different to the original. What am i doing wrong?

EDIT:

If i apply the inverse transform to the adjusted points, using:

std::vector<Eigen::Vector3d> pntsBack;
for (int i = 0; i < ptsMoved.size(); i++)
{
    //transform point       
    Vector3d rot = rotateBy.inverse() * (ptsMoved[i] - translateBy);
    pntsBack.push_back(rot);
}

It gives me an even worse result. (dark green = original points, white = transformed, light green = transformed inverse)

enter image description here

Solution

  • Your Quaternion is not a unit-Quaternion, therefore you will get unspecified results. If you are not sure your quaternion is normalized, just write 

    rotateBy.normalize();

    before using it. Additionally, if you want to rotate more than one vector it is more efficient to convert the Quaternion to a rotation matrix:

    Eigen::Matrix3d rotMat = rotateBy.toRotationMatrix();
// ...
// inside for loop:
    Vector3d rot = rotMat * (ptsMoved[i] - translateBy);

    Also, instead of .inverse() you can use .conjugate() for unit quaternions and .adjoint() or .transpose() for orthogonal Matrices.