python 点云IPC匹配 ndt点云匹配

一家之言，仅作分享，如有不合理或需要改进的地方，欢迎各位讨论。

前言

点云配准的方法有相关性扫描匹配，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的值即可。

二、基本流程

1. 将reference scan在空间上划分成多个网格。
2. 将reference scan中的点云投影在网格上，并计算每个网格的概率密度函数。计算方法为计算该网格内所有点云的均值向量$\vec{\mu}$与协方差矩阵$\sum$,$\vec{\mu}={1\over{m}}\sum_{k=1}^m\vec{y_k}$,$\sum={1\over{m}}\sum_{k=1}^m(\vec{y_k}-\vec{\mu})(\vec{y_k}-\vec{\mu})^T$，其中$\vec{y_k}$表示该网格内的点。则该网格的概率密度函数为$f(\vec{x})={1\over{(2\pi)^{3\over2}\sqrt{|\sum|}}}e^{-{{(\vec{x}-\vec{\mu})^T\sum^{-1}(\vec{x}-\vec{\mu})}\over2}}$
3. 将current scan的点云按照转换矩阵$T$坐标转换到reference scan坐标系下，其中$T$中的位姿变化值$\vec{p}$正是要优化的量。
4. 计算current scan中的每个点转换后距离最近的Cell，并计算得出相应的概率密度函数的值$f(T(\vec{p},\vec{x_i}))$，其中$T(\vec{p},\vec{x_i})$表示current scan中的点$\vec{x_i}$经过位姿变化$\vec{p}$后在reference scan坐标系下的值。
5. 那么最佳的位姿变化值$\vec{p}$应该最大化似然函数：$Likelihood:\psi = \prod_{i=1}^nf(T(\vec{p},\vec{x_i}))$，即最小化负对数似然$-log\psi = -\sum_{k=1}^nlog(f(T(\vec{p},\vec{x_i})))$。
6. 使用牛顿法优化位姿变化参数$\vec{p}$以最小化这个负对数似然。

三、优化建议

NDT的BaseMap中的网格尺寸在计算中会有很大的影响，过大会影响精度，过小会增大计算量。可以通过点云聚类将basemap划分为大小不同的网格，这样每个网格的PDF将 会更加符合实际情况，在关联配准时会更加准确。