NDT点云配准算法原理及PCL实现

chennuo0125-HIT

于 2021-01-11 17:20:06 发布

4243

版权

NDT算法因其具有较强的鲁棒性而被广泛的应用，下面这些项目中的点云配准算法采用的都是NDT:
Autoware
hdl_graph_slam
hdl_localization
可见该配准方法的重要性，下面我将对这个算法进行分析．

依据上述伪码流程可以看出，该算法主要的思路就是将目标点云刻画成多个概率分布，然后通过位姿变换关系将待配准点云转换到目标点云坐标系下，计算转换后待配准点云的总概率，并将此概率的负值作为目标函数，通过高斯牛顿迭代法优化该目标函数以求获得负的最小概率值(即最大概率值)．高斯牛顿迭代公式见第23行，由梯度向量、海森矩阵和待求的位姿增量组成，因此最后整个NDT算法可以简单的归为求解梯度向量、求解海森矩阵和求解位姿增量三个主要过程．下面主要分析较复杂的梯度向量和海森矩阵的求解．

目标函数(对应伪码中的 s c o r e score score)

总概率计算如下:
在这里插入图片描述
为了能使用高斯牛顿迭代法，需要将目标函数转换成求最小值，而我们的目标是要求取公式(6.5)的最大值，因此可通过下式进行转换，将求取公式(6.5)的最大值转换成求取公式(6.6)的最小值:

假设每个栅格内点的分布呈正态分布，按公式(6.6)对一个正态分布的PDF求取-log值会出现下图红线所示的没有最大边界问题，该问题会使得噪声对优化结果产生很大影响(如果噪声很大，-log值会非常大，导致优化算法主要的目标是去优化噪声了，结果将会被严重影响)

为了抑制噪声影响，提出了如下正态分布与均匀分布结合的概率分布模型，可以实现图中绿线所示的有边界的效果:
在这里插入图片描述

其中常数 c 1 , c 2 c_{1}, c_{2} c1,c2 可以通过要求 p ˉ ( x ⃗ ) \bar{p}(\vec{x}) pˉ(x ) 的概率质量在一个单元格跨越的空间内等于1来确定。公式(6.7)没有简单的一阶、二阶微分．但通过观察上图发现公式(6.7)可以被下式近似:
在这里插入图片描述

式中的 d 1 , d 2 , d 3 d_{1}, d_{2}, d_{3} d1,d2,d3 可通过令 x = 0 , x = σ , x = ∞ x=0,x=\sigma, x=\infty x=0,x=σ,x=∞建立方程求解，结果为:

该式具有简单的微分方程．基于高斯近似后的概率分布函数如下:
在这里插入图片描述

于是最终的目标函数由(6.6)转换成了下式:
在这里插入图片描述

注意:
虽然目标函数已确定，但目标函数中的协方差矩阵 Σ \Sigma Σ 的确定需要采取一些策略．因为目标函数中需要求取协方差矩阵的逆 Σ − 1 \Sigma ^{-1} Σ−1 ，如果栅格中的点是共面或共线的，协方差矩阵是奇异的，无法求取逆矩阵，因此由三维空间中三个及以下的点来求取协方差矩阵必然是奇异的．为了解决该问题，NDT算法中的PDF计算要求至少需要5个点，并且当 Σ \Sigma Σ 几乎为奇异值时，就对该协方差进行调整，调整的策略为: 如果 Σ \Sigma Σ 最大的特征值 λ 3 \lambda_{3} λ3 比 λ 1 , λ 2 \lambda_{1}, \lambda_{2} λ1,λ2 大100倍时，令 λ 1 = λ 2 = λ 3 / 100 \lambda_{1} = \lambda_{2} = \lambda_{3}/100 λ1=λ2=λ3/100 ，然后再令 Σ = V ∧ ′ V \Sigma = \mathbf{V}\boldsymbol{\wedge}^{'}\mathbf{V} Σ=V∧′V，其中 V \mathbf{V} V 包含了 Σ \Sigma Σ 的特征向量， ∧ ′ \wedge^{'} ∧′ 的计算如下:

至此确定了NDT算法的目标函数．

梯度向量的求解

令
在这里插入图片描述
可得梯度向量:

海森矩阵的求解

从上面的分析可知，NDT算法主要的任务是对梯度向量和海森矩阵的求解，而从公式(6.12)和(6.13)可以看出，针对不同场景，主要的差异体现 δ x ⃗ k ′ δ p i \frac{\delta\vec{\mathbf{x}}_{k}^{'}}{\delta\mathbf{p}_{i}} δpiδx k′和 δ 2 x ⃗ k ′ δ p i δ p j \frac{\delta^{2}\vec{\mathbf{x}}_{k}^{'}}{\delta\mathbf{p}_{i}\delta\mathbf{p}_{j}} δpiδpjδ2x k′．下面以2D场景为例对这两项进行解算．
假设位姿变换参数为 p ⃗ = ( t x , t y , ϕ ) \vec{\mathbf{p}}=(t_{x},t_{y},\phi) p =(tx,ty,ϕ)，待转换的点为 x ⃗ = ( x 1 , x 2 ) \vec{\mathbf{x}} = (x_{1}, x_{2}) x =(x1,x2)，则转换位姿变换方程如下:
在这里插入图片描述
则 δ x ⃗ k ′ δ p i \frac{\delta\vec{\mathbf{x}}_{k}^{'}}{\delta\mathbf{p}_{i}} δpiδx k′结果为:
在这里插入图片描述
δ 2 x ⃗ k ′ δ p i δ p j \frac{\delta^{2}\vec{\mathbf{x}}_{k}^{'}}{\delta\mathbf{p}_{i}\delta\mathbf{p}_{j}} δpiδpjδ2x k′的结果为: