迭代最近点实现

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    

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

Answer 1

第 10 张幻灯片上的公式（它们实际上是编写同一公式的两种等效方法）为您提供对齐的均方误差。您想要选择旋转和平移（q 向量是它们的组合）以最小化均方误差。 MSE 是 q 向量的一个很好的可微函数，因此很容易使用诸如共轭梯度法之类的方法从当前对齐开始找到一个新的对齐（至少局部地）最小化 MSE。