Find a rotation matrix given some constaints on points transformation

Question

I'm looking for elegant way to find a rotation matrix between RefA and RefB with all the points known in RefA and a 6 constraints (corresponding to a solid transformation with 6doF) in RefB. I set as a prerequisite that the translation was managed before and that therefore the problem has a solution.

1 - Example

Given points in ref A :

Point 1 (+10,+0,+0)
Point 2 (+20,+5,+0)
Point 3 (+20,-5,+0)

What would be the rotation matrix to get them in a ref B where we have 6 constraints (to lock position with 6DoF):

Point 1' (6.7151763    4.7140706     -5.7169875)
Point 2' (14.1649161   12.78572935   PT2'_Z)
Point 3' (12.6957891   PT3'_Y        PT3'_Z)

Here the answer I would like to obtain is the following rotation matrix :

[[ 0.67151763  0.1469127   0.72627869]
 [ 0.47140706  0.67151763 -0.57169875]
 [-0.57169875  0.72627869  0.38168025]]

2 - First attempts

I tried by following the matrix calculation as :

[[ R11 R12 R13 ]   x  [[ +10 +20 +20 ]  = [[ +6.7151763  PT2'_X       PT3'_X      ]
 [ R21 R22 R23 ]       [ +00 +05 -05 ]     [ +4.7140706  12.78572935  PT3'_Y      ]
 [ R31 R32 R33 ]       [ +00 +00 +00 ]]    [ -5.7169875  -7.80258155  -15.06536845]]

We therefore obtain a problem with 9 variables (R11...R33) and 6 equations as follow :

R11*10 + R12*00 + R13*00 = +6.7151763

R21*10 + R22*00 + R23*00 = +4.7140706
R21*20 + R22*+5 + R23*00 = 12.78572935

R31*10 + R32*00 + R33*00 = -5.716987
R31*20 + R32*+5 + R33*00 = -7.80258155
R31*20 + R32*-5 + R33*00 = -15.06536845

Just to try, I tried to solve the last 3 equations (3 variables, 3 equations):

coeffs = np.array(
    [
        [10,0,0],
        [20,+5,0],
        [20,-5,0],
    ])
res = np.array([-5.7169875, -7.80258155, -15.06536845])
x = np.linalg.solve(coeffs, res)
print(x)

Result :

numpy.linalg.LinAlgError: Singular matrix

But I also tried :

coeffs = np.array(
    [
        [10,0,0],
        [20,+5,0],
        [20,-5,0],
    ])
res = np.array([-5.7169875, -7.80258155, -15.06536845])
x = np.linalg.lstsq(coeffs, res)
print(x)

Result :

array[-0.57169875,  0.72627869,  0.        ]
array[30.        ,  7.07106781,  0.        ]

Found 2 values, but not the nice way.

Ideas :

• I might use properties of rotation matrix (orthogonality, determinant..)
• I might decompose rotation to Rx, Ry, Rz and try to find a adequations between variables and equations

Thanks !

Ronan

Answer 1

You have the initial points matrix

$ P = \begin{bmatrix} 10 && 20 && 20 \\ 0 && 5 && -5 \\ 0 && 0 && 0 \end{bmatrix}$

And you partially have the rotated points matrix

$ Q = \begin{bmatrix} 6.7151763 && 14.1649161 && 12.6957891 \\ 4.7140706 && 12.78572935 && Y_3 \\ -5.7169875 && Z_2 && Z_3 \end{bmatrix}$

And we want to find the rotation matrix $R$ such that

$ Q = R P $

Now, the rotation matrix does not change the length of vectors or the length of their differences, therefore,

$ 10^2 + 0^2 + 0^2 = 6.7151763^2 + 4.7140706^2 + (-5.7169875)^2 \tag{1}$

$ 20^2 + 5^2 + 0^2 = 14.1649161^2 + 12.78572935^2 + Z_2^2 \tag{2}$

$ 20^2 + (-5)^2 + 0^2 = 12.6957891^2 + Y_3^2 + Z_3^2 \tag{3}$

And in addition,

$ (10 - 20)^2 + (0 - 5)^2 + (0 - 0)^2 = (6.7151763 - 14.1649161)^2 + (4.7140706 - 12.78572935)^2 + (-5.7169875 - Z_2)^2 \tag{4} $

$ (10 - 20)^2 + (0 - (-5) )^2 +0^2 = (6.7151763 - 12.6957891)^2 + (4.7140706 - Y_3)^2 + (-5.7169875 - Z_3)^2 \tag{5}$

$ (20 - 20)^2 + (5 - (-5) )^2 + (0 - 0)^2 = (14.1649161 - 12.6957891)^2 + (12.78572935 - Y_3)^2 + (Z_2 - Z_3)^2 \tag{6}$

From equation $(2)$, we have

$ Z_2^2 = 60.8802768685 $

So that $ Z_2 = \pm \sqrt{ 60.8802768685} = \pm 7.80258142338 $

To choose the correct value, we use equation $(4)$ which says

$ | Z_2 + 5.7169875 | = 2.08554939 $

Therefore, $Z_2$ must be $ - 7.80258142338 $

Now, equations $(3)$ and $(5)$ are quadratic in $Y_3$ and $Z_3$, and can be solved without much difficulty. The result is

$ Y_3 = 6.070552911 $

$ Z_3 = -15.06536844 $

Now the rotation matrix $R$ can be obtained because

$Q = R P$

which when written in column format gives

$[Q_1,Q_2, Q_3] = R [P_1, P_2, P_3] $

Matrices $P$ and $Q$ are singular, so $R$ cannot be obtained by matrix inversion. However, a rotation matrix $R$ has the property that

$ (R v) \times (R w) = R (v \times w) $

Therefore, we'll compute the vector

$ P_4 = P_1 \times P_2 $ and $Q_4 = Q_1 \times Q_2 $

then the matrix equation becomes

$ [Q_1, Q_2, Q_4] = R [P_1, P_2, P_4] $

The matrices now are invertible, therefore,

$ R = [Q_1 , Q_2, Q_4] [P_1, P_2,P_4]^{-1} $

I've written a small code to implement this, and got

$R = \begin{bmatrix} 0.67151763 && 0.1469127 && 0.726278704 \\ 0.47140706 && 0.67151763 && -0.571698769 \\ -0.57169875 && 0.726278716 && 0.381680243 \end{bmatrix} $

Note that in this particular set up, we did not need $Y_3$ or $Z_3$, and all we needed was $Z_2$.