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
}
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;