Find a rotation matrix to minimize the sum of distance to x-y plane of a point cloud

Question

Consider there is a point cloud with $n$ point $\overrightarrow{r_1}, \overrightarrow{r_2}, \ldots, \overrightarrow{r_n}$, where $\overrightarrow{r_i} = (x_i, y_i, z_i)$.

I want to find a rotation matrix $R$ such that $\overrightarrow{r_i'} = \overrightarrow{r_i} \times R$ and $\overrightarrow{r_i'} = (x_i', y_i', z_i')$, and $R$ minimizes the $\sum_{i=1}^n z_i'^2$, with aditional conndition that $y_1' = 0$ and $z_1' > 0$.

My questions are,

1. What's the potential solutions to find the rotation matrix?

2. Does this condition guarantee a unique matrix $R$?

I think one potential solution is to use the fomular of 3d rotation matrix and find the minimal value doc. But I am not sure if it is the best solution to implement with Python.

I am trying to find a method to put all points to a reference plane as close as possible, and then I can visualize different point cloud to see how flat they are.

Answer 1

This is a constrained extremum problem. It is usually solved using Lagrange multipliers. The "surface" in this case is the special orthogonal group $SO(3)$. This group can be thought of as the set of all matrices $A$ which solve:

$$ \begin{align} &\det A = 1\\ &AA^T=I \end{align} $$

I believe this can easily be implemented into software.

Answer 2

Let the $3 \times n$ matrix $A$ be defined as follows

$ A = [ r_1, r_2, r_3, \dots , r_n ] $

where $r_i$ is the $i$-th vector listed in matrix A as the $i$-th column.

And let $R$ be a rotation matrix, then the image of rotation is

$ B = R A = [ r_1', r_2' , r_3', \dots, r_n' ] $

The row vector of the $z$ coordinates of these rotated vectors is given by

$ Z^T = e_3^T B $

where $e_3 = [0, 0, 1]^T $

Hence, the objective function we want to minimize is

$ F = Z^T Z = e_3^T B B^T e_3 = e_3^T R A A^T R^T e_3 $

Let $W = R^T = [ w_1, w_2 , w_3 ] $

Then $ R^T e_3 = w_3 $, and therefore,

$ F = w_3^T A A^T w_3 $

From classical quadratic functions and since $w_3$ is a unit vector, then the minimum of this function is achieved when $w_3$ is the unit eigenvector of $A A^T $ corresponding to the minimum eigenvalue (which is non-negative).

Once we have diagonalized the $ 3 \times 3$ matrix $A A^T $ and found its minimum eigenvalue and the corresponding eigenvector then we set $w_3$ to that eigenvector.

We are free to select $w_1$ and $w_2$, and we have $1$ degree of freedom in doing so, because given $w_3$ we can find two mutually orthogonal unit vectors $u_1 $ and $u_2 $ that are orthogonal to $w_3$, and then we can assign

$ w_1 = \cos \theta u_1 + \sin \theta u_2$

$ w_2 = - \sin \theta u_1 + \cos \theta u_2 $

Now matrix $R = W^T = [ w_1, w_2, w_3 ]^T$

This answers part (a) and part (b) of the question.

In conclusion, matrix $R$ is not unique but its third row ( $w_3^T$) is (up to a sign).