Calculate rotation/translation matrix to match measurement points to nominal points

Question

I have two matrices, one containing 3D coordinates that are nominal positions per a CAD model and the other containing 3D coordinates of actual measured positions using a CMM. Every nominal point has a corresponding measurement, or in other words the two matrices are of equal length and width. I'm not sure what the best way is to fit the measured points to the nominal points. I need a way of calculating the translation and rotation to apply to all of the measured points that produce the minimum distance between each nominal/measured pair of points while not exceeding allowed tolerances on maximum distance at any other point. This is similar to Registration of point clouds but different in that each pair of nominal/measured points has a unique tolerance/limit on how far apart they are allowed to be. That limit is higher for some pairs and lower for others. I'm programming in .Net and have looked into Point Cloud Library (PCL), OpenCV, Excel, and basic matrix operations as possible approaches. This is a sample of the data

X Nom    Y Nom  Z Nom   X Meas  Y Meas  Z Meas  Upper Tol   Lower Tol
118.81  2.24    -14.14  118.68  2.24    -14.14  1.00    -0.50
118.72  1.71    -17.19  118.52  1.70    -17.16  1.00    -0.50
115.36  1.53    -24.19  115.14  1.52    -23.98  0.50    -0.50
108.73  1.20    -27.75  108.66  1.20    -27.41  0.20    -0.20

Below is the type of matrix I need to calculate in order to best fit the measured points to the nominal points. I will multiply it by the measured point matrix to best fit to the nominal point matrix.

    Transformation  
0.999897324 -0.000587540    0.014317661
0.000632725 0.999994834 -0.003151567
-0.014315736    0.003160302 0.999892530
-0.000990993    0.001672040 0.001672040

Answer 1

The problem is called (among other names) "absolute orientation." There are several solutions to this. I like the one by Umeyama, which is implemented in Eigen (and pcl) see here

The solution is elegant and easy. Assuming the noise in your data is Gaussian, here is some Matlab code to compute a transformation

function [R,t,c] = umeyama(x, y, do_similarity)
  % function [R,t,c] = umeyama(x, y, do_similarity)
  %
  % Computes the absolute orientation between the two point set such that 
  %
  %   y = Rx + t + epsilon
  %
  % This is a least squares methods as descibed in 
  %
  %   S. Umeyama, ``Least Squares Estimation of Transformation Parameters
  %   Between Two Point Patterns,'' PAMI, 1991
  %
  % INPUT 
  %    x, y the two point sets in correspondence 
  %    do_similarity if true we estimate the scale factor between the two point
  %    sets (a similarity transform instead of only a Euclidean)
  %
  % OUTPUT 
  %    R,t,c  transformation parameters such that 
  %
  %       y = cRx + t + noise
  % 

  narginchk(2,3);
  if nargin < 3, do_similarity = false; end
  assert(isequal(size(x),size(y)), 'inputs must have the same size');
  assert(size(x,2) > size(x,1), 'insufficient number of points'); 

  % compute the mean 
  x_bar = mean(x,2);
  y_bar = mean(y,2);

  % center the points (eliminate the translation)
  x = bsxfun(@minus, x, x_bar);
  y = bsxfun(@minus, y, y_bar);

  % compute the covariance 
  Sigma = y*x';

  % compute the optimal rotation and account for reflections 
  [U,D,V] = svd(Sigma); 
  S = diag([1 1 sign(det(U)*det(V))]); % for reflections
  R = U*S*V';

  if do_similarity 
    c = trace(D*S)./sum(sum(x.^2,2));
  else 
    c = 1.0;
  end 

  % the translation 
  t = y_bar - c*R*x_bar;

end 

On a different note, the example matrix you provided is not a rigid body transformation.