Pose_trans and pose_inv functions in URCap

URCap Development URCaps SDK
2

For future readers, I managed to re-implement pose_trans and pose_inv in java.

You first have to convert the pose to an affine transformation. To do this, you need to calculate the rotation vector as a 3x3 rotation matrix (see Maths - AxisAngle to Matrix - Martin Baker).

Then you extend that 3x3 rotation matrix to the 4x4 affine transformation by adding a 4th column vector that corresponds to <x,y,z,1>, and a 4th row vector <0,0,0,1> so it looks like

|   R   | T | 
|-------+---| 
| 0 0 0 | 1 |

where R is your rotation vector and T is the translation (XYZ) vector.

pose_inv() is as simple as taking the inverse of that matrix, whose formula is like so:

|   R   | T |    |  R^-1 | -(R^-1)*T |
|-------+---| -> |-------+-----------|
| 0 0 0 | 1 |    | 0 0 0 |     1     |

And pose_trans(p1, p2) is as simple as multiplying the two affine matrices, A and B, corresponding to poses p1 and p2 respectively:

|   R1  | T1 |    |   R2  | T2 |
|-------+----| X  |-------+----|
| 0 0 0 |  1 |    | 0 0 0 |  1 |

For my implementation, I had a class to handle matrix manipulations, a class to handle converting to/from rotation vector and transformation matrix, and a class to represent the pose and associated functions.

Hope this helps.

4 Likes