icp变换矩阵解释

Question

我使用PCL从ICP (getTransformationMatrix())获得转换矩阵。对于没有旋转平移运动的例子，所获得的结果是

0.999998         0.000361048   0.00223594     -0.00763852
-0.000360518     1             -0.000299474   -0.000319525
-0.00223602      0.000298626   0.999998       -0.00305045
       0         0             0              1

如何从矩阵中找到变换？这个想法是为了观察静止和真实运动之间的误差。

Answer 1

我没有使用您在这里引用的库，但我很清楚您提供的结果是一个同质变换，即左上角的3x3矩阵(R)是旋转矩阵，右侧的3x1 (T)是平移：

M1 = [  **[** [R], [T] **], [** 0 0 0 1 **]**  ] 

请参阅此处的“矩阵表示法”部分：http://en.wikipedia.org/wiki/Kinematics

使用此表示法是为了使您可以通过将转换矩阵相乘来获得连续转换后的最终点。

如果你有一个点p0变换n次，你得到的点p1为：

P0 = [[p0_x], [p0_y], [p0_z], [1]]
P1 = [[p1_x], [p1_y], [p1_z], [1]]
M = M1*M2*...*Mn
P1 = M*P0

Answer 2

tROTA是具有平移和旋转的矩阵：

auto trafo = icp.getFinalTransformation();

Eigen::Transform<float, 3, Eigen::Affine> tROTA(trafo);

float x, y, z, roll, pitch, yaw;

pcl::getTranslationAndEulerAngles(tROTA, x, y, z, roll, pitch, yaw);