运动畸变去除

1.纯估计法
2.里程计辅助法

1、纯估计方法

ICP（变形）

• 目的
  
  给定两个点云集合:

𝑋 = {𝑥1, 𝑥2, ⋯ , 𝑥𝑁𝑥}
𝑃 = {𝑝1, 𝑝2, ⋯ , 𝑝𝑁𝑝}

求解R和t，使得下式最小:

找到一个R和t，使两个点云距离最小

X和P为 图中黄色和白色形成的点云集合，假设他们的点云都是一一匹配的（打向物理空间的同一个点）。

1、已知对应点的求解方法
> 通过闭环 close-from 求得一个解析解，

1. 求x,p点云平均位姿
2. 把每个点云移到平均位姿
3. 假设一个如上式的w,并对其进行svd分解
4. 显然得，同一坐标系下的解为一个向量t

ICP，R=UV(T)式，可以查阅相关论文或博客ICP去除运动畸变

2、位置对应点的求解方法

• 实际中，不知道对应点匹配
• 不能一步到位计算出R和t
• 进行迭代计算
• EM算法的一个特例

ICP算法为EM算法的一个特例，求解非线性MLE（极大似然估计）的工具。

（A（R和t）,B(点对应)隐变量)–先固定A求B，再固定B求A，循环迭代得到收敛位姿过程。

算法流程

对算法的优化也是从以下四个点进行的。

• 寻找对应点
• 根据对应点，计算R和t
• 对点云进行转换，计算误差
• 不断迭代，直至误差小于某一个