认知机器人：ICP扫描匹配

本文章收录在黑鲸智能系统知识库-黑鲸智能系统知识库成立于2021年，致力于建立一个完整的智能系统知识库体系。我们的工作：收集和整理世界范围内的学习资源，系统地建立一个内容全面、结构合理的知识库。

作者博客：途中的树

动机

背景

• 如果机器人的姿势是已知的，我们就可以通过累加点云point cloud来获得准确的环境的三维表现

问题

• 由于机器人脚（或者轮子）的打滑和传感器的噪音，测距仪的估计容易产生误差
• 相应地，连续的深度相机观测可能不一致
• 误差会随着时间的推移越来越大!

解决方案

• 对多次扫描的点云对齐以减少误差
• 对齐（Alignment）
  • 估计两个点云之间的相对变换
  • 目标：找到最能对齐相应数据点的变换参数
  • 迭代最接近点（ICP）算法
  • 寻找局部变换来对齐点
    

方法

问题定义

• 已知两个点集：

  • Q = { q 1 , . . . , q N } Q=\{q_1,...,q_N\} Q={q1​,...,qN​}
  • P = { p 1 , . . . , p M } P=\{p_1,...,p_M\} P={p1​,...,pM​}
  • 两个点集内的点对应的链接 C = { ( i , j ) } \mathcal{C}=\{(i,j)\} C={(i,j)}, $i\in N,j\in M$

• 目标：求用于最小化平方误差之和的平移向量 $t$ 和旋转矩阵 $R$

  • $E(R,t)={\sum }_{(i,j)\in \mathcal{C}}||q_i-(R{p}_j+t)|{|}^2$

如果两个点集内的点与点之间的关系是已知的

• 如过两个点集内的点与点之间的关系是已知的，则正确的平移向量和旋转矩阵可以直接被计算出来

  1. 通过质量中心（COM）转移
  2. 旋转对准

  

点集的质心COM转移

• 计算两组中相应点的质心COMs：
  • ${\mu }_Q=\frac{1}{|C|}{\sum }_{(i,j)\in C}{q}_i$
  • ${\mu }_P=\frac{1}{|C|}{\sum }_{(i,j)\in C}{p}_i$
• 将两个点集里的点减去对应的质心COM
  • Q ′ = { q i − μ Q } = q i ′ Q'=\{q_i-\mu_Q\}={q'_i} Q′={qi​−μQ​}=qi′​
  • P ′ = { p i − μ P } = p i ′ P'=\{p_i-\mu_P\}={p'_i} P′={pi​−μP​}=pi′​

最小化误差方程

• 最小化方程

  • $E(R,t)={\sum }_{(i,j)\in \mathcal{C}}||q_i-(R{p}_j+t)|{|}^2$

• 可以通过奇异值分解（SVD）来解决

  1. 计算交叉协方差矩阵
     • $W={\sum }_{(i,j)\in \mathcal{C}}{q}_i^{\prime }{p^{\prime }}_j^T$
  2. 求SVD分解
     • $W=UD{V}^T$
     • 矩阵$U,V$是$3\ast 3$的旋转矩阵
     • 对角线矩阵
       • $D=Diag({\sigma }_1,{\sigma }_2,{\sigma }_3)$ ，$W$的奇异值
  3. 如果$rank(W)=3$$W$($W$的秩是3)则最小化误差函数$E(R,t)$得
     • $R=U{V}^T$
     • $t={\mu }_Q-R{\mu }_P$
  4. 平移旋转点集
     • $p_j\leftarrow R{p}_j+{\mu }_Q-R{\mu }_P$
       

如果两个点集之间的关系未知

如果不知道正确的对应关系，通常不可能在一个循环中确定最佳的相对旋转和平移，因此需要迭代。

基本的迭代最接近点（ICP）算法

• 思路：迭代以找到最佳对应关系
• 迭代最接近点[Besl & McKay 92]。
• 如果最初的猜测 "足够接近 "就会收敛

init error = inf //初始化误差
while(error decresed and error > threshold)
	Determine corresponding points //确定点集对应关系
	Compute rotation R, translation t via SVD //通过SVD计算R和t
	Apply R and t to the points of set to be registered // 应用R和t到点集
	error = E(R,t) //计算误差

ICP变体

• 在基本的ICP算法基础上，针对以下几个方面进行优化可以得到不同的ICP算法变体
  • 选择点子集的方式（从一个或两个点子集）
  • 对应关的权重
  • 数据关联 Data association
  • 拒绝（剔除）离群的点对

ICP性能评测

• 如何评估ICP算法的性能？
  • 收敛速度
  • Robustness 耐用性(噪声和异常值，复杂的三维结构）
  • 收敛盆地Basin of convergence（最大的初始错位）

点集的选择方法

• 使用所有的点

• 均匀的分项抽样

• 随机抽样

• 基于特征的采样 Feature-based sampling

  • 尝试找到 "重要 "的点
  • 简化了对应关系的搜索
  • 更高的效率和准确性
  • 需要进行预处理
  • 例子
    • 在使用俯视摄像机的机器人导航中,地面通常在场景中占主导地位
    • 过滤掉属于地面的点

• 正态空间采样 Normal-space sampling

  • 选择与表面曲率变化成正比的点
  • 正态空间取样对于具有稀疏特征的大部分平滑区域表现更好

  

数据关联

• 对收敛性和速度的影响最大

• 匹配方法

  • 最接近的点

    • 在另一个点集中寻找最近的点（使用kd-trees）。
    • 高度依赖于对准的初始猜测
    • 可能会导致收敛速度慢，并且需要预处理
      

• 正态射击

  • 沿法线投影，与其他点集的网格相交，选择最近的点
  • 对于平滑的结构，收敛结果比最近点稍好，对于噪声或复杂的结构，收敛结果较差。

• 点到平面
  • 最小化一个点与另一表面的切平面之间的平方距离之和
  • 点对平面的误差矩阵
    • 使用点到平面的距离而不是点到点的距离，可以让平面区域 “相互滑动”
    • 每次迭代通常比点对点版本慢，然而，通常收敛率明显更好
    • 使用非线性最小二乘法来解决

• …

拒绝离群的点对

• Outier point pairs ：点对点距离高于给定阈值的对应点

• 或者拒绝与相邻的配对不一致的配对
  
  

ICP算法总结

• 潜在的子样点
• 确定相应的点
• 潜在的加权或拒绝配对
• 计算旋转R、平移t（通过SVD）。
• 将R和t应用于要注册的集合的所有点
• 计算误差E(R,t)
• 当误差减少且误差>阈值时。
• 重复确定对应关系等。
• 输出最终对齐方式

步行中的扫描匹配

• 通过将每个点云与之前的点云对齐来进行正确的姿势估计

• 根据里程表估计值初始化ICP

• 转换矩阵$T_{odom}^t:$ 描述在时间$t$时相对于环境坐标系的相机坐标系的坐标的位置转换过程

• 纠正的姿势 $T^t=T_{corr}^t{T}_{odom}^t$

  • $T_{corr}^t=argmi{n}_{T^{\prime }}{\sum }_i||T^{t-1}{\mathrm{q}}_i-T^{\prime }{T}_{odom{\mathrm{p}}_i}^t|{|}^2$
  • $T_{corr}^t$用以校正两个连续获得的点云 ${\mathrm{q}}_i\leftrightarrow {\mathrm{p}}_i$

• ICP 的作用就是递归的提升对应关系 ${\mathrm{q}}_i\leftrightarrow {\mathrm{p}}_i$ 和 $T_{corr}^t$
  

步骤

1. 估计在时间$t-1$时正确的相机姿态

2. 根据在现在时间和$T^{t-1}$之间，估计在时间$t$时的相机姿态

3. 在通过ICP进行扫描匹配之后估计时间$t$时的相机姿态

应用

• 从深度摄影机的观察中学习高度图

• 扫描匹配过程的动画（ICP迭代）youtube

https://www.youtube.com/watch?v=1rUskOfW-Gs

总结

• 强大的算法，用于计算点云之间的转换
• 只要有正确的数据关联，就可以使用SVD有效地计算转换。
• 主要问题：确定正确的数据关联，ICP是迭代式的
• ICP并不总是收敛到正确的排列（而是收敛到局部最小值）。
• 性能取决于初始猜测

参考

• A method for registration of 3-D shapes, P.J. Besl and N.D. McKay, IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992
• Efficient Variants of the ICP Algorithm, S. Rusinkiewicz and M. Levoy, Prof. of the Int. Conf. on 3D Digital Imaging and Modeling, 2001
• Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration, **K.-L. Low,**Technical Report Univ. of North Carolina, 2004
• Chapter 5, Camera-Based Humanoid Robot **Navigation,**Daniel Maier, PhD thesis, 2015