c++ - 将 std::vector 旋转为刚性变换？

Question

我有几个 3d 点，存储在 std::vector<Eigen::Vector3d> 中.我需要严格地旋转和平移这些点，而不改变它们之间的关系。仿佛整个云都在移动。

基于这个问题:

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

我有这个代码:

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

}

当我查看这些点并将它们与原始点进行比较时，我得到了这个:(白色是原始点，绿色是转换后的点)。

我期望的是，云作为一个整体看起来是一样的，只是位置和方向不同。我得到的是移动、旋转和缩放的云，看起来与原始云不同。我究竟做错了什么？

编辑:

如果我对调整后的点应用逆变换，使用:

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

它给了我一个更糟糕的结果。 (深绿色=原始点，白色=转换后，浅绿色=转换后的逆)

Answer 1

您的四元数不是单位四元数，因此您将得到未指定的结果。如果你不确定你的四元数是否被归一化，就写

rotateBy.normalize();

使用前。此外，如果您想旋转多个 vector ，将四元数转换为旋转矩阵会更有效:

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

此外，您可以使用 .conjugate() 代替 .inverse() 单位四元数和 .adjoint() 或 .transpose() 用于正交矩阵。