迭代最近点算法实现

Question

我目前正在使用以下伪代码在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更清晰地解释一下，那就太好了 :) 我已经编写了计算对应误差和对齐误差的方法，但我不确定如何应用这些方法来获得新的更新对齐。

- Rick

---

你好Rick，你已经完成ICP C#实现了吗？能否将其作为答案与我们分享？ - Pedro77

Answer 1

幻灯片10上的公式（实际上是写同一个公式的两种等价方式）给出了您对齐的均方误差。您希望选择旋转和平移（q向量是这些的组合）以最小化均方误差。MSE是q向量的一个漂亮、可微分函数，因此可以使用类似共轭梯度法的方法从当前对齐开始找到一个新的对齐，使得MSE（至少在局部范围内）最小化。

- deinst