icp transformation matrix interpretation

Question

I'm using PCL to obtain the transformation matrix from ICP (getTransformationMatrix()). The result obtained for exemple for a translation movement without rotation is

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

how can I find the trasformation from the matrix? The idea is to see the error made between the stimation and the real movement

Answer 1

tROTA is the matrix with translation and rotation:

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

Answer 2

I have not used the library you refer to here, but it is pretty clear to me that the result you provide is a homogenous transform i.e. the upper left 3x3 matrix (R) is the rotation matrix and the right 3x1 (T) is the translation:

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

refer to the 'Matrix Representation' section here: http://en.wikipedia.org/wiki/Kinematics

This notation is used so that you can get the final point after successive transforms by multiplying the transform matrices.

If you have a point p0 transformed n times you get the point p1 as:

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