Determine rotary axis from imprecise observations

Question

Given a set of 3d points such that each point $p_i^j$ is the point $p_i^k$ rotated about some unknown axis by a known angle $\alpha_j^k$. The goal is to find the axis of rotation.

For three points $p_i^j$ with $j=1,2,3$ we could compute a circle passing through the points (without using the angles) to solve the problem. However, the points are errornous and I'm interested in a solution that minimizes the transformation error of the obtained axis.

What methods are appropriate to compute the axis with or without using the rotation angles. Since the angles are known very precisely, using them should improve the result.

---

Beeing a rather general problem, below I outline some specifics about my instance of the problem:

The points are identified by circular markers on a flat board which is placed on a high accuracy rotary table with a typical absolute accuracy of 0.006 degree. The points are at maximum distance of 200 mm from the rotation axis. Given the accuracy of the rotary this yields an uncertainty of 20µm measured along the circle of rotation. This is roughly a factor 10 better than the measuring precision of the marker points (which may not be independent from the angle of rotation, since it changes the angle under which the circular marks are observed by the measurement device). Currently I take three measurements by driving the rotary to some angular position $\theta_j$ in its own coordinate system. Hence, we have $a_j^k=\theta_k-\theta_j$ for $j,k \in \{1,2,3\}$. Typically, I see around $400$ corresponding points in each pair of rotated frames. Usually, we use $\theta_{j+1} = \theta_j + \Delta $ and $\Delta = 15°$.

---

Thanks to the inpiring post of "Nominal Animal" I come up myself with the following two-stage solution:

1. Compute rotary's coordinate system using SVD by using the property that all vectors $p_i^k - p_i^j$ are perpendicular to the axis of rotation. Thus, create a matrix $A$ with a row $p_i^k - p_i^j$ for each corresponding pair of points. The largest eigenvector of $A$ is the axis of rotation. Using SVD $A=USV^T$ of $A$, the matrix $V$ is an orthonormal base of the rotary.

2. Compute a point $c$ in space that supports the line of the rotation axis. Therefore, let the rotary rotate about its $z$-axis and let $R_j^k$ the rotation matrix in the rotary's coordinate system for $\alpha_j^k$. Given the matrix $V$ from the first step we have for each pair of corresponding points $$R_j^k V' (p_i^j - c) = V' (p_i^k - c)$$ Let $M_j^k = VR_j^k V'$ we can rewrite the above identity to a system of linear equations, which can be resolved for $c$ $$c(I_3 - M_j^k) = p_i^k-M_j^kp_i^j$$

This works well, since both steps yield an optimal solution in the least squares sense using SVD. We only use systems of linear equations and it may be possible to combine the two in one step. However, I have not figured it out yet.

Answer 1

This is not an answer, but a few notes on the properties of the problem at hand.

I've worked with a rather similar problem, except with $\theta$ being unknown too: estimating the rotation of atom clusters in molecular dynamics simulations (for the purpose of rotation cancellation). With certain temperature control schemes, this would be extremely useful. While there is no observational error, there is considerable noise in the positions due to a finite temperature, and often with atomic interactions at the outer layers of the cluster. It is often very hard to determine whether an atom belongs to the cluster or not, unless you have several observations. Finally, memory restrictions are sometimes such that one cannot store the coordinates for several observations; otherwise, the overall "costs" are too high, and simply using a different thermal control scheme, one where cluster rotation is not induced, is a better solution.

---

Let the unit vector describing the axis of rotation be $$\hat{u} = \left ( u_x, u_y, u_z \right )$$ and the angle of rotation $\theta$. For simplicity, let's also use $$\begin{align}c &= \cos\theta \\ s &= \sin\theta \\ n &= 1 - \cos\theta \end{align}$$ The matrix describing the rotation is then $$\mathbf{R} = \begin{bmatrix} u_x u_x n + c & u_x u_y n - u_z s & u_x u_z n + u_y s \\ u_y u_x n + u_z s & u_y u_y n + c & u_y u_z n - u_x s \\ u_z u_x n - u_y s & u_z u_y n + u_x s & u_z u_z n + c \end{bmatrix}$$ and the overall problem is to fit $\hat{u}$ into $$\vec{q}_i = \mathbf{R} \vec{p}_i + \vec{T} + \vec{\epsilon}_i$$ where $\vec{T}$ is the translation keeping some point $\vec{o}$ stationary, $\vec{T} = \vec{o} - \mathbf{R}\vec{o}$, i.e. $$\vec{q}_i = \mathbf{R} \left ( \vec{p}_i - \vec{o} \right ) + \vec{o} + \vec{\epsilon}_i$$ and $\vec{\epsilon}_i$ are small unknown errors in the observations.

If we assume all errors $\vec{\epsilon}_i$ have the same distribution, say uniform within some range, or perhaps a Gaussian, then the errors in observations for points $\vec{p}_i$ closest to the axis affect the result the most. Because of that, you'll want to add weights $w_i$ to each point for fitting, using some monotonic function that is zero at the axis, and increases as the distance from point $\vec{p}_i$ to the axis increases.

If we assume point $\vec{o}$ is on the axis, then the distance from $\vec{p}_i$ to the axis is $$d_i = \left\lvert \hat{u} \times \left ( \vec{o} - \vec{p}_i \right ) \right\rvert$$ and I personally would start with $w_i = d_i^2$.

---

Why not use $$\Delta_i = \left\lvert \vec{q}_i - \vec{p}_i \right\rvert$$and find the axis (where $\Delta = 0$)?

That should actually work if $\theta$ is small, so that the straight distance is a good approximation for the length of the circular arc for $i$. The larger $\theta$ is (in magnitude, up to $180°$, of course), the larger the difference between the arc length and the endpoints' distance. For a sensible fitting model, the two should be approximately equal for the fit to correctly locate the axis (at $\Delta = 0$).

For small $\theta$, one could actually simply assume $$\Delta_i = \left\lvert \vec{q}_i - \vec{p}_i \right\rvert + \epsilon_i$$where $\epsilon_i$ is some signed random number, with the same distribution as the radial component in the observational error for point $i$.

Again, this means that larger $\Delta_i$ should be weighted more than smaller $\Delta_i$ when fitting (to find the axis; zero $\Delta$).

An iterative scheme, where the known angle, and the axis from the previous step is used to calculate the expected displacement for point $i$, would allow one to estimate the amount of error in the observations for each point -- actually, the error vectors $\vec{\epsilon}_i$. If we know or assume some distribution for them, we could use the error in the distribution to adjust the axis location. As long as the adjustment results in a better error distribution, this should optimize the solution.

Answer 2

I wrote the following answer on sci.math a few years ago. It is a least squares method, but it will work given $3$ or more source and destination points. I have just converted it from ASCII-art to $\LaTeX$; please let me know if there are any transcription errors.

---

Least Squares Conformal Multilinear Regression

Given $\left\{P_j:1\le j\le m\right\}$ and $\left\{Q_{\,j}:1\le j\le m\right\}$, two sets of points, we want to find a conformal map, defined by a linear map, $M$, and a vector, $R$, which maps one set of points to the other via $$ Q=PM+R\tag{1} $$ where we require $MM^T=r^2I$ and that the square residue $$ \sum_{j=1}^m\left|P_jM+R-Q_{\,j}\right|^{\,2}\tag{2} $$ is minimal. Minimality implies that for any variations $\delta M$ and $\delta R$, $$ 0=\sum_{j=1}^m\left\langle P_jM+R-Q_{\,j},P_j\delta M+\delta R\right\rangle\tag{3} $$ Varying $\boldsymbol{R}$

Since $(3)$ holds for any variations, we can set $\delta M=0$ to get $$ 0=\sum_{j=1}^m\left\langle P_jM+R-Q_{\,j},\delta R\right\rangle\tag{4} $$ Since $(4)$ holds for any $\delta R$, the following must hold for each vector component, and therefore, for the whole vector: $$ 0=\sum_{j=1}^m\left(P_jM+R-Q_{\,j}\right)\tag{5} $$ which says that $$ \overline{P}M+R=\overline{Q}\tag{6} $$ where $$ \overline{P}=\frac1m\sum_{j=1}^mP_j\tag{7} $$ and $$ \overline{Q}=\frac1m\sum_{j=1}^mQ_{\,j}\tag{8} $$ Thus, the conformal map we seek sends $\overline{P}$ to $\overline{Q}$.

Varying $\boldsymbol{M}$

We could also set $\delta R=0$ in $(3)$ to get $$ 0=\sum_{j=1}^m\left\langle P_jM+R-Q_{\,j},P_j\delta M\right\rangle\tag{9} $$ The conformality restriction, $MM^T=r^2I$, restricts the variations allowed. Taking the difference of this restriction gives $$ \begin{align} 2r\,\delta r\,I &=\delta\left(MM^T\right)\\ &=M\delta M^T+\delta MM^T\\ &=\left(\delta MM^T\right)^T+\delta MM^T\tag{10} \end{align} $$ Equation $(10)$ implies that, for some skew-symmetric $\delta A$, we have $$ \delta M=(\delta A+r\,\delta r\,I)M/r^2\tag{11} $$ Since $(9)$ holds for any $\delta M$ of the form $(11)$, if we let $\delta R = 0$, we get $$ \begin{align} 0 &=\sum_{j=1}^m\left\langle P_jM+R-Q_{\,j},P_j(\delta A+r\,\delta r\,I)M\right\rangle\\ &=r\,\delta r\sum_{j=1}^m\left\langle P_jM+R-Q_{\,j},P_jM\right\rangle\\ &+\sum_{j=1}^m\left\langle R-Q_{\,j},P_j\,\delta A\,M\right\rangle\tag{12} \end{align} $$ We didn't forget a $P_jM$ in the last sum; since $\delta A$ is skew-symmetric, we have $\left\langle P_jM,P_j\,\delta A\,M\right\rangle=0$.

Varying $\boldsymbol{r}$

Letting $\delta A=0$, we get the condition $$ 0=\sum_{j=1}^m\left\langle P_jM+R-Q_{\,j},P_jM\right\rangle\tag{13} $$ We can eliminate $R$ using $(6)$: $$ 0=\sum_{j=1}^m\left\langle\left(P_j-\overline{P}\right)M-\left(Q_{\,j}-\overline{Q}\right),\left(P_j-\overline{P}\right)M\right\rangle\tag{14} $$ Varying $\boldsymbol{M/r}$

Letting $\delta r=0$, we are left with $$ 0=\sum_{j=1}^m\left\langle R-Q_{\,j},P_j\,\delta A\,M\right\rangle\tag{15} $$ for any skew-symmetric $\delta A$. By letting $\delta A$ run over the basic skew-symmetric matrices, equation $(15)$ implies $$ 0=\sum_{j=1}^m\left(P_j^T\left(R-Q_{\,j}\right)M^T-M\left(R-Q_{\,j}\right)^T P_j\right)\tag{16} $$ which, through simple manipulation, remembering $MM^T=r^2I$ and using $(6)$, yields $$ \begin{align} 0 &=\sum_{j=1}^m\left(\left(P_j-\overline{P}\right)M\right)^T\left(Q_{\,j}-\overline{Q}\right)\\ &-\sum_{j=1}^m\left(Q_{\,j}-\overline{Q}\right)^T\left(P_j-\overline{P}\right)M\tag{17} \end{align} $$ which simply says that $SM$ is symmetric, where $$ S=\sum_{j=1}^m\left(Q_{\,j}-\overline{Q}\right)^T\left(P_j-\overline{P}\right)\tag{18} $$ Symmetry of $\boldsymbol{SM}$

Let $S = U D V^T$ be the Singular Value Decomposition of $S$ ($U$ and $V$ are unitary and $D$ is diagonal with non-negative real entries). Note that $D$ is unique up to permutation of the diagonal elements. All symmetric matrices formed via right multiplication of $S$ by an unitary matrix are of the form $$ S\underbrace{\,\,VEU^T\,\,}_{\text{unitary}}=\underbrace{UDEU^T}_{\text{symmetric}}\tag{19} $$ where $E$ is a diagonal matrix whose diagonal elements are $+1$ or $-1$. This is because the orthonormal bases for $U$ and $V$ are essentially determined up to sign and permutation. Thus, $M$ must be of the form $rW$ where $$ W=VEU^T\tag{20} $$ for some $r$ and an E as above, which we will determine soon. First, we need to analyze the condition for the minimality of $(2)$.

Minimizing The Square Residue

Here, we see that whether we vary the scale or not, we minimize $(2)$ by maximizing $$ \sum_{j=1}^m\left\langle\left(P_j-\overline{P}\right)W,Q_{\,j}-\overline{Q}\right\rangle\tag{21} $$ If we don't vary the scale, we lose $(14)$, but then we have $$ \begin{align} &\sum_{j=1}^m\left|P_jM+R-Q_{\,j}\right|^{\,2}\\ &=\sum_{j=1}^m\left\langle\left(P_j-\overline{P}\right)M-\left(Q_{\,j}-\overline{Q}\right),\left(P_j-\overline{P}\right)M-\left(Q_{\,j}-\overline{Q}\right)\right\rangle\\ &=r^2\sum_{j=1}^m\left|P_j-\overline{P}\right|^{\,2}+\sum_{j=1}^m\left|Q_{\,j}-\overline{Q}\right|^{\,2}-2r\sum_{j=1}^m\left\langle\left(P_j-\overline{P}\right)W,Q_{\,j}-\overline{Q}\right\rangle\tag{22} \end{align} $$ The first two sums are fixed, so we minimize $(2)$ by maximizing $(21)$.

If we vary the scale, we can use $(14)$ to get $$ r\sum_{j=1}^m\left|P_j-\overline{P}\right|^{\,2} =\sum_{j=1}^m\left\langle\left(P_j-\overline{P}\right)W,Q_{\,j}-\overline{Q}\right\rangle\tag{23} $$ Combining $(22)$ and $(23)$ yields $$ \sum_{j=1}^m\left|P_jM+R-Q_{\,j}\right|^{\,2} =\sum_{j=1}^m\left|Q_{\,j}-\overline{Q}\right|^{\,2} -r^2\sum_{j=1}^m\left|P_j-\overline{P}\right|^{\,2}\tag{24} $$ Both sums are fixed, so we minimize $(2)$ by maximizing $r$, and $(23)$ says that to maximize $r$, we need to maximize $(21)$.

Therefore, as claimed, whether varying the scale or not, we minimize $(2)$ by maximizing $(21)$.

Computation of $\boldsymbol{E}$

Let $I_k$ be the matrix with the $(k,k)$ element set to $1$ and all the other elements set to $0$. Letting $a_k$ be $+1$ or $-1$, we can write $$ E=\sum_{k=1}^na_kI_k\tag{25} $$ To maximize $(21)$, we can use $(20)$ and $(25)$ to get $$ \begin{align} &\sum_{j=1}^m\left\langle\left(P_j-\overline{P}\right)W,Q_{\,j}-\overline{Q}\right\rangle\\ &=\sum_{j=1}^m\left\langle\left(P_j-\overline{P}\right)VEU^T,Q_{\,j}-\overline{Q}\right\rangle\\ &=\sum_{k=1}^na_k\sum_{j=1}^m\left[\left(P_j-\overline{P}\right)V\right]_k\left[\left(Q_{\,j}-\overline{Q}\right)U\right]_k\tag{26} \end{align} $$ where $[X]_k$ is coordinate $k$ of the vector $X$. Thus, we can maximize $(21)$ by computing $$ c_k=\sum_{j=1}^m\left[\left(P_j-\overline{P}\right)V\right]_k\left[\left(Q_{\,j}-\overline{Q}\right)U\right]_k\tag{27} $$ and setting $$ a_k=\operatorname{sgn}(c_k)\tag{28} $$ Therefore, we can compute $E$ using $(25)$ and $(28)$.

From the Singular Value Decomposition $S=UDV^T$, we get that $D=U^TSV$. Using this in $(18)$, we get $$ D=\sum_{j=1}^m\left[\left(Q_{\,j}-\overline{Q}\right)U\right]^T\left[\left(P_j-\overline{P}\right)V\right]\tag{29} $$ Comparison of $(29)$ and $(27)$ shows that $D = \operatorname{diag}(c)$.

Computation of $\boldsymbol{r}$

Using $(20)$ to compute $W$, we can substitute $M=rW$ into equation $(14)$ to get $$ r^2\sum_{j=1}^m\left|P_j-\overline{P}\right|^{\,2} =r\sum_{j=1}^m\left\langle\left(P_j-\overline{P}\right)W,Q_{\,j}-\overline{Q}\right\rangle\tag{30} $$ Therefore, we can compute $r$ from $$ r\sum_{j=1}^m\left|P_j-\overline{P}\right|^{\,2} =\sum_{j=1}^m\left\langle\left(P_j-\overline{P}\right)W,Q_{\,j}-\overline{Q}\right\rangle\tag{31} $$ Effect of $\boldsymbol{a_k}$ and $\boldsymbol{c_k}$ on the square residue

Combining $(26)$ and $(27)$, we have the following equation for $(21)$: $$ \sum_{j=1}^m\left\langle\left(P_j-\overline{P}\right)W,Q_{\,j}-\overline{Q}\right\rangle =\sum_{k=1}^na_kc_k\tag{32} $$ If we don't vary the scale, we can use $(32)$ to get $$ \sum_{j=1}^m\left|P_jM+R-Q_{\,j}\right|^{\,2} =r^2\sum_{j=1}^m\left|P_j-\overline{P}\right|^{\,2}+\sum_{j=1}^m\left|Q_{\,j}-\overline{Q}\right|^{\,2}-2r\sum_{k=1}^na_kc_k\tag{33} $$ If we vary the scale, $(31)$ says $$ r=\left.\sum_{k=1}^na_kc_k\middle/\sum_{j=1}^m\left|P_j-\overline{P}\right|^{\,2}\right.\tag{34} $$ and therefore combining $(33)$ and $(34)$, we get $$ \begin{align} \sum_{j=1}^m\left|P_jM+R-Q_{\,j}\right|^{\,2} =\sum_{j=1}^m\left|Q_{\,j}-\overline{Q}\right|^{\,2}-\left.\left(\sum_{k=1}^na_kc_k\right)^2\middle/\sum_{j=1}^m\left|P_j-\overline{P}\right|^{\,2}\right.\tag{35} \end{align} $$ Summary of the Method

To find the least squares solution to $PM+R=Q$ for a given set of $\left\{P_j\right\}$ and $\left\{Q_{\,j}\right\}$, under the restriction that the map be conformal, we first compute the centroids $$ \overline{P}=\frac1m\sum_{j=1}^mP_j\quad\text{and}\quad\overline{Q}=\frac1m\sum_{j=1}^mQ_{\,j}\tag{36} $$ Next, compute the matrix $$ \begin{align} S &=\sum_{j=1}^m\left(Q_{\,j}-\overline{Q}\right)^T\left(P_j-\overline{P}\right)\\ &=\sum_{j=1}^mQ_{\,j}^TP_j-m\overline{Q}^T\overline{P}\tag{37} \end{align} $$ Let the Singular Value Decomposition of $S$ be $$ S=UDV^T\tag{38} $$ Next compute $\{c_k\}$ with $$ \begin{align} c_k &=\sum_{j=1}^m\left[\left(P_j-\overline{P}\right)V\right]_k\left[\left(Q_{\,j}-\overline{Q}\right)U\right]_k\\ &=\sum_{j=1}^m\left[P_jV\right]_k\left[Q_{\,j}U\right]_k-m\left[\overline{P}V\right]_k\left[\overline{Q}U\right]_k\tag{39} \end{align} $$ and define $$ a_k=\operatorname{sgn}\left(c_k\right)\tag{40} $$ Let $I_k$ be the matrix with the $(k,k)$ element set to $1$ and all the other elements set to $0$. Then calculate $$ E=\sum_{k=1}^na_kI_k\tag{41} $$ Compute the orthogonal matrix $$ W=VEU^T\tag{42} $$ if $\det(W)\lt0$ and $\det(W)\gt0$ is required, change the sign of the $a_k$ associated with the $c_k$ with the smallest absolute value.

If required, compute $r$ by $$ r\sum_{j=1}^m\left|P_j-\overline{P}\right|^{\,2} =\sum_{j=1}^m\left\langle\left(P_j-\overline{P}\right)W,Q_{\,j}-\overline{Q}\right\rangle\tag{42} $$ or equivalently, $$ r\left(\sum_{j=1}^m\left|P_j\right|^{\,2}-m\left|\overline{P}\right|^{\,2}\right) =\sum_{j=1}^m\left\langle P_jW,Q_{\,j}\right\rangle-m\left\langle\overline{P}W,\overline{Q}\right\rangle\tag{43} $$ Finally, we have the desired conformal map $Q=PM+R$ where $$ M=rW\tag{44} $$ and $$ R=\overline{Q}-\overline{P}M\tag{45} $$