Rotate set of vectors to a set of planes defined by their normal vectors.

222 Views Asked by At

Given a set of unitary vectors $\{a_1, a_2, \ldots, a_n\}$ and a set of normals $\{n_1, n_2, \ldots, n_n\}$ (all vectors are in $\mathbb{R}^3$), I am trying to find the rotation matrix $R\in SO(3)$ that best aligns each vector $a_i$ with the planes defined by $n_i$. In this case, all planes pass through the origin.

An example of "best" alignment could be the one that minimizes the projections of the vectors in the least squares sense: $$ R = \arg\min_{R} \sum_{i}(n_i^\top R a_i)^2, \qquad\text{s.t. } R\in SO(3)~. $$ However, based on previous questions (such as this one and this one), this expression seems to be difficult to solve in closed form.

I feel that there may be other better optimization criteria that could lead to a closed-form solution. Ideally, the criteria should still minimize the distance of the points $\{a_i\}_i$ to the planes defined by $\{n_i\}_i$, so that the rotation would still be optimal with this criteria.

This is a related visualization where two vectors, $a_1$ and $a_2$, are rotated via $R$ such that they lie on the planes defined by the normal vectors $n_1$ and $n_2$ (the ideal case):

enter image description here


Edit
This is the python code that I have used to test the idea proposed by @MauricioCeleLopezBelon. Unfortunately, as long as the code has no bugs, it does not seem to work.

import numpy as np
from scipy.stats import special_ortho_group as SO

# create unitary 3d points (the set {a_1, a_2 ... a_n}).
npoints = 10
a = np.random.rand(3, npoints)
a /= np.linalg.norm(a, axis=0, keepdims=True)

# Get exemplar normal vectors. For that:
# 1) sample a random rotation matrix R (the one that we want to estimate).
R = SO(3).rvs()
# 2) rotate each a_i with it.
a_rot = R @ a
# 3) create normal vectors of a plane to which each a_rot_i belongs.
n = np.zeros((3, npoints))
n[0] = -a_rot[1]
n[1] = a_rot[0]
n /= np.linalg.norm(n, axis=0, keepdims=True)  # normalization

# orthogonally project each given point to the planes defined by the normals.
a_proj = a - (a * n).sum(axis=0, keepdims=True) * n

# align with SVD (solve Wahba's problem -- https://en.wikipedia.org/wiki/Wahba%27s_problem).
B = np.sum(a_proj.T[..., None] @ a.T[:, None], axis=0)
U, d, VT = np.linalg.svd(B)
M = np.eye(3)
M[2, 2] = np.linalg.det(U) * np.linalg.det(VT)
R_est = U @ M @ VT

print(f"True rotation:\n{R}\n")
print(f"Estimated rotation:\n{R_est}\n")

"""
Exemplar output

True rotation:
[[ 0.74085984  0.51771055  0.42790476]
 [-0.4977192   0.85095016 -0.16780769]
 [-0.45100143 -0.08865444  0.88810928]]

Estimated rotation:
[[ 0.83871159  0.54320098 -0.03867256]
 [-0.54101482  0.82301563 -0.1730556 ]
 [-0.06217585  0.16606616  0.98415253]]

"""
1

There are 1 best solutions below

2
On

Any rotation matrix can be specified in terms of three angles $\theta, \phi, \beta $ as follows

$ R = \begin{bmatrix} c_3 c_2 c_1 - s_3 s_2 && -s_3 c_2 c_1 - c_3 s_2 && s_1 c_2 \\ c_3 s_2 c_1 + s_3 c_2 && - s_3 s_2 c_1 + c_3 c_2 && s_1 s_2 \\ - c_3 s_1 && s_3 s_1 && c_1 \end{bmatrix}$

where $c_1 = \cos(\theta), s_1 = \sin(\theta), c_2 = \cos(\phi), s_2 = \sin(\phi), c_3 = \cos(\beta) , s_3 = \sin(\beta) $

Define the objective function as

$ f(\theta, \phi, \beta) = \displaystyle \sum_{i=1}^n (n_i^T R a_i)^2 $

Find the partial derivatives of $f$ with respect to $\theta, \phi, \beta $

$\dfrac{\partial f }{\partial \theta} = \displaystyle 2 \sum_{i=1}^n (n_i^T R a_i) ( n_i^T \dfrac{ \partial R }{\partial \theta} a_i ) $

$\dfrac{\partial f }{\partial \phi} = \displaystyle 2 \sum_{i=1}^n (n_i^T R a_i) ( n_i^T \dfrac{ \partial R }{\partial \phi} a_i ) $

$\dfrac{\partial f }{\partial \beta} = \displaystyle 2 \sum_{i=1}^n (n_i^T R a_i) ( n_i^T \dfrac{ \partial R }{\partial \beta} a_i ) $

Now you can use the simple steepest descent algorithm to minimize $f$. I did that for $n = 3$ and it resulted in a solution that I verified. The matrix $R$ that I obtained resulted in $f = 0 $. I also found that the solution $R$ is not unique if $n = 3$, but I think it will be unique for $n \gt 3 $.