NDT算法详解与C++实现

　　点云匹配在感知环节是一个很重要的信息获取手段，而其中的算法也有几个比较经典了，例如ICP（Iterative Closest Point，迭代最近点）算法，而本文决定记录学习的是NDT算法，也就是Normal Distribution Transform，正态分布变换算法。什么是正态分布变换算法呢，简言之，就是把空间中的点云进行整理，整理成具有分布特性的块，然后根据这些块来对不同帧的点云做匹配，求解这些分布块的位姿变换。

但实际上，还是要好好学习其原理以及实现才行。

0.本文大纲

以下给出本文大纲，一共分为学习部分以及总结部分：

NDT算法的基本原理学习
NDT算法C++代码实现
总结

1.NDT算法的基本原理学习

　　首先需要明确一点，任何算法都是一系列逻辑和数值运算组成的针对某一问题的解决方案。NDT也不例外，因此其算法原理就要从提出这一系列运算的作者文章和原始代码中去学习了。而此处使用的是IEEE的论文：《The Normal Distributions Transform: A New Approach to Laser Scan Matching》-Peter Biber.

1.1 NDT算法核心思想与论文学习

　　正态分布变换（NDT）算法是一个匹配算法，可以较好的获取前后两个目标之间的姿态变动关系以及匹配度等信息，故常用于匹配定位，地图构建等，而NDT最经典的应用就是激光点云的匹配，获取位姿变换，也就是旋转平移变化参数 [,] 。

核心思想

　　其核心思想在于把目标点云（Target）分成若干个小立方块（常称作“网格”，论文或代码常用“cell”表示），根据设置好的参数，求解每个网格的多维正态分布并计算其概率分布模型，当处在同坐标下的源点云（Source）进入时，根据正态分布参数计算每个转换点在对应网格中的概率，累计所有网格的概率得到，当这个概率达到最大时，则找到了最优的匹配关系。

　　还有一个关键点在于如何使概率达到最大，这里就要提出后端优化的方法，其核心在于根据选用的方法来计算目标函数的下降梯度或雅克比（Jacobian）矩阵或海塞（Hessian）矩阵达到最优，可以参考之前的文章：状态估计之非线性优化的学习

论文学习—— Abstract-摘要

第一部分：摘要（重点）表示：^[1]文章提出了一种新的激光匹配方式，而这种匹配方式可以较好的应用在SLAM方案中，实时建图且不依赖于里程计数据；

第二部分：随后在介绍和先前工作中介绍了SLAM现有成果，以及NDT算法在SLAM方案中的效果和优势，包括对比了ICP匹配方法以及一些复杂的工况讨论；

第三部分：NDT算法流程（与前述核心思想对照）

主要包括点云网格化，求均值，协方差矩阵，计算正态分布参数等；

第四部分：扫描匹配流程（前提是在同坐标系，或应用起来认为是相同一个传感器的相邻数据帧）

　　主要包括：

　　在第一帧Target上建立NDT关系（获取到计算参数），初始化匹配参数（这一步可以用0或者里程计数据给出，后续会详细分析区别），

　　在第二帧Source映射到第一帧数据的坐标系中，计算每个点的正态分布情况，获得各网格的概率得分及总分，通过后端优化来获取一个最好的参数估计值，持续计算每个点的正态分布情况直到收敛；

第五部分：后端优化方法，文中选取的是牛顿法，求解 Δ=− 即可，这一部分对应了前述的第二个关键点，即非线性状态估计优化；

第六部分：位姿跟踪与地图构建，这一部分主要描述了NDT定位的以及建图的一些效果。

建图效果

论文整体理解难度不高，可以结合核心思想学习，此处不再赘述。

1.2 NDT算法详细流程分解与公式学习

　　第二小节的主要目标是把NDT算法中每一个流程出现的步骤再细化，出现公式的步骤学习清楚。

第一个流程：求解NDT（注意理解什么是NDT）。步骤如下：

　　①将空间划分为一个个网格，网格大小可以自拟，文中用的1m × 1m的2D网格，这个网格大小可以根据环境来确认，例如激光扫描200m范围时，可以取到更大的网格；遍历这些网格，保留至少包含3个点的网格（这个单元格中的最小点数为3是确保正态分布可计算，实际中可以取大于3的值，例如在某些室外环境中可以取到十几到几十上百个点）；

　　实际计算过程中，为了避免离散化影响，可以适当采取离散点去除算法或增加网格属性，例如文中采取了4重重叠的网格；另外在计算协方差矩阵时，为了防止奇异值出现，应该在求解协方差矩阵后，出现数值过小的应当被手动设置为系统最小，从而完成后续计算。

2.NDT算法流程与C++代码实现

NDT算法可以直接根据原理写C++代码，也可以调用现成的库来实现，此处就给一个简单的基于PCL库的NDT实现方法。

2.1 基于PCL的NDT算法C++代码实现（部分）

//其他的前置部分略去，如果写成函数，函数输入输出应为原始点云和点云地图

//此处的调用写法基本思想是写为类(Class)

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

//1.基本变量定义等

int max_iter_ = 35;//迭代最大次数

double step_size_ = 0.2;//线搜索的最大步长，理论上这个值是网格分辨率的一半，但也不能过小，当匹配效果不好时，应该放大这个参数，分辨率同理

double ndt_res_ = 2.0;//NDT网格分辨率

double trans_eps_ = 0.001;//终止条件

double voxel_leaf_size_ = 0.5;//体素滤波size，经验判断其值因该是点云跨度最大除以30

double min_scan_range_ = 2.0;//最小过滤范围

double max_scan_range_ = 200.0;//最大过滤范围

//map_是先前已有的点云，注意这个若是第一帧则需要初始化

pcl::PointCloud<pcl::PointXYZI>::Ptr map_(new pcl::PointCloud<pcl::PointXYZI>);

//scan_ptr是经过预处理后的原始点云，例如过近过远的点需要去除

pcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());

//filtered_scan_ptr是经过降采样后的点云

pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());

//NDT转换完的点云

pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());

//增广后的旋转平移矩阵

Eigen::Matrix4f t_localizer(Eigen::Matrix4f::Identity());

Eigen::Matrix4f t_base_link(Eigen::Matrix4f::Identity());

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

//2.部分模块初始化

//地图初始化

if (initial_scan_loaded == 0)

{

  pcl::transformPointCloud(*scan_ptr, *transformed_scan_ptr, tf_btol_);//tf_btol_此时是一个空阵，即不存在旋转关系

  *map_ += *transformed_scan_ptr;

  initial_scan_loaded = 1;

}

pcl::PointCloud<pcl::PointXYZI>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZI>(*map_)); //存放第一帧点云，随后放置先前的所有地图

if (is_first_map_ == true)

{

  ndt.setInputTarget(map_ptr);//此处是调用PCL库。如pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> ndt;

  is_first_map_ = false;

  pcl::copyPointCloud(*map_ptr, *final_map);

}

//....

//GPS关系初始化

if (initial_GPS_loaded == 0)

{

  //initial

  current_gps_rtk_pose_.x = gps_rtk.x;

  current_gps_rtk_pose_.y = gps_rtk.y;

  current_gps_rtk_pose_.z = gps_rtk.z

  current_gps_rtk_pose_.roll = gps_rtk.roll;

  current_gps_rtk_pose_.pitch = gps_rtk.pitch;

  current_gps_rtk_pose_.yaw = gps_rtk.yaw;

  //update

  previous_gps_rtk_pose_.x = current_gps_rtk_pose_.x;

  previous_gps_rtk_pose_.y = current_gps_rtk_pose_.y;

  previous_gps_rtk_pose_.z = current_gps_rtk_pose_.z;

  previous_gps_rtk_pose_.roll = current_gps_rtk_pose_.roll;

  previous_gps_rtk_pose_.pitch = current_gps_rtk_pose_.pitch;

  previous_gps_rtk_pose_.yaw = current_gps_rtk_pose_.yaw;

  initial_scan_loaded = 1;

}

//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

//3.NDT部分

//①获取预处理后的点云，此处用半径方式对点云过滤

for (unsigned int i = 0; i < input_cloud->size(); i++)

{

  r = pow(pow(input_cloud->points[i].x, 2.0) + pow(input_cloud->points[i].y, 2.0) + pow(input_cloud->points[i].z, 2.0), 0.5);

  if (min_scan_range_ < r && r < max_scan_range_)

  {

    scan_ptr->push_back(input_cloud->points[i]);

  }

}

//②降维处理

pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter_;

voxel_grid_filter_.setInputCloud(scan_ptr);

voxel_grid_filter_.filter(*filtered_scan_ptr);

//③求解与转换

Eigen::Translation3f init_translation(current_pose_.x, current_pose_.y, current_pose_.z);//初始化第一个位姿

Eigen::AngleAxisf init_rotation_x(current_pose_.roll, Eigen::Vector3f::UnitX());

Eigen::AngleAxisf init_rotation_y(current_pose_.pitch, Eigen::Vector3f::UnitY());

Eigen::AngleAxisf init_rotation_z(current_pose_.yaw, Eigen::Vector3f::UnitZ());

Eigen::Matrix4f init_guess =

      (init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix() * tf_btol_;

pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZI>);

ndt.align(*output_cloud, init_guess);

t_localizer = ndt.getFinalTransformation();

t_base_link = t_localizer * tf_ltob_;

pcl::transformPointCloud(*scan_ptr, *transformed_scan_ptr, t_localizer);

//④更新

Eigen::Matrix3d mat_b;

mat_b << static_cast<double>(t_base_link(0, 0)), static_cast<double>(t_base_link(0, 1)), static_cast<double>(t_base_link(0, 2)),

      static_cast<double>(t_base_link(1, 0)), static_cast<double>(t_base_link(1, 1)), static_cast<double>(t_base_link(1, 2)),

      static_cast<double>(t_base_link(2, 0)), static_cast<double>(t_base_link(2, 1)), static_cast<double>(t_base_link(2, 2));

// 更新当前位姿的XYZ

current_pose_.x = t_base_link(0, 3);

current_pose_.y = t_base_link(1, 3);

current_pose_.z = t_base_link(2, 3);

// 更新当前的朝向，使用欧拉角来表示

Eigen::Vector3d euler_angles = mat_b.eulerAngles(0, 1, 2);

current_pose_.roll = double(euler_angles(0));

current_pose_.pitch = double(euler_angles(1));

current_pose_.yaw = double(euler_angles(2));

//如果有必要，根据GPS与RTK模块更新的位姿做一次滤波，此处略，有需要请私信

//⑤地图更新与NDT更新

double shift = sqrt(pow(current_pose_.x - previous_pose_.x, 2.0) + pow(current_pose_.y - previous_pose_.y, 2.0));

if (shift >= min_add_scan_shift_)

{

  *map_ += *transformed_scan_ptr;//更新地图

  previous_pose_.x = current_pose_.x;

  previous_pose_.y = current_pose_.y;

  previous_pose_.z = current_pose_.z;

  previous_pose_.roll = current_pose_.roll;

  previous_pose_.pitch = current_pose_.pitch;

  previous_pose_.yaw = current_pose_.yaw;

  std::cout << "map_ptr  is : " << map_ptr->size() << std::endl;

  ndt.setInputTarget(map_);//把先前的地图作为下一次的匹配目标

  pcl::copyPointCloud(*map_, *final_map);//把更新的地图传出去

}

//⑥打印输出校验结果

std::cout << "NDT has converged: " << ndt.hasConverged() << std::endl;

std::cout << "Fitness score: " << ndt.getFitnessScore() << std::endl;

std::cout << "Number of iteration: " << ndt.getFinalNumIteration() << std::endl;

std::cout << "(x,y,z,roll,pitch,yaw):" << std::endl;

std::cout << "(" << current_pose_.x << ", " << current_pose_.y << ", " << current_pose_.z << ", " << current_pose_.roll

            << ", " << current_pose_.pitch << ", " << current_pose_.yaw << ")" << std::endl;

std::cout << "Transformation Matrix:" << std::endl;

std::cout << t_localizer << std::endl;

std::cout << "shift: " << shift << std::endl;

std::cout << "final_map: " << final_map->points.size() << std::endl;

//其余后置部分也做赘述

2.2 NDT算法流程图

　　代码的实现流程符合前述的原理，此处再给出一个流程图，流程图中默认有GPS与RTK数据，实际上也可以只用点云匹配来进行结果计算和输出。

　　纯手画，如果觉得有帮助的老铁还请给一键三连支持一下~（转发）

在GitHub上也有许多开源的解决方案，可以自行搜索学习。

3.结果展示与总结

3.1 结果展示

以下展示的是基于Kitti数据集的结果。数据集编号为2011_09_26_drive_0009_sync，取用了velodyne_points里的数据。

基于Kitti数据的NDT地图结果

3.2 总结

　　前述整理了NDT算法的相关学习内容。

　　第一部分小结：

　　首先是NDT算法的理论，那么从最根本的研读算法论文中我们学习到了：

　　NDT的核心思想在于把点云网格化，计算不同帧间点云网格的正态分布概率的匹配程度，求解匹配度最高时的点云变换关系；

　　其次就是算法论文中的核心步骤：分为两大步，一是求解两帧的NDT，二是用非线性优化求解旋转平移关系。

　　第二部分小结：

　　在第二部分中我们整理了一下相关代码，主要包括以下几个部分：

　　1.点云及旋转平移关系的定义；

　　2.各个模块初始化；

　　3.NDT主要部分：

　　　　①预处理；

　　　　②降维；

　　　　③求解与转换；

　　　　④更新位姿；

　　　　⑤地图更新与NDT目标更新；

　　　　⑥校验结果。

　　绘制了流程图，流程图中有关于GPS与RTK的部分，在实现时可以选择性完成，如果不选择使用，只用点云也可以获得结果，但可能精度有所下降。

参考文献：SLAM算法工程师之路：NDT算法详解与C++实现 - 知乎 (zhihu.com)