Eigen, rotate a vector3d with a quaternion?

Question

I have an array of 3D points, as a std::vector<Eigen::Vector3d>.

I need to transform these points with a position and quaternion.

My questions are:

How can I rotate these points with a quaternion? And is there a faster way than:

Eigen::Vector3d Trans;     // position to move by
Eigen::Quaterniond quats;  // quat to rotate by

for (int p = 0; p < objectPoints.size(); p++) {
    Eigen::Vector3d pnt;
    // add pose
    pnt.x = objectPointsTri[p].x + -Trans.x();
    pnt.y = objectPointsTri[p].y + -Trans.y();
    pnt.z = objectPointsTri[p].z + -Trans.z();

    Eigen::Vector3d pntRot =  // rotate pnt by the quaternion
}

Pforzheim

Answer 1

Operator * will do the job, and you can of course simplify your code:

pnt = objectPointsTri[p] - Trans;
pntRot = quat * pnt;

or even:

pnt = quat * (objectPointsTri[p] - Trans);

or, if you store your points in a Matrix3Xd:

Matrix3Xd in_pts;
Matrix3Xd out_pts;
Affine3d T = quats * Translation3d(-Trans);
out_pts = T * in_pts;

Guyon

Answer 2

The answer from @ggael is perfectly correct, I'd just like to provide some background.

In this Wikipedia article they explain quaternion-vector multiplication v’ = qvq^-1. The Eigen shorthand with operator* we're using is also apparently in Unity libraries.

In the current version of Eigen, you'd be selecting this overload of operator*, which calls _transformVector

template<typename RotationDerived,typename OtherVectorType>
struct rotation_base_generic_product_selector<RotationDerived,OtherVectorType,true>
{
  ...
  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE ReturnType run(const RotationDerived& r, const OtherVectorType& v)
  {
    return r._transformVector(v);
  }
};

See the Remarks on _transformVector here:

If the quaternion is used to rotate several points (>1) then it is much more efficient to first convert it to a 3x3 Matrix. Comparison of the operation cost for n transformations:

• Quaternion2: 30n
• Via a Matrix3: 24 + 15n

ggael asked you to change how you solve the problem for these efficiency reasons.

Adjudge