# Math Help - Trying to find "best" rotation matrix

1. ## Trying to find "best" rotation matrix

$
\text{Let }A_i =
\left[ \begin{array}{cc}
R_i & p_i \\
0 & 1\end{array}\right]
\text{ where } R_i \in SO[3] \text{ , and } p_i \in R^3.
$

$
\text{Further, let } \overline{R} =
\left[ \begin{array}{cc}
R & 0 \\
0 & 1\end{array}\right]
\text{ where } R \in SO[3].
$

Now, given a set of $A_i$, I'm looking to find a least squares (or some other optimal form) of $R$ such that

$
A_i \overline{R}
\left[\begin{array}{c}0 \\ -L \\ 0 \\ 1 \end{array}\right]
=
\left[\begin{array}{c}c_x \\ c_y \\ c_z \\ 1 \end{array}\right]
$

I've tried to present the mathematical description of a problem I'm facing. I have a rod that is connected to the end of a 6dof robot. The robot provides position/orientation information in the form of a homogeneous transform ( $A_i$). However, the orientation needs to be calibrated (hence the separate $R$ matrix). To do so, I fix the end of the rod and trace out a sphere which provides me the position and orientation vectors that should all intersect at the center point $c$.

Obviously if there were only 1 data point (i.e. 1 position vector, 1 orientation matrix, and 1 center point), the solution could (rather trivially) be determined in a unique fashion. What I'm looking for is a way to solve this in a least squares (or some other "optimal") sense. Ideas?

My best has been to use the following (note: slightly different notation here as I'm using inhomogeneous coordinates)...

$
\text{Let } A_i \in SO[3], p_i \in R^3, R \in SO^3, c \in R^3.
$

$
A_i R
\left[ \begin{array}{c} 0 \\ -L \\ 0 \end{array}\right]
+ p_i
=
c
$

Thus,
$
R_2 = \frac{-1}{L} A_i^T (c - p_i) \text{ where } R_2 \text{ is the 2nd column of } R
$

I could try to take the mean over $R_2$ and attempt to solve for the entire matrix using the single column, but it will certainly be messy.

EDIT: On 2nd thought, I can't solve for the matrix with only a single column

2. Warning: this might be a somewhat boneheaded approach. Given the equation

$A_i \overline{R}
\left[\begin{array}{c}0 \\ -L \\ 0 \\ 1 \end{array}\right]
=
\left[\begin{array}{c}c_x \\ c_y \\ c_z \\ 1 \end{array}\right],$

isn't it true that $A_{i}, L, c_{x}, c_{y},$ and $c_{z}$ are all known? If that's the case, can't you come up with a general description of the matrix $\overline{R}$ in terms of, say, Euler angles, and combine that matrix with

$\left[\begin{array}{c}0 \\ -L \\ 0 \\ 1 \end{array}\right],$

then solve the resulting system, and finally solve the smaller problem? It's kind of like this: Solve ABx = b by first setting y = Bx, then solve Ay = b, and then solve y = Bx. The only difference is that in your second problem, you're solving for the matrix instead of the vector.

Any reason why this wouldn't work?

3. If I were trying to solve for R given a single $A_i$ and $p_i$, then that'd be the way to go obviously. However, I have an overdetermined system with a BUNCH of $A_i$ and $p_i$. Therefore, even if I were to solve each one individually, the best I'd be able to do is to average them (which isn't exactly robust to outliers).

4. Well, it's beyond me, I'm afraid. Maybe Captain Black can help you.