Eigen，如何使用四元数旋转3D向量？

Question

我有一个由3D点组成的数组，是一个std::vector<Eigen::Vector3d>。

我需要使用位置和四元数来转换这些点。

我的问题是：

如何使用四元数旋转这些点？是否存在比以下方法更快的方法：

    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



        }

- anti

Answer 1

运算符*可以完成这项工作，当然你也可以简化代码：

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

甚至更多的可能性是：

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

如果您将您的点存储在Matrix3Xd中：

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

- ggael

Answer 2

@ggael的回答是完全正确的，我只想提供一些背景。

在这篇维基百科文章中，他们解释了四元数-向量乘法 v’ = qvq^-1。我们使用的具有operator*简写的Eigen库显然也在Unity库中。

在当前版本的Eigen中，您将选择operator*的此重载，它调用_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);
  }
};

请参考此处有关_transformVector的备注:

如果该四元数用于旋转多个点（>1），则将其首先转换为3x3矩阵要更加高效。对于n次变换的操作成本比较:

• Quaternion2: 30n
• 通过Matrix3: 24 + 15n

出于这些效率原因，ggael要求您更改解决问题的方式。

- davidvandebunte