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

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

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)...

Thus,

I could try to take the mean over 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