将 std::vector<Eigen::Vector3d> 旋转为刚性变换？

Question

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

基于这个问题：

我有这个代码：



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

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

将 std::vector<Eigen::Vector3d> 旋转为刚性变换？

Rotate a std::vector<Eigen::Vector3d> as a rigid transformation?

c++

point-clouds

eigen