旋转std :: vector <eigen :: vector3d>作为刚性变换？

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

在使用它之前。此外，如果要旋转多个向量，则将四元数转换为旋转矩阵会更有效：

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

此外，您可以将.inverse()用于单位四元数，并将.conjugate()或.adjoint()用于正交矩阵，而不是.transpose()。