一家之言,仅作分享,如有不合理或需要改进的地方,欢迎各位讨论。
前言
点云配准的方法有相关性扫描匹配,ICP,以及NDT。前两种在我之前的博客中已经有了简单介绍,
相关性扫描匹配CSM与分支限界
多种形式ICP问题的ceres实例应用
这里将对NDT(Normal Distribution Transform)正态分布变化进行介绍。相比于ICP,NDT对比较差的初始值也可以完成更好的估计。
一、与ICP、CSM的区别
不论是ICP还是CSM,要想有更好的配准效果,就需要假设环境的大部分是不变的。但是实际中,无论是行驶的车辆还是行人,都会带来小部分的环境变化,为了解决这种细微的差异,NDT会有更好的效果。
NDT的这种transform的核心思想是将点云映射到平滑表面来表示,使用一组局部概率密度函数(PDF)来描述,每个PDF描述表面的一部分的形状。NDT的代价函数是指点云在有着PDF的BaseMap下的概率之和要尽可能的大。
NDT不需要消耗大量代价计算最近邻搜索匹配点,它只需要计算点在最近的cell中PDF的值即可。
二、基本流程
- 将reference scan在空间上划分成多个网格。
- 将reference scan中的点云投影在网格上,并计算每个网格的概率密度函数。计算方法为计算该网格内所有点云的均值向量 μ ⃗ \vec{\mu} μ与协方差矩阵 ∑ \sum ∑, μ ⃗ = 1 m ∑ k = 1 m y k ⃗ \vec{\mu}={1\over{m}}\sum_{k=1}^m\vec{y_k} μ=m1k=1∑myk, ∑ = 1 m ∑ k = 1 m ( y k ⃗ − μ ⃗ ) ( y k ⃗ − μ ⃗ ) T \sum={1\over{m}}\sum_{k=1}^m(\vec{y_k}-\vec{\mu})(\vec{y_k}-\vec{\mu})^T ∑=m1k=1∑m(yk−μ)(yk−μ)T,其中 y k ⃗ \vec{y_k} yk表示该网格内的点。则该网格的概率密度函数为 f ( x ⃗ ) = 1 ( 2 π ) 3 2 ∣ ∑ ∣ e − ( x ⃗ − μ ⃗ ) T ∑ − 1 ( x ⃗ − μ ⃗ ) 2 f(\vec{x})={1\over{(2\pi)^{3\over2}\sqrt{|\sum|}}}e^{-{{(\vec{x}-\vec{\mu})^T\sum^{-1}(\vec{x}-\vec{\mu})}\over2}} f(x)=(2π)23∣∑∣1e−2(x−μ)T∑−1(x−μ)
- 将current scan的点云按照转换矩阵 T T T坐标转换到reference scan坐标系下,其中 T T T中的位姿变化值 p ⃗ \vec{p} p正是要优化的量。
- 计算current scan中的每个点转换后距离最近的Cell,并计算得出相应的概率密度函数的值 f ( T ( p ⃗ , x i ⃗ ) ) f(T(\vec{p},\vec{x_i})) f(T(p,xi)),其中 T ( p ⃗ , x i ⃗ ) T(\vec{p},\vec{x_i}) T(p,xi)表示current scan中的点 x i ⃗ \vec{x_i} xi经过位姿变化 p ⃗ \vec{p} p后在reference scan坐标系下的值。
- 那么最佳的位姿变化值 p ⃗ \vec{p} p应该最大化似然函数: L i k e l i h o o d : ψ = ∏ i = 1 n f ( T ( p ⃗ , x i ⃗ ) ) Likelihood:\psi = \prod_{i=1}^nf(T(\vec{p},\vec{x_i})) Likelihood:ψ=∏i=1nf(T(p,xi)),即最小化负对数似然 − l o g ψ = − ∑ k = 1 n l o g ( f ( T ( p ⃗ , x i ⃗ ) ) ) -log\psi = -\sum_{k=1}^nlog(f(T(\vec{p},\vec{x_i}))) −logψ=−∑k=1nlog(f(T(p,xi)))。
- 使用牛顿法优化位姿变化参数 p ⃗ \vec{p} p以最小化这个负对数似然。
三、优化建议
NDT的BaseMap中的网格尺寸在计算中会有很大的影响,过大会影响精度,过小会增大计算量。可以通过点云聚类将basemap划分为大小不同的网格,这样每个网格的PDF将 会更加符合实际情况,在关联配准时会更加准确。