Iterative Closest Point Implementation

Question

I am currently using the following pseudo code to implement the ICP algorithm in C#. Obtained from ICP Powerpoint

function ICP(Scene,Model)
 begin
  E` = + ∞;
  (Rot,Trans) = In Initialize-Alignment(Scene,Model);
  repeat 
E = E`;
Aligned-Scene = Apply-Alignment(Scene,Rot,Trans);
Pairs = Return-Closest-Pairs(Aligned-Scene,Model);
(Rot,Trans,E`) = Update-Alignment(Scene,Model,Pairs,Rot,Trans);
  Until |E`- E|  < Threshold
  return (Rot,Trans);
 end    

However I am not entirely sure how update alignment should be implemented? If someone could explain this a little clearer than the powerpoint that would be great :) I have written the methods for calculating correspondence error and alignment error, however I am not sure how to apply these to get the new updated alignment.

Answer 1

The formulas on slide 10 (they are actually two equivalent ways of writing the same formula) give you the mean squared error of your alignment. You want to choose your rotation and translation (the q vector is the combination of these) to minimize the mean squared error. The MSE is a nice, differentiable function of the q vector, so it is easy to use something like the Conjugate Gradient Method starting from the current alignment to find a new alignment that (at least locally) minimizes the MSE.