ICP和NDT匹配算法精度、速度和鲁棒性对比

2024-06-05 17:32
文章标签 算法 匹配 速度 精度 对比 icp ndt 鲁棒性

本文主要是介绍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匹配算法精度、速度和鲁棒性对比的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/1033715。

相关文章

不懂推荐算法也能设计推荐系统

不懂推荐算法也能设计推荐系统

本文以商业化应用推荐为例，告诉我们不懂推荐算法的产品，也能从产品侧出发， 设计出一款不错的推荐系统。 相信很多新手产品，看到算法二字，多是懵圈的。 什么排序算法、最短路径等都是相对传统的算法（注：传统是指科班出身的产品都会接触过）。但对于推荐算法，多数产品对着网上搜到的资源，都会无从下手。特别当某些推荐算法 和 “AI”扯上关系后，更是加大了理解的难度。 但，不了解推荐算法，就无法做推荐系
阅读更多...
康拓展开（hash算法中会用到）

康拓展开（hash算法中会用到）

康拓展开是一个全排列到一个自然数的双射（也就是某个全排列与某个自然数一一对应） 公式： X=a[n]*(n-1)!+a[n-1]*(n-2)!+...+a[i]*(i-1)!+...+a[1]*0! 其中,a[i]为整数,并且0<=a[i]<i,1<=i<=n。（a[i]在不同应用中的含义不同）； 典型应用： 计算当前排列在所有由小到大全排列中的顺序，也就是说求当前排列是第
阅读更多...
csu 1446 Problem J Modified LCS （扩展欧几里得算法的简单应用）

csu 1446 Problem J Modified LCS （扩展欧几里得算法的简单应用）

这是一道扩展欧几里得算法的简单应用题，这题是在湖南多校训练赛中队友ac的一道题，在比赛之后请教了队友，然后自己把它a掉 这也是自己独自做扩展欧几里得算法的题目 题意：把题意转变下就变成了：求d1*x - d2*y = f2 - f1的解,很明显用exgcd来解 下面介绍一下exgcd的一些知识点：求ax + by = c的解 一、首先求ax + by = gcd(a,b)的解 这个
阅读更多...
综合安防管理平台LntonAIServer视频监控汇聚抖动检测算法优势

综合安防管理平台LntonAIServer视频监控汇聚抖动检测算法优势

LntonAIServer视频质量诊断功能中的抖动检测是一个专门针对视频稳定性进行分析的功能。抖动通常是指视频帧之间的不必要运动，这种运动可能是由于摄像机的移动、传输中的错误或编解码问题导致的。抖动检测对于确保视频内容的平滑性和观看体验至关重要。 优势 1. 提高图像质量 - 清晰度提升：减少抖动，提高图像的清晰度和细节表现力，使得监控画面更加真实可信。 - 细节增强：在低光条件下，抖
阅读更多...
【数据结构】——原来排序算法搞懂这些就行，轻松拿捏

【数据结构】——原来排序算法搞懂这些就行，轻松拿捏

前言：快速排序的实现最重要的是找基准值，下面让我们来了解如何实现找基准值 基准值的注释：在快排的过程中，每一次我们要取一个元素作为枢纽值，以这个数字来将序列划分为两部分。 在此我们采用三数取中法，也就是取左端、中间、右端三个数，然后进行排序，将中间数作为枢纽值。 快速排序实现主框架： //快速排序 void QuickSort(int* arr, int left, int rig
阅读更多...
【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

✨✨ 欢迎大家来到景天科技苑✨✨ 🎈🎈 养成好习惯，先赞后看哦~🎈🎈 🏆 作者简介：景天科技苑 🏆《头衔》：大厂架构师，华为云开发者社区专家博主，阿里云开发者社区专家博主，CSDN全栈领域优质创作者，掘金优秀博主，51CTO博客专家等。 🏆《博客》：Python全栈，前后端开发，小程序开发，人工智能，js逆向，App逆向，网络系统安全，数据分析，Django，fastapi
阅读更多...
poj 3974 and hdu 3068 最长回文串的O（n）解法(Manacher算法)

poj 3974 and hdu 3068 最长回文串的O（n）解法(Manacher算法)

求一段字符串中的最长回文串。 因为数据量比较大，用原来的O（n^2）会爆。 小白上的O(n^2)解法代码：TLE啦~ #include<stdio.h>#include<string.h>const int Maxn = 1000000;char s[Maxn];int main(){char e[] = {"END"};while(scanf("%s", s) != EO
阅读更多...
秋招最新大模型算法面试，熬夜都要肝完它

秋招最新大模型算法面试，熬夜都要肝完它

💥大家在面试大模型LLM这个板块的时候，不知道面试完会不会复盘、总结，做笔记的习惯，这份大模型算法岗面试八股笔记也帮助不少人拿到过offer ✨对于面试大模型算法工程师会有一定的帮助，都附有完整答案，熬夜也要看完，祝大家一臂之力 这份《大模型算法工程师面试题》已经上传CSDN，还有完整版的大模型 AI 学习资料，朋友们如果需要可以微信扫描下方CSDN官方认证二维码免费领取【保证100%免费
阅读更多...
dp算法练习题【8】

dp算法练习题【8】

不同二叉搜索树 96. 不同的二叉搜索树 给你一个整数 n ，求恰由 n 个节点组成且节点值从 1 到 n 互不相同的 二叉搜索树 有多少种？返回满足题意的二叉搜索树的种数。 示例 1： 输入：n = 3输出：5 示例 2： 输入：n = 1输出：1 class Solution {public int numTrees(int n) {int[] dp = new int
阅读更多...
hdu 3065 AC自动机 匹配串编号以及出现次数

hdu 3065 AC自动机 匹配串编号以及出现次数

题意： 仍旧是天朝语题。 Input 第一行，一个整数N（1<=N<=1000），表示病毒特征码的个数。 接下来N行，每行表示一个病毒特征码，特征码字符串长度在1—50之间，并且只包含“英文大写字符”。任意两个病毒特征码，不会完全相同。 在这之后一行，表示“万恶之源”网站源码，源码字符串长度在2000000之内。字符串中字符都是ASCII码可见字符（不包括回车）。
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2