C# 迭代最近点实现

我目前正在使用以下伪代码在C#中实现ICP算法。从

功能ICP（场景、模型）
开始
E`=+∞;
（旋转、平移）=初始对齐（场景、模型）；
重复
E=E`；
对齐场景=应用对齐（场景、旋转、平移）；
Pairs=返回最近的对（对齐的场景、模型）；
（Rot，Trans，E`）=更新对齐方式（场景、模型、对、Rot，Trans）；
直到| E`-E |<阈值
返回（Rot，Trans）；
结束

---

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

幻灯片10中的公式（它们实际上是编写相同公式的两种等效方法）给你校准的均方误差。您需要选择旋转和平移（q向量是这些的组合）以最小化均方误差。MSE是q向量的一个很好的可微函数，因此可以很容易地从当前对齐开始使用共轭梯度法来找到一个新的对齐方式，该对齐方式（至少在局部）使MSE最小化。

你好，里克，你能完成ICP C#实现吗？你能和我们分享一个答案吗？

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