интерпретация матрицы преобразования icp

Question

Я использую PCL для получения матрицы преобразования из ICP (getTransformationMatrix()). Полученный результат для Exemple для поступательного движения без вращения

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 

, как я могу найти trasformation из матрицы? Идея состоит в том, чтобы увидеть ошибку, сделанную между стимуляцией и реальным движением

Answer 1

Я не использовал библиотеку, на которую вы ссылаетесь здесь, но мне совершенно ясно, что результат, который вы предоставляете, является однородным преобразованием, т. Е. Верхним оставил матрица 3х3 (R) является матрицей вращения и правой 3x1 (T) является перевод:

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

приведена в разделе «матричное представление» здесь: http://en.wikipedia.org/wiki/Kinematics

Эта запись используется для того, чтобы вы можете получить конечную точку после последовательного трансфо rms путем умножения матриц преобразования.

Если у вас есть точка р0 преобразуется в п раз вы получите точку 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);