本文主要是介绍ICP和NDT匹配算法精度、速度和鲁棒性对比，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
注意：如下实验是针对里程计任务而进行的，默认是帧间匹配，而对于帧到局部地图匹配情况下的长时间下的轨迹精度对比，请参考我的这篇博客：
【附优化方法的ICP源码】ICP与NDT匹配算法精度对比，以及手动实现的ICP和基于优化方法的ICP精度对比
1.实验条件
前提：以下ICP与NDT算法均使用的是PCL 1.8库中提供的实现方法，它们可以设置的参数较多，这里只探讨那些最常用的参数。
数据：杭州海创园区的室外点云数据，其效果如下：
后续点云的匹配，均以该点云为基础，对其增加平移、旋转，来模拟各种不同的状态，以观察ICP和NDT的匹配效果。
匹配结果优异的判定条件：
- 对于平移的判定采用欧氏距离来判断，欧氏距离越接近0，表示匹配的结果越优；
- 对于旋转的判定，难以直接采用某个距离范数表示，下文将通过相对旋转矩阵的对角矩阵是否更接近单位阵来做简单判定，或者直接通过变换之后的效果观察判定。
2.实验
实验时，会将两帧关键帧点云分别平移和旋转一定的数值，然后对比icp和ndt对匹配结果的精度(fitness，欧式距离)、用时，以及目测效果。
由于该实验是为了验证室外里程计情况下的匹配结果，所以所有实验均是围绕这个主题进行。里程计前后两帧角度相差较小，位移也较小。实验时以等步长的方式对点云施加平移和旋转。
对于icp和ndt的匹配，均使用一个单位矩阵作为位姿的预测，不给任何先验。
2.1 平移
实际里程计推算情况下，前后两帧点云多数情况下只在x方向产生平移差距，所以后续关于平移的比对只针对x方向。
所用参数：
icp.setMaximumIterations(30);icp.setTransformationEpsilon(1e-3);icp.setMaxCorrespondenceDistance(1.0);ndt.setMaximumIterations(30);ndt.setTransformationEpsilon(1e-3);ndt.setStepSize(0.01);ndt.setResolution(1.0);
第一组实验：两帧点云之间只在x方向差了0.1米
实际效果
匹配效果
以下但凡是两张图片并排排列的情形，左边那张是icp的效果，右边那张是ndt的效果
icp use time(s): 0.072361 distance: 0.0558645 fitness: 0.117922
R
0.999977 0.0067574 -0.000550125
-0.00675799 0.999977 -0.00104073
0.000543081 0.00104442 1
ndt use time(s): 0.571204 distance: 0.0728875 fitness: 0.116304
R
0.999925 0.0119963 -0.00228154
-0.0119934 0.999927 0.00124959
0.00229637 -0.00122213 0.999997
第二组实验：两帧点云之间只在x方向差了0.5米
实际效果
匹配效果：
icp use time(s): 0.199003 distance: 0.386582 fitness: 0.115997
R
0.999945 0.01043 -0.000422132
-0.0104305 0.999945 -0.00107328
0.000410915 0.00107763 1
ndt use time(s): 1.95574 distance: 0.319805 fitness: 0.124826
R
0.999989 0.00409717 -0.00221225
-0.00409767 0.999992 -0.000222853
0.00221132 0.000231915 0.999998
第三组实验：两帧点云之间只在x方向差了1.0米
实际效果
匹配效果：
第四组实验：两帧点云之间只在x方向差了1.5米
icp use time(s): 0.561625 distance: 1.37462 fitness: 0.116247
R
0.999939 0.0110428 -0.000453268
-0.0110427 0.999939 -0.001075
0.000441394 0.00107992 1
ndt use time(s): 1.71849 distance: 0.310204 fitness: 0.375815
R
0.999936 0.011257 -0.000859613
-0.0112575 0.999936 -0.000661424
0.000852113 0.000671059 0.999999
第五组实验：两帧点云之间只在x方向差了2.0米
icp use time(s): 0.797556 distance: 1.88913 fitness: 0.115873
R
0.999937 0.0113971 -0.000474734
-0.0113979 0.999936 -0.00108384
0.000462384 0.00108916 1
ndt use time(s): 1.62229 distance: 0.315326 fitness: 0.573589
R
0.999972 0.00701515 -0.00255213
-0.00701366 0.999975 0.000589553
0.00255621 -0.000571636 0.999997
第六组实验：两帧点云之间只在x方向差了2.5米
icp use time(s): 1.09614 distance: 2.26465 fitness: 0.124819
R
0.999943 0.0106488 -0.000221578
-0.0106502 0.999945 -0.00103709
0.00021052 0.00103936 1
ndt use time(s): 1.61744 distance: 0.315463 fitness: 0.819934
R
0.999987 0.00442289 -0.00238708
-0.00441808 0.999988 0.00201511
0.00239597 -0.00200454 0.999995
第七组实验：两帧点云之间只在x方向差了3.0米
icp use time(s): 0.519895 distance: 0.501095 fitness: 0.978154
R
0.99999 0.00402708 -0.0018392
-0.00401507 0.999969 0.00682577
0.00186659 -0.00681832 0.999976
ndt use time(s): 1.52669 distance: 0.30553 fitness: 1.07398
R
0.999983 0.00564351 -0.00119864
-0.00563951 0.999979 0.00331299
0.00121731 -0.00330617 0.999994
结论：在没有旋转时，对于小范围的平移，在平移距离小于1m时，icp和ndt精度几乎相当，icp的精度略高于ndt，当平移大于1m之后，ndt精度急剧下降，几乎失效（调整参数也无济于事）。当平移大于2.5米时，icp精度开始下降。在时间方面，icp一直优于ndt，比其快2~3倍。所以对于帧间匹配或者回环匹配情形下，应选择icp算法。之所以这里icp的精度优于ndt其中一个重要的原因是，这里没有对点云进行滤波，所以icp展现出了绝对优势，而当采用了体素栅格滤波时，会丢失很多点，所以在一定程度上损失了精度。如果想在回环中进行点云匹配，一个最好的方法是使用icp，而且不要对点云做滤波，这样将会得到一个更优的匹配结果。
在做里程计推算时，为了追求高效，需要对点云进行滤波，一旦滤波，icp的精度就会下降很快，而ndt的精度则s会下降慢一些。基于这个实验，可以做一步大胆猜测，基于更强特征的loam匹配算法，在对点云滤波的情形下，一定在精度和速度上比icp和ndt优秀。这一点与视觉中的里程计却不相同，视觉中当使用直接法时，其速度和精度都优于特征法，但是在激光雷达中使用全数据的icp其速度却低于基于特征的loam，loam利用线和面特征确实很高明。另外可以继续发散这个想法，如果想搭建一个室内的长走廊环境下的里程计，使用icp直接匹配原始点云，是不是精度就会更高呢？但是有一点可以肯定的是，长走廊的特征太少，ndt的高斯假设不成立，而loam的面和线特征几乎都退化为了面特征，精度也会大打折扣。
2.2 旋转
对于旋转，实际上里程计的前后两帧推算时，角度不会变化很大，多数情况下都是yaw角发生变化，所以本实验从1度开始，按照步长为1度增加，对点云施加旋转。
第一组实验：两帧点云之间之间的yaw角差1度
第二组实验：两帧点云之间之间的yaw角差2度
第三组实验：两帧点云之间之间的yaw角差3度
第四组实验：两帧点云之间之间的yaw角差4度
第五组实验：两帧点云之间之间的yaw角差5度
第六组实验：两帧点云之间之间的yaw角差6度
第七组实验：两帧点云之间之间的yaw角差7度
第八组实验：两帧点云之间之间的yaw角差8度
第九组实验：两帧点云之间之间的yaw角差9度
第十组实验：两帧点云之间之间的yaw角差13度
第十一组实验：两帧点云之间之间的yaw角差14度
结论：通过上面的实验发现，在角度变化时，ndt的鲁棒性明显优于icp。当角度大于2度时，icp就已经开始在旋转上体现出比较明显的误差了。但是对与点云的旋转达到12度时，ndt才开始显现比较明显的旋转误差。
从上面可以得到一个很明显的思路，对于里程计的情形下，可以使用icp来计算平移，然后使用ndt来计算旋转。另外如果使用icp或者ndt做回环检测时，如果整个系统的平移误差较小，但是旋转误差较大，那么应该首选ndt做匹配，如果旋转误差较小，但是平移误差较大，应该首选icp做匹配。另外如果旋转和平移都比较大时，应该先选用icp做匹配，获得一个点云的平移预测，然后将这个平移预测载入到ndt算法中，进行进一步的匹配，这样在一定程度上可以提高回环匹配的成功率，而不必盲目的采用分支定界等耗时的匹配策略。最好的方法还是重写pcl库的icp和ndt，在实现时将二者融合成一套，而不必分开调用。
代码如下：
#include <eigen3/Eigen/Dense>
#include <pcl-1.8/pcl/point_types.h>
#include <pcl-1.8/pcl/point_cloud.h>
#include <pcl-1.8/pcl/registration/icp.h>
#include <pcl-1.8/pcl/registration/ndt.h>
#include <pcl-1.8/pcl/visualization/pcl_visualizer.h>
#include <pcl-1.8/pcl/io/pcd_io.h>#include <iostream>
#include <chrono>using namespace std;void DisplayPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr &point_cloud_1_ptr) {pcl::visualization::PCLVisualizer viewer("3D viewer");viewer.setBackgroundColor(0.3, 0.3, 0.3);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color_1(point_cloud_1_ptr, 255, 0, 0);viewer.addPointCloud<pcl::PointXYZ>(point_cloud_1_ptr, single_color_1, "point cloud 1");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "point cloud 1");viewer.addCoordinateSystem(5.0);viewer.initCameraParameters();while (!viewer.wasStopped()) {viewer.spin();}
}void DisplayPointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr &point_cloud_1_ptr,const pcl::PointCloud<pcl::PointXYZ>::Ptr &point_cloud_2_ptr) {pcl::visualization::PCLVisualizer viewer("3D viewer");viewer.setBackgroundColor(0.3, 0.3, 0.3);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color_1(point_cloud_1_ptr, 255, 0, 0);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color_2(point_cloud_2_ptr, 0, 255, 0);viewer.addPointCloud<pcl::PointXYZ>(point_cloud_1_ptr, single_color_1, "point cloud 1");viewer.addPointCloud<pcl::PointXYZ>(point_cloud_2_ptr, single_color_2, "point cloud 2");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "point cloud 1");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "point cloud 2");viewer.addCoordinateSystem(5.0);viewer.initCameraParameters();while (!viewer.wasStopped()) {viewer.spin();}
}int main(int argc, char **argv) {const std::string data_path = "../key_frame_100.pcd";const std::string data_path_1 = "../key_frame_101.pcd";pcl::PointCloud<pcl::PointXYZ>::Ptr origin_point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr origin_point_cloud_ptr_1(new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPCDFile(data_path, *origin_point_cloud_ptr);pcl::io::loadPCDFile(data_path_1, *origin_point_cloud_ptr_1);pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_point_cloud_ptr_1(new pcl::PointCloud<pcl::PointXYZ>);Eigen::AngleAxisf angle_axis(M_PI / 180.0 * 2, Eigen::Vector3f::UnitZ());Eigen::Matrix4f T;T.block<3, 3>(0, 0) = angle_axis.matrix();T.block<3, 1>(0, 3) = Eigen::Vector3f(0, 0, 0);pcl::transformPointCloud(*origin_point_cloud_ptr_1, *transformed_point_cloud_ptr_1, T);DisplayPointCloud(origin_point_cloud_ptr, transformed_point_cloud_ptr_1);pcl::PointCloud<pcl::PointXYZ>::Ptr icp_result_point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;icp.setMaximumIterations(30);icp.setTransformationEpsilon(1e-3);icp.setMaxCorrespondenceDistance(1.0);icp.setInputSource(transformed_point_cloud_ptr_1);icp.setInputTarget(origin_point_cloud_ptr);chrono::steady_clock::time_point icp_t1 = chrono::steady_clock::now();icp.align(*icp_result_point_cloud_ptr, Eigen::Matrix4f::Identity());chrono::steady_clock::time_point icp_t2 = chrono::steady_clock::now();chrono::duration<double> icp_time_used = chrono::duration_cast<chrono::duration<double>>(icp_t2 - icp_t1);Eigen::Matrix4f icp_result_T = icp.getFinalTransformation();std::cout << "icp use time(s): " << icp_time_used.count() << " distance: "<< (Eigen::Vector3f(0.0f, 0.0f, 0.0f) - icp_result_T.block<3, 1>(0, 3)).norm()<< " fitness: " << icp.getFitnessScore()<< "\nR \n" << icp_result_T.block<3, 3>(0, 0).transpose()<< std::endl << std::endl;DisplayPointCloud(origin_point_cloud_ptr, icp_result_point_cloud_ptr);pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;ndt.setMaximumIterations(30);ndt.setTransformationEpsilon(1e-3);ndt.setStepSize(0.01);ndt.setResolution(1.0);ndt.setInputSource(transformed_point_cloud_ptr_1);ndt.setInputTarget(origin_point_cloud_ptr);pcl::PointCloud<pcl::PointXYZ>::Ptr ndt_result_point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);chrono::steady_clock::time_point ndt_t1 = chrono::steady_clock::now();ndt.align(*ndt_result_point_cloud_ptr, Eigen::Matrix4f::Identity());chrono::steady_clock::time_point ndt_t2 = chrono::steady_clock::now();chrono::duration<double> ndt_time_used = chrono::duration_cast<chrono::duration<double>>(ndt_t2 - ndt_t1);Eigen::Matrix4f ndt_result_T = ndt.getFinalTransformation();std::cout << "ndt use time(s): " << ndt_time_used.count() << " distance: "<< (Eigen::Vector3f(0.0f, 0.0f, 0.0f) - ndt_result_T.block<3, 1>(0, 3)).norm()<< " fitness: " << ndt.getFitnessScore()<< "\nR \n" << ndt_result_T.block<3, 3>(0, 0).transpose()<< std::endl;DisplayPointCloud(origin_point_cloud_ptr, ndt_result_point_cloud_ptr);return 0;
}
///TODO: 对比LOAM特征匹配的精度和速度！
这篇关于ICP和NDT匹配算法精度、速度和鲁棒性对比的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
