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 

我怎么能找到从矩阵的改造中？ 这个想法是看到stimation与真实运动之间产生的错误

Answer 1

我还没有使用你在这里提到的库，但我很清楚你提供的结果是一个同质变换，即上左3x3矩阵（R）是旋转矩阵和右3×1（T）被翻译：

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

参阅“矩阵表示”部分的位置： http://en.wikipedia.org/wiki/Kinematics

这种表示法使用，以便你可以在连续转换后获得最后一个点rms乘以变换矩阵。

如果你有一个点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);