Generalized-ICP

文章目录

Generalized-ICP
- Abstract
- Introduction
- ScanMatching
- - Generalized ICP
  - Generalized ICP的应用：plane-to-plane ICP
- Results

Abstract

这篇文章的提供一种新方法，可以将point-to-point icp 与point-to-plane icp整合起来。新方法对于错误匹配更加鲁棒，使得maximum match distance参数(详见下文)的调节更加容易。除了性能上的提升以外，新方法允许更加expressive的 probabilistic model 整合进ICP框架里来。

Introduction

ScanMatching

整理了ICP的各种变体，回顾了经典ICP方法。

强调了 max_correspondence参数的设置，该参数是为了防止两片点云之间不存在overlap，所以要限制查找最近点的距离。在使用时，如果设置的太小，算法会变得short sighted，容易导致bad convergence。设的太大，则容易出现错误匹配，影响算法结果。

Generalized ICP

本文方法的核心思想是如何从概率的角度去看待和推导出ICP算法的目标函数。

假设有两个匹配好的点集，A={ai}i=1,2...N,B={bi}i=1,2...N,且ai和bi是对应点A=\{a_i\}_{i=1,2...N},B=\{b_i\}_{i=1,2...N},且a_i和b_i是对应点A={ai}i=1,2...N,B={bi}i=1,2...N,且ai和bi是对应点（A为source，B为target）

再假设两个点云中的每个点，都是服从高斯分布的，即：
ai∼N(ai^,CiA)bi∼N(bi^,CiB)a_i\sim N(\hat{a_i},C_i^{A})\\ b_i\sim N(\hat{b_i},C_i^{B})\\ ai∼N(ai^,CiA)bi∼N(bi^,CiB)
（个人理解，由于测量等环节的误差，每个点的位置的测量值实际上是和真值（ai^,bi^即是真值\hat{a_i},\hat{b_i}即是真值ai^,bi^即是真值）有偏差的，我们可以合理假设他们的分布都是高斯分布）

对于ai^,bi^\hat{a_i},\hat{b_i}ai^,bi^有：
b^i=T∗a^\hat{b}_i=T^*\hat{a} b^i=T∗a^
T∗T^*T∗是理想中的correct rigid transform。

定义残差di(T)=bi−Taid_i^{(T)}=b_i-Ta_idi(T)=bi−Tai

因为ai,bia_i,b_iai,bi都被我们假设为独立的、服从高斯分布的随机变量，所以有：
diT∗∼N(0,CiB+(T∗)CiA(T∗)T)d_i^{T*}\sim N(0,C_i^{B}+(T^*)C_i^{A}(T^*)^T) diT∗∼N(0,CiB+(T∗)CiA(T∗)T)
接下来就是这篇文章的重点，TTT可以被看作diTd_i^TdiT的概率分布中待估计的分布参数，借助最大似然估计（MLE）的思想，有：
T=arg⁡max⁡T∏ip(diT)T=\mathop{\arg\max}_T\prod_ip(d_i^{T}) T=argmaxTi∏p(diT)
上式可以化简为：
T=arg⁡min⁡T∑idi(T)T(CiB+TCiATT)−1di(T)T=\mathop{\arg\min}_T\sum_id_i^{(T)^{T}} (C_i^B+TC_i^AT^T)^{-1}d_i^{(T)} T=argminTi∑di(T)T(CiB+TCiATT)−1di(T)
这里有点疑问，我们稍微推导下：
多元正态分布(KKK维随机变量)的概率密度函数：
fμ,Σ(x)=1(2π)K2⋅1∣Σ∣12⋅e−12(x−μ)TΣ−1(x−μ)f_{\mu, \Sigma}(x)=\frac{1}{(2 \pi)^{\frac{K}{2}}} \cdot \frac{1}{|\Sigma|^{\frac{1}{2}}} \cdot e^{-\frac{1}{2}(x-\mu)^{T} \Sigma^{-1}(x-\mu)} fμ,Σ(x)=(2π)2K1⋅∣Σ∣211⋅e−21(x−μ)TΣ−1(x−μ)
对于NNN个样本点x1,x2...xnx_1,x_2...x_nx1,x2...xn，其似然函数：
L(μ,Σ)=fμ,Σ(x1)fμ,Σ(x2)…fμ,Σ(xN)=(2π)−KN2⋅∣Σ∣−N2⋅e−12∑n=1N(xn−μ)TΣ−1(xn−μ)\begin{array}{c} L(\mu, \Sigma)=f_{\mu, \Sigma}\left(x^{1}\right) f_{\mu, \Sigma}\left(x^{2}\right) \ldots f_{\mu, \Sigma}\left(x^{N}\right) \\ =(2 \pi)^{-\frac{K N}{2}} \cdot|\Sigma|^{-\frac{N}{2}} \cdot e^{-\frac{1}{2} \sum_{n=1}^{N}\left(x^{n}-\mu\right)^{T} \Sigma^{-1}\left(x^{n}-\mu\right)} \end{array} L(μ,Σ)=fμ,Σ(x1)fμ,Σ(x2)…fμ,Σ(xN)=(2π)−2KN⋅∣Σ∣−2N⋅e−21∑n=1N(xn−μ)TΣ−1(xn−μ)
取对数：
ln⁡L(μ,Σ)=−KN2ln⁡(2π)−N2ln⁡∣Σ∣−12∑n=1N(xn−μ)TΣ−1(xn−μ)=C−N2ln⁡∣Σ∣−12∑n=1N(xn−μ)TΣ−1(xn−μ)\begin{aligned} \ln L(\mu, \Sigma)=-\frac{K N}{2} \ln (2 \pi)-\frac{N}{2} \ln |\Sigma|-\frac{1}{2} \sum_{n=1}^{N}\left(x^{n}-\mu\right)^{T} \Sigma^{-1}\left(x^{n}-\mu\right) \\ &=C-\frac{N}{2} \ln |\Sigma|-\frac{1}{2} \sum_{n=1}^{N}\left(x^{n}-\mu\right)^{T} \Sigma^{-1}\left(x^{n}-\mu\right) \end{aligned} lnL(μ,Σ)=−2KNln(2π)−2Nln∣Σ∣−21n=1∑N(xn−μ)TΣ−1(xn−μ)=C−2Nln∣Σ∣−21n=1∑N(xn−μ)TΣ−1(xn−μ)

当正态分布的协方差矩阵的行列式为常数时，只需要优化最后一项就可以了。最后一项的二次型又被称作马哈拉诺比斯距离（马氏距离），极大似然估计等价于最小化样本点与均值之间的马氏距离。更详细的内容可以参考高翔《视觉SLAM14讲》6.1 状态估计问题。

但是本文中协方差矩阵为Σ=CiB+(T∗)CiA(T∗)T\Sigma=C_i^{B}+(T^*)C_i^{A}(T^*)^TΣ=CiB+(T∗)CiA(T∗)T,尽管TTT是三维刚体变换矩阵，其行列式为1。但无法说明∣Σ∣|\Sigma|∣Σ∣是常数，所以不是太明白为什么作者把这项直接忽略了,有可能是这项相当于一个TTT的平方项,对于最小值影响不大。？？？？？

以上就是Generalized ICP的核心内容。有了上式，point-to-point ICP可以看成其special case,令CiB=I,CiA=0即可C_i^B=I,C_i^A=0即可CiB=I,CiA=0即可。point-to-plane icp也可以被纳入此框架下(这里涉及正交投影矩阵的知识正交投影阵)。

Generalized ICP的应用：plane-to-plane ICP

这一节要借助上文的结论，推导出plane-to-plane ICP

前提假设：

我们平常处理的点云有什么特点？它实际上是3维空间中surface的采样点集合。这意味着我们处理的是3维空间中的2维流形。现实世界中的surface，都是局部可微的，因此我们可以假设点云的局部是一个平面。
两个视角获取的点云，相当于从两个视角对2维流形进行了采样，在两个视角采样到同一个点的概率很小，也就是说correspondance will not be exact. 因此，每一个采样点实际上只提供了一个约束，那就是法向量的方向。

基于以上两点，我们可以假设点PPP处的协方差矩阵为以下形式：在沿着基向量e1e_1e1的方向(local plane的法向量方向)方差较小，而在另外两个正交方向的方差较大。
(ϵ00010001)\left(\begin{array}{lll} \epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right) ⎝⎛ϵ00010001⎠⎞
设点bi,aib_i,a_ibi,ai处的法向量分别为μi和 νi\mu_{i} \text { 和 } \nu_{i}μi 和 νi，则协方差矩阵为：
CiB=Rμi⋅(ϵ00010001)⋅RμiTCiA=Rνi⋅(ϵ00010001)⋅RνiT\begin{aligned} C_{i}^{B} &=\mathbf{R}_{\mu_{i}} \cdot\left(\begin{array}{ccc} \epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right) \cdot \mathbf{R}_{\mu_{i}}^{T} \\ C_{i}^{A} &=\mathbf{R}_{\nu_{i}} \cdot\left(\begin{array}{ccc} \epsilon & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right) \cdot \mathbf{R}_{\nu_{i}}^{T} \end{aligned} CiBCiA=Rμi⋅⎝⎛ϵ00010001⎠⎞⋅RμiT=Rνi⋅⎝⎛ϵ00010001⎠⎞⋅RνiT

其中RxR_xRx为旋转矩阵，可以将e1e_1e1转换到与xxx 一致的方向。

个人理解，协方差矩阵实际上起到了为cost function TTT的每一项的不同维度加权的作用。效果是：

在优化过程中，沿着法向量方向的约束较紧，沿着其他两个方向的约束较松
如果两个法向量方向不一致的点，却因为距离较近而错误地匹配在了一起，那么以上协方差矩阵的设计，将使得沿着法向量方向的约束被削弱

图示如下：

Results

作者对三个算法进行了比较。

standard icp: 尽管有封闭解，但为了可比较性，还是使用了共轭梯度法做优化。最大迭代次数250
point to plane icp：最大迭代次数50
plane to plane icp：最大迭代次数50

测试方法：

在两个已知位姿关系的点云测试算法的收敛性。

对simulated data 和real data分别进行了测试：

simulated data 是使用SICK公司的scanner获取的，分别扫描了indoor和outdoor的场景。然后模拟了 scanner 在不同位置产生的点云，并加入Gauss噪声。
real data 是车载激光雷达采集的。ground truth由SLAM 技术给出。

以上三个数据集：simulated data indoor, simulated data outdoor, real data ，每个都采集了多组scan pairs

初始位姿是给每组scan pair的ground truth 上加上一个均匀分布的噪声（±1.5m，±15度）产生的（每个scan pair 会测十个初始位姿）。针对不同的数据集，算法的performance以scan pairs的平移误差的均值来表征。不考虑旋转误差。

max_correspondence参数对配准结果很重要，作者测量了该参数对误差的影响：

从图中可以看出：

算法的平均error： standard icp > point to plane icp > generalized icp
generalized icp 对max correspondence 参数的设置最不敏感，即使设的过大，其performance的下降也比其他两种方法要小;但是仍然可以看到，在simulated data 的表现比real data要好，这是因为real data中会有更多高频的细节信息，存在更多generalized icp处理不了的情况：两个点是错误匹配，但是法向量方向却是一致的。

[论文阅读][ICP变种] Generalized-ICP相关推荐

点云配准论文阅读笔记--Comparing ICP variants on real-world data sets
目录写在前面点云配准系列摘要 1引言(Introduction) 2 相关研究(Related work) 3方法( Method) 3.1输入数据的敏感性 3.2评价指标 3.3协议 4 模块 ...
Learning Multiview 3D point Cloud Registration论文阅读笔记
Learning multiview 3D point cloud registration Abstract 提出了一种全新的,端到端的,可学习的多视角三维点云配准算法. 多视角配准往往需要两个阶段 ...
rgb fusion检测不到显卡_【论文阅读27】Co-Fusion
主要内容物体级别的语义SLAM.维护一个背景模型和多物体模型,每个模型由面元地图表示. 基于运动分割和语义信息检测运动物体. 使用基于ICP对齐的几何误差和基于颜色差异的光度误差跟踪背景模型(相机位 ...
DCP（Deep Closest Point）论文阅读笔记以及详析
DCP论文阅读笔记前言本文中图片仓库位于github,所以如果阅读的时候发现图片加载困难.建议挂个梯子. 作者博客:https://codefmeister.github.io/ 转载前请联系作者 ...
论文阅读：ThinLTO: Scalable and Incremental LTO
ThinLTO: Scalable and Incremental LTO 论文阅读笔记 A little bit of history. SYZYGY – A Framework for Scala ...
【论文阅读】7-Discovering Structural Regularity in 3D Geometry
[论文阅读]7-Discovering Structural Regularity in 3D Geometry 1.Input: 2.Processing pipeline 2.1.Transfor ...
深度学习论文阅读目标检测篇（三）：Faster R-CNN《 Towards Real-Time Object Detection with Region Proposal Networks》
深度学习论文阅读目标检测篇(三):Faster R-CNN< Towards Real-Time Object Detection with Region Proposal Networks&g ...
【论文阅读】RoadMap: A Light-Weight Semantic Map for Visual Localization towards Autonomous Driving
文章:RoadMap: A Light-Weight Semantic Map for Visual Localization towards Autonomous Driving 作者:Tong Q ...
论文阅读：SuMa++
论文阅读:SuMa++ 论文:<SuMa++: Efficient LiDAR-based Semantic SLAM> SuMa++是建立在SuMa基础上的方法,该方法使用了基于Ran ...

[论文阅读][ICP变种] Generalized-ICP