2020 波恩大学《传感器及状态估计-ICP部分》学习笔记（二）附论文资料下载链接

资料下载链接

Prof. Dr. Cyrill Stachniss

个人简介：https://www.ipb.uni-bonn.de/people/cyrill-stachniss/
课程资料下载：https://www.ipb.uni-bonn.de/msr2-2020/

YouTube: y2u.be/4QG0y0pIOBE
Bilibili: www.bilibili.com/video/BV1DK411L7M6

课程视频（附字幕）
https://mp.weixin.qq.com/s/LKHbeeACGvXiotpJxGNH9Q

ICP（iterative closest point）经典论文下载链接：
https://sci-hub.im/10.1109/34.121791或者
http://www-evasion.inrialpes.fr/people/Franck.Hetroy/Teaching/ProjetsImage/2007/Bib/besl_mckay-pami1992.pdf

第二课 ICP : Point Cloud Alignment

ICP迭代最近点算法针对激光的sensor data处理以及3D重建有很重要的作用。两次位置观测同一物理实体，目的是找到转换关系，让两帧点云对齐。对齐的方法可以采用最小二乘或者稳健最小二乘；以及ICP。

为什么要对齐点云？因为在实际中，我们可能是从不同角度观测不同以及相同物理实体，如果未知自身位置时，需要对点云进行旋转平移。

1、ICP问题的数学描述

• 假设给定的两个点云 集合分别为P、Q:

Q = { q 1 , … , q N } P = { p 1 , … , p M } Q=\left\{\boldsymbol{q}_{1}, \ldots, \boldsymbol{q}_{N}\right\} \quad P=\left\{\boldsymbol{p}_{1}, \ldots, \boldsymbol{p}_{M}\right\} Q={q1​,…,qN​}P={p1​,…,pM​}

• 其中两点集对应的已知点对为C:

C = { ( i , j ) } \mathcal{C}=\{(i, j)\} C={(i,j)}

• 求解使得误差平方和最小的R和T（其中2d:xy+rotation;3d:3shift+3rotation）

$$E(R,t)=\sum _{(i,j)\in C}{\Vert {\mathbf{q}}_i-R{\mathbf{p}}_j-\mathbf{t}\Vert }^2$$

2、假设理想情况下求解-两组点云能够完全重合

• 但是实际情况中存在噪声与误差，需要采用一定方法最小化误差

3、ICP求解过程
（1）点云中心化-center of mass

• 分别计算两组点云中心,这里假设参与计算的点对都来自C，即都对应点的数目为C

$${\mathbf{\mu }}_Q=\frac{1}{|C|}\sum _{(i,j)\in C}{\mathbf{q}}_i{\mathbf{\mu }}_P=\frac{1}{|C|}\sum _{(i,j)\in C}{\mathbf{p}}_j$$

• 坐标原点对齐：每一点减去相应点云中心，即统一坐标系的一个过程

Q ′ = { q i − μ Q } = { q i ′ } P ′ = { p j − μ P } = { p j ′ } \begin{array}{l} Q^{\prime}=\left\{\boldsymbol{q}_{i}-\boldsymbol{\mu}_{Q}\right\}=\left\{\boldsymbol{q}_{i}^{\prime}\right\} \\ P^{\prime}=\left\{\boldsymbol{p}_{j}-\boldsymbol{\mu}_{P}\right\}=\left\{\boldsymbol{p}_{j}^{\prime}\right\} \end{array} Q′={qi​−μQ​}={qi′​}P′={pj​−μP​}={pj′​}​

（2）Orthogonal Procrustes Problem-可以解决目标函数的求解问题

https://blog.csdn.net/itnerd/article/details/104598742正交普鲁克问题：本质是F范数，定义为求所有元素的平方和。

$$\Vert X{\Vert }_F=\sqrt{\mathrm{trace}\left(X^{\top }X\right)}=\sqrt{\sum _{i,j}{x}_{ij}^2}$$

• 结合ICP求解的目标函数
  $$E(R,t)=\sum _{(i,j)\in C}{\Vert {\mathbf{q}}_i-R{\mathbf{p}}_j-\mathbf{t}\Vert }^2$$

• 最小化目标函数，其中误差函数E prime只考虑旋转矩阵R,等号右边第一项代表点云从Q中抽象出来，依次按照矩阵形式排列；P矩阵中的点经过旋转矩阵R的变换，两者之间作差，等价于
  $$E^{\prime }(R)={\Vert \left[{\mathbf{q}}_1^{\prime }\dots {\mathbf{q}}_n^{\prime }\right]-R\left[{\mathbf{p}}_1^{\prime }\dots {\mathbf{p}}_n^{\prime }\right]\Vert }_F^2$$

• F范数计算对应点之间的差异，2范数计算值得平方。通过SVD（Singular Value Decomposition）分解进行求解

（3）SVD（Singular Value Decomposition）分解求解过程

• 计算协方差矩阵

$$W=\sum _{(i,j)\in C}{\mathbf{q}}_i^{\prime }{\mathbf{p}}_j^{\prime \top }$$

• SVD分解-可以具体参考下方链接

https://www.cnblogs.com/endlesscoding/p/10033527.html

SVD用在图像数据压缩中，奇异值可以被看作成一个矩阵的代表值，或者说，奇异值能够代表这个矩阵的信息。当奇异值越大时，它代表的信息越多。因此，我们取前面若干个最大的奇异值，就可以基本上还原出数据本身。
$$W=UD{V}^{\top }$$
其中U、V为3*3的旋转矩阵，对角矩阵D如下
$$D=\mathrm{Diag}\left({\sigma }_1,{\sigma }_2,{\sigma }_3\right)$$

• 计算R,T

$$\begin{array}{rl}R& =U{V}^{\top }\\ t& ={\mu }_Q-R{\mu }_P\end{array}$$

• 计算出一组R,T，不断迭代求解
  $${\mathbf{p}}_j\leftarrow R\left({\mathbf{p}}_j-{\mathbf{\mu }}_P\right)+{\mu }_Q$$
• SVD图形化表示，其中黑色点状为噪声值
  
  4、假设未知数据关联
  （1）图形化表述
  如果不知道正确的对应关系，通常不可能一步确定最佳的相对旋转和平移。
  
  需要对未知数据之间的关联关系作一个假设，不断迭代，对齐。

（2）基础的ICP过程

• 假设error=inf(infinite)无穷大
• 当误差一直大于给定阈值，while (error decreased and error > threshold)
• 确定对应点，Determine corresponding points
• 通过SVD，计算R和T
• 将R和T应用到待对齐点上，Apply R and t to the points of the set to be registered
• 计算误差函数R=(R,T)
• 不断迭代，当关联关系不再改变，结束迭代

5、ICP变形的具体表现量

（1）速度speed，在真实世界应用当中，如果为了考虑算法的效率而减少点的数目，speed显得很重要
（2）稳定性Stability (local minima)，比如3d lidar,针对每秒10次或者20次的采样频率，采集点的数目是巨大的，需要减少点的数目，这个过程中，要保证稳定性，局部最小值
（3）潜在的离散群Tolerance w.r.t. noise and outliers，scan过程中的行人，是我们最后想要移除的
（4）收敛性Basin of convergence，最大化初始的对准误差

6、ICP方法的变体-目标函数（构建误差函数）的不同。之前都是建立在点与点之间的对应关系，比如点到平面（针对稀疏采样点的平面）

（1）减少采样点的数目 (from one or both point sets)

• Use all points
• Uniform sub-sampling 均匀采样，针对flat平坦区域较好
• Random sampling 随机采样
• Feature based sampling 基于特征的采样
• Normal-space sampling 法向量空间采样（法向采样保证了法向上的连续性Ensure that samples have normals distributed as uniformly as possible）可以更好的保留曲率变化明显的区域，以及具有稀疏特征的平滑区域
  

推荐论文下载链接https://www.cs.princeton.edu/~smr/papers/fasticp/fasticp_paper.pdf
这篇论文主要对ICP的变体-引入了一种基于法线空间均匀采样的新变式。
Abstract:The ICP (Iterative Closest Point) algorithm is widely used for geometric alignment of three-dimensional models when an initial estimate of the relative pose is known. Many variants of ICP have been proposed, affecting all phases of the algorithm from the selection and matching of points to the minimization strategy. We enumerate and classify many of these variants, and evaluate their effect on the speed with which the correct alignment is reached. In order to improve convergence for nearly-flat meshes with small features, such as inscribed surfaces, we introduce a new variant based on uniform sampling of the space of normals. We conclude by proposing a combination of ICP variants optimized for high speed. We demonstrate an implementation that is able to align two range images in a few tens of milliseconds, assuming a good initial guess. This capability has potential application to real-time 3D model acquisition and model-based tracking.

实验对比


可以看出，normal-space sampling对曲率变化较大区域，对齐的更好。有时候，我们在跑cartographer的时候，如果有圆形区域，也会通过观察这部分区域是否构成一个完整圆润的圆形，以及墙壁的厚度，来判断本次扫描的效果。
同时，基于特征的ICP，通常需要对数据进行预处理，找到特征明显的点，地面对地面，墙壁对墙壁，简化对应点的搜索范围，具有更高的效率与精度。

（2）为相关对应关系设定权重
就双目相机以及深度相机，为了衡量测定某些点的不确定性，增加一些点的信息，对点云信息加入权重，更好衡量每个点对对齐的作用；但是激光测量中，不太需要，因为激光的精度很高。可仍然需要考虑其他传感器噪声的影响，假设部分点的权重，降低影响。

• Weight the corresponding pairs 对应点对加权
• Noise: Weighting based on sensor uncertainty 基于传感器不确定性的加权
• Outlier: Assign lower weights for points with higher point-point distances 离群值：距离越远的点，分配越小的权重
• Determine transformation that minimizes the weighted error function 确定一个转换，使得误差函数最小

（3）数据关联
估计给定的原始对齐数据，即找到对应的点集C。数据关联对收敛的速度影响很大。有几种常用的数据关联方法：Closest point、Normal shooting、Closest compatible point、Point-to-plane（kinetic type sensor popolar）、Projection-based approaches

• Closest-Point Matching
  使用kd-trees，在对应点集上找到最邻近点，一般情况下，比较稳定，但是收敛速度较慢，需要进行预处理，也需要一个良好的初值。

https://blog.csdn.net/app_12062011/article/details/51986805 Kd-树是K-dimension tree的缩写，是对数据点在k维空间（如二维(x，y)，三维(x，y，z)，k维(x1，y，z…)）中划分的一种数据结构，主要应用于多维空间关键数据的搜索（如：范围搜索和最近邻搜索）。本质上说，Kd-树就是一种平衡二叉树 。

• Normal Shooting
  沿点法向量投影，与待匹配点集相交。
  光滑结构实体的收敛结果略好于Closest-Point Matching，而噪声或复杂结构的收敛结果较差。
  
• 兼容性 Robustification by considering the compatibility of the points，除了提到的法向量，可以通过兼容性用其他的特征进行替换，比如颜色、曲率、高阶导数（Higher-order derivatives）。
• point to plane,最小化一点到对应点的切平面距离的平方和,具体可以参看论文

论文下载链接 https://graphics.stanford.edu/~smr/ICP/comparison/chen-medioni-align-rob91.pdf

Abstract：We study the problem of creating a complete model of a physical object.Although this may be possible using intensity images,we use here range images which directly provide access to three dimensional information.The first problem that we need to solve is to find the transformation between the different views.Previous approaches have elther assumed this transformation to be known (which is extremely diffcult for a complete model), or computed it with feature matching (which is not accurate enough for integration).In this paper,we propose a new approach which works on range data directly,and registers successive views with enough overlapping areato get an accurate transformation between views.This is performed by minimizing a functional which does not require point to point matches. We give the details of the registration method and modeling procedure,and illustrate them on real range images of complex objects.

在采样频率很低的情况下，point to point很容易出错，而point to plane可以更好的实现这一点，让平面区域相互滑动；每次迭代通常比point to point 速度更慢，然而，通常收敛速度明显更好。

（4）识别异常点对（比如有人经过的场景）
对异常点进行鉴别，需要设定阈值。图中蓝色值比红色值小，所以阈值设为与蓝色相关。

• 如果相邻点对之间距离差异很大，就选择拒绝。

参考论文链接
http://graphics.stanford.edu/~smr/ICP/comparison/dorai-reg-pami98.pdf
Abstract—Automatic 3D object model construction is important in applications ranging from manufacturing to entertainment,since CAD models of existing objects may be either unavallable or unusable.We describe a prototype system for automatically registering and integrating multiple views of objects from range data. The results can then be used to construct geometric models of the objects.New techniques for handling key problems such as robust estimation of transiormations relating multiple views and seamless integration of registered data to form an unbroken surface have been proposed and implemented in the system.Experimental results on real surface data acquired using a digital interferometric sensor as well as a laser range scanner demonstrate the good performance of our system.

• Trimmed ICP: Sort correspondences w.r.t. their error, ignore the worst t%.

论文下载链接
https://sci-hub.tw/10.1109/ICPR.2002.1047997
ppt链接：http://www.docin.com/p-1585538278.html
Abstract：The problem of geometric alignment of two roughly preregistered, partially overlapping, rigid, noisy 3D point sets is considered. A new natural and simple, robustified extension of the popular Iterative Closest Point (ICP) algorithm (Besl and McKay, 1992) is presented, called the Trimmed ICP (TrICP). The new algorithm is based on the consistent use of the least trimmed squares (LTS) approach in all phases of the operation. Convergence is proved and an efficient implementation is discussed. TrICP is fast, applicable to overlaps under 50%, robust to erroneous measurements and shape defects, and has easy-to-set parameters. ICP is a special case of TrICP when the overlap parameter is 100%. Results of testing the new algorithm are shown.

关于t，t is related to overlap and outlier ratio，与重叠区域相关以及异常值所占概率。其中，重叠部分需要预先估计。

7、总结


have a good night