Thread: Trying to find "best" rotation matrix

1. Trying to find "best" rotation matrix

$\displaystyle \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.$

$\displaystyle \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 $\displaystyle A_i$, I'm looking to find a least squares (or some other optimal form) of $\displaystyle R$ such that

$\displaystyle 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 ($\displaystyle A_i$). However, the orientation needs to be calibrated (hence the separate $\displaystyle 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 $\displaystyle 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)...

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

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

Thus,
$\displaystyle 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 $\displaystyle 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

$\displaystyle 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 $\displaystyle A_{i}, L, c_{x}, c_{y},$ and $\displaystyle c_{z}$ are all known? If that's the case, can't you come up with a general description of the matrix $\displaystyle \overline{R}$ in terms of, say, Euler angles, and combine that matrix with

$\displaystyle \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 $\displaystyle A_i$ and $\displaystyle p_i$, then that'd be the way to go obviously. However, I have an overdetermined system with a BUNCH of $\displaystyle A_i$ and $\displaystyle 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.