NDT算法原理
算法提出原文：The Normal Distributions Transform: A New Approach to Laser Scan Matching(From IEEE)
参考：https://www.cnblogs.com/21207-iHome/p/8039741.html
NDT是基于栅格化地图的算法，主要算法思路：基于2D场景
将参考帧点云地图分为一个个小块(cell)，计算下一时刻的目标帧得到的点云，变换到参考坐标，落到对应参考帧的cell中的概率。就是同一个环境点，不同时刻检测结果，变换到同一坐标，落在同一个cell。通过这种关系得到评分指标，最优化得到R，t。
详细步骤：
- 将参考点云（reference scan）所占的空间划分成指定大小（CellSize）的网格或体素（Voxel）；并计算每个网格的多维正态分布参数：
- 计算网格的概率分布模型：
- 变换要配准的点云（second/Target scan）到参考(reference)坐标系下(参考点云的坐标系，通常是前一帧)。
- 根据正态分布参数计算每个转换点落在对应cell中的概率
- NDT配准得分（score）：计算对应点落在对应网格cell中的概率之和
- 根据牛顿优化算法对目标函数 − s c o r e −score −score进行优化，即寻找变换参数 p p p使得 s c o r e score score的值最大。优化的关键步骤是要计算目标函数的梯度和Hessian矩阵：
令 q = x ′ i − q i q=x′_i−q_i q=x′i−qi，则：
根据链式求导法则以及向量、矩阵求导的公式，s梯度方向为：
其中q对变换参数 p i p_i pi的偏导数 ∂ q ∂ p i \frac{\partial q}{
{\partial p}_i} ∂pi∂q即为变换T的雅克比矩阵：
根据上面梯度的计算结果，继续求 s s s关于变量 p i p_i pi、 p j p_j pj的二阶偏导：
根据变换方程，向量 q q q对变换参数 p p p的二阶导数的向量为：
- 跳转到第3步继续执行，直到达到收敛条件为止
版权声明：本文内容由互联网用户自发贡献，该文观点仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请联系我们举报，一经查实，本站将立刻删除。
发布者：全栈程序员-站长，转载请注明出处：https://javaforall.net/233924.html原文链接：https://javaforall.net
