I have a 12-point isometry matrix made of four three-dimensional points. They are some point o and the tips of orthogonal unit vectors from that point, x, y, and z.

Here's a drawing for clarity: isometry matrix visualization

\begin{bmatrix} x_{x} & x_{y} & x_{z} \\ y_{x} & y_{y} & y_{z} \\ z_{x} & z_{y} & z_{z} \\ o_{x} & o_{y} & o_{z} \\ \end{bmatrix}

I need to convert this pose to a translation from the origin and quaternion, and vice versa: \begin{bmatrix} x \\ y \\ z \\ \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ w \\ \end{bmatrix}

I'm dealing with 3D CAD software which stores all of its poses in 12-point isometry matrices, but to manipulate them with more standard software libraries I need the data in standard pose format x, y, z, qx, qy, qz, qw which is simply the translation vector and quaternion.

How can I convert between the two pose representations?


To solve this, first subtract the origin from the three unit vectors. Then, you have a rotation matrix. From there, conversion code is:

w = math.sqrt(max(0,1 + a[0][0] + a[1][1] + a[2][2]))/2
x = math.sqrt(max(0,1 + a[0][0] - a[1][1] - a[2][2]))/2
y = math.sqrt(max(0,1 - a[0][0] + a[1][1] - a[2][2]))/2
z = math.sqrt(max(0,1 - a[0][0] - a[1][1] + a[2][2]))/2

Your Answer

