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