NDT算法详解与C++实现-编程知识

NDT算法详解与C++实现

news/2024/11/15 6:03:23/文章来源:https://www.cnblogs.com/Gaowaly/p/18355619

　　点云匹配在感知环节是一个很重要的信息获取手段，而其中的算法也有几个比较经典了，例如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）进入时，根据正态分布参数计算每个转换点在对应网格中的概率，累计所有网格的概率得到

　　还有一个关键点在于如何使概率

论文学习—— 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)
{//initialcurrent_gps_rtk_pose_.x = gps_rtk.x;current_gps_rtk_pose_.y = gps_rtk.y;current_gps_rtk_pose_.z = gps_rtk.zcurrent_gps_rtk_pose_.roll = gps_rtk.roll;current_gps_rtk_pose_.pitch = gps_rtk.pitch;current_gps_rtk_pose_.yaw = gps_rtk.yaw;//updateprevious_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;
//其余后置部分也做赘述