Minimizing mean square error for a body tracking problem

Seeking big-picture suggestions on how to tackle the following problem: I have measurements of the positions of 4 marker locations on a 3D body, and I know how to write the rotation/translation matrix given the body's unknown yaw, pitch, roll, and x,y,z translation. I want to solve for the yaw, pitch, etc. which minimizes the error of [markers' estimated separations] - [known marker separations]. I could loop through the expected range of yaw, pitch, etc., calculate the resulting marker separation, and choose the combination with smallest distance from their known separation. But is there a way to perform the minimization in 1 step? I assume this would be faster, especially since this will be inside a time loop.

Answer 1

[Anton Semechko]

No pen and paper necessary. Here is code for the function that I already have:

function [R,t,s]=SymTformParams(Xref,Xsrc)
% Compute rotation (R), translation (t) and scaling (s) parameters, such 
% that Xref=s*R*Xsrc+t, where Xref and Xsrc are two point sets, respectively.
%
% - Xref,Xsrc   : N-by-3 arrays of corresponding point coordinates.
% - R         : 3x3 rotation matrix.
% - t         : 1x3 translation vector.
% - s         : scaling factor.
%
% AUTHOR: Anton Semechko ([e-mail])
% DATE: Apr.2012
%

% Find the centroids
Cref=mean(Xref,1);
Csrc=mean(Xsrc,1);

% Center the point sets
Xref=bsxfun(@minus,Xref,Cref);
Xsrc=bsxfun(@minus,Xsrc,Csrc);
N=size(Xref,1);

% Compute the covariance matrix
Cmat=(Xref'*Xsrc)/N;

% Find rotation using SVD
[U,~,V] = svd(Cmat);
R=U*diag([1 1 det(U*V')])*V';

% Find the scaling factor
Xref_L2=sum(Xref.^2,2);
Xsrc_L2=sum(Xsrc.^2,2);
s=Xref_L2./Xsrc_L2;
s=mean(sqrt(s));

% Translation 
t=Cref-(s*R*Csrc')';

[K E]

Fabulous. What a leg up, thanks.

Answer 2

[Anton Semechko]

If point correspondences are known, do the following:

1) solve for translation, by comparing the centroids of the landmark sets before and after the transformation 2) normalize for translation and find rotation matrix using SVD (<http://kwon3d.com/theory/jkinem/rotmat.html>)

If you are interested in finding the Euler angles, you can use the 't2x' which can be downloaded from here: http://www.mathworks.com/matlabcentral/fileexchange/956-3d-rotations.

[K E]

Thanks, Anton. It is paper and pencil time to work through this.