迭代最近点实现

我目前正在使用以下伪代码在C＃中实现ICP算法。 从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 

但是我不完全确定应该如何实现更新对齐？ 如果有人能解释这一点比powerpoint更清楚:)我已经编写了计算对应错误和对齐错误的方法，但是我不知道如何应用这些来获得新的更新对齐。

Answer 1

幻灯片10上的公式（它们实际上是两种编写相同公式的等效方式）可以给出对齐的均方误差。 您希望选择旋转和平移（q向量是这些的组合）以最小化均方误差。 MSE是q向量的一个很好的，可微分的函数，因此很容易使用像Conjugate Gradient Method这样的东西，从当前对齐开始，找到一个新的对齐方式（至少在本地）最小化MSE。