SLAM前端之ndt_omp使用

article/2025/4/30 15:34:04

ndt_omp(部分参考https://zhuanlan.zhihu.com/p/48853182)

简介：

koide3/ndt_omp。继承自pcl ndt，并做了多线程等优化。参考：koide3/ndt_omp

环境需求：

1) 需要编译安装pcl-1.8.1或以上版本。因为ndt_omp是继承自pcl ndt的。

使用方法：

1）将整个ndt_omp-master放到工程下的src目录内

2)在CMakeLists.txt中添加语句

打开c++11

add_compile_options(-std=c++11)

添加库

find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs pcl_conversions pcl_ros ndt_omp)

寻找库

find_package(OpenMP)

if (OPENMP_FOUND)

set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")

set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")

endif()

3) 在.cpp文件中需要有支持PCL ndt的头文件包含（同pcl ndt），并需增加以下头文件包含

#include <pclomp/ndt_omp.h>

4) 声明ndt_omp对象，并用它来执行align()，具体可参照官方样例程序align.cpp。

pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());

参数设置：

普通参数配置 resolution epsilon maxIterations stepSize都和pcl ndt相同。

注意：stepSize=0.1不要变它。改成0.4会在直线行驶时加快匹配速度，但在转弯时容易匹配错误。

独特参数配置

sear_methods=pclomp::DIRECT7 // 这个最快最好（其他的参数都是有特殊场景的用途）

setNumTreads(omp_get_max_threads()) // 使用能使用到的最多的线程（线程越多越快

前端测试，代码作用：保存history_points_num帧数据，两帧之间的间隔大于history_points_dis

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/registration/ndt.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <pclomp/ndt_omp.h>/*******************测试用程序**************************/
ros::Publisher test_pub;
sensor_msgs::PointCloud2 test_points;
/*****************************************************///实现多帧激光雷达之间的合并，做一个滑窗
class MatchPoint
{public:Eigen::Matrix4f transMatrix = Eigen::Matrix4f::Identity(4,4);//存储变换矩阵pcl::PointCloud<pcl::PointXYZI>::Ptr cloud{new pcl::PointCloud<pcl::PointXYZI>};
};
std::vector<MatchPoint> multiPoint; //用来存储10帧点云数据及变换矩阵
pcl::PointCloud<pcl::PointXYZI>::Ptr localMapCloud{new pcl::PointCloud<pcl::PointXYZI>};
ros::Publisher local_map_pub;
sensor_msgs::PointCloud2 localMapCloudPoints;static float voxel_size = 0.2 ;
static float ndt_epsilon = 0.01 ;
static float ndt_step_size = 0.1 ;
static float ndt_resolution = 1.0;
static float ndt_iterations = 100 ;
static float ndt_num_threads = 6;
static int history_points_num = 20;
static float history_points_dis = 0.4;//两帧数数据之间的最小距离void lidar_subCallback(const sensor_msgs::PointCloud2ConstPtr& input_pointcloud2)
{//接收点云pcl::PointCloud<pcl::PointXYZI>::Ptr incloud(new pcl::PointCloud<pcl::PointXYZI>);pcl::fromROSMsg (*input_pointcloud2, *incloud);std::vector<int> indices;pcl::removeNaNFromPointCloud(*incloud, *incloud, indices);//降采pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZI>);pcl::VoxelGrid<pcl::PointXYZI> voxel_filter;voxel_filter.setLeafSize(voxel_size, voxel_size, voxel_size);voxel_filter.setInputCloud(incloud);voxel_filter.filter(*filtered_cloud);MatchPoint matchpoint;matchpoint.cloud->points.clear();matchpoint.cloud->points.assign(incloud->points.begin(),incloud->points.end());//录入第一帧数据  if(multiPoint.size() == 0){std::cout<<multiPoint.size()<<std::endl;multiPoint.push_back(matchpoint);localMapCloud->points.clear();pcl::toROSMsg(*localMapCloud, localMapCloudPoints);localMapCloudPoints.header.frame_id = "velodyne";local_map_pub.publish(localMapCloudPoints);localMapCloud->points.clear();return;};pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_source_cloud(new pcl::PointCloud<pcl::PointXYZI>);pcl::VoxelGrid<pcl::PointXYZI> voxel_filter_source_cloud;voxel_filter_source_cloud.setLeafSize(voxel_size, voxel_size, voxel_size);voxel_filter_source_cloud.setInputCloud(multiPoint[multiPoint.size()-1].cloud);voxel_filter_source_cloud.filter (*filtered_source_cloud);std::cout<<multiPoint.size()<<std::endl;/***************************ndt_omp**************************/pclomp::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> ndt_omp;ndt_omp.setResolution(ndt_resolution);ndt_omp.setNumThreads(ndt_num_threads);ndt_omp.setNeighborhoodSearchMethod(pclomp::KDTREE);//pclomp::KDTREE,pclomp::DIRECT1,pclomp::DIRECT7ndt_omp.setTransformationEpsilon (ndt_epsilon);ndt_omp.setStepSize(ndt_step_size);ndt_omp.setMaximumIterations(ndt_iterations);ndt_omp.setInputSource(filtered_source_cloud);ndt_omp.setInputTarget(filtered_cloud);/*pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> ndt;ndt.setTransformationEpsilon (ndt_epsilon);//为More-Thuente线搜索设置最大步长ndt.setStepSize(ndt_step_size);//设置NDT网格结构的分辨率（VoxelGridCovariance）ndt.setResolution(ndt_resolution);//设置匹配迭代的最大次数ndt.setMaximumIterations(ndt_iterations);// 设置要配准的点云ndt.setInputSource(filtered_source_cloud);//设置点云配准目标ndt.setInputTarget(filtered_cloud);//输入的数据和匹配点云的最后一个点云匹配//设置使用机器人测距法得到的初始对准估计结果
*/Eigen::Translation3f init_translation(0, 0, 0);Eigen::AngleAxisf init_rotation_x(0, Eigen::Vector3f::UnitX());Eigen::AngleAxisf init_rotation_y(0, Eigen::Vector3f::UnitY());Eigen::AngleAxisf init_rotation_z(0, Eigen::Vector3f::UnitZ());Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix();//计算需要的刚体变换以便将输入的点云匹配到目标点云pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZI>);ndt_omp.align(*output_cloud, init_guess);pcl::transformPointCloud (*multiPoint[multiPoint.size()-1].cloud, *output_cloud, ndt_omp.getFinalTransformation());/************************测试用程序****************************/pcl::toROSMsg(*output_cloud, test_points);test_points.header.frame_id = "velodyne";test_pub.publish (test_points);/*************************************************************/Eigen::Matrix4f current_trans = ndt_omp.getFinalTransformation();std::cout<< " score: " << ndt_omp.getFitnessScore () << " prob:" << ndt_omp.getTransformationProbability() << std::endl;std::cout<<current_trans<<std::endl;//所有的点云累加到一块localMapCloud->points.insert(localMapCloud->points.end(),incloud->points.begin(),incloud->points.end());//形成非地面点云 //以当前帧为坐标，将历史帧的数据加到当前帧的坐标中for(size_t i = 0;i<multiPoint.size();i++){Eigen::Matrix4f transMatrix_1 = multiPoint[i].transMatrix*ndt_omp.getFinalTransformation();//所有转换矩阵进行更新pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZI>);pcl::transformPointCloud (*multiPoint[i].cloud, *transformed_cloud, transMatrix_1);     localMapCloud->points.insert(localMapCloud->points.end(),transformed_cloud->points.begin(),transformed_cloud->points.end());//形成非地面点云 }     pcl::toROSMsg(*localMapCloud, localMapCloudPoints);localMapCloudPoints.header.frame_id = "velodyne";local_map_pub.publish(localMapCloudPoints);localMapCloud->points.clear();//更新数据//计算两帧之间的距离，当距离大于0.2米时，数据进入multiPointif((fabs(current_trans(0,3))+fabs( current_trans(1,3))+fabs( current_trans(2,3))) > history_points_dis) //当大于0.4时候数据进入{multiPoint.push_back(matchpoint);if(multiPoint.size()>history_points_num) {multiPoint.erase(std::begin(multiPoint)); //Delete the first element} for(size_t i = 0;i<multiPoint.size()-1;i++)//更新所有的矩阵{multiPoint[i].transMatrix = multiPoint[i].transMatrix* ndt_omp.getFinalTransformation();}//multiPoint[multiPoint.size()-1].cloud->points.assign(incloud->points.begin(),incloud->points.end());//点云加入到里面}}int main(int argc, char * argv[]) {ros::init(argc, argv, "scan2scan");ros::NodeHandle nh;ros::NodeHandle nh_private("~");ros::Subscriber sub = nh.subscribe("/sourround_points", 1, lidar_subCallback);local_map_pub = nh.advertise<sensor_msgs::PointCloud2>("/local_map", 1);test_pub = nh.advertise<sensor_msgs::PointCloud2>("/test_points1", 1);//测试用ros::spin ();
}

效果

 

 


http://chatgpt.dhexx.cn/article/xJGQGASd.shtml

相关文章

正交匹配追踪算法OMP

正交匹配追踪算法OMP

浅谈压缩感知&#xff08;九&#xff09;&#xff1a;正交匹配追踪算法OMP </h1><div class"clear"></div><div class"postBody">主要内容&#xff1a; OMP算法介绍 OMP的MATLAB实现 OMP中的数学知识 一、OMP算法介绍 来源&#…
阅读更多...
OMP算法代码学习

OMP算法代码学习

版权声明&#xff1a;本文为博主原创文章&#xff0c;转载请注明出处&#xff0c;谢谢&#xff01; https://blog.csdn.net/jbb0523/article/details/45130793 </div><link rel"stylesheet" href"https://csdnimg.cn/release/phoenix/…
阅读更多...
美团饿了么返利公众号小程序搭建（付源码）

美团饿了么返利公众号小程序搭建（付源码）

外卖返利小程序源码; 轻松部署搭建&#xff0c;小程序服务号数据互通&#xff1b; 对接美团官方; 佣金比例自定义分配; 三级分佣&#xff0c;所有资金数据一目了然&#xff1b; 拉新立减最低4.9元购月卡&#xff1b; 签到20天免费领取会员卡&#xff1b; 提现秒到账&#xff01…
阅读更多...
OMP算法笔记

OMP算法笔记

OMP算法笔记 OMP算法整理&#xff08;以备自己后期查阅&#xff0c;集合了几篇博主的文章&#xff09; &#xff08;1&#xff09;数理知识基础–投影矩阵 详见&#xff1a; 作者&#xff1a;nineheaded_bird 来源&#xff1a;CSDN 原文&#xff1a;https://blog.csdn.net/teng…
阅读更多...
使用python实现微信小程序自动签到2.0

使用python实现微信小程序自动签到2.0

微信小程序自动签到 功能描述目标输出包管理 程序的结构设计步骤1步骤2步骤3步骤4 代码实现使用findler抓包工具查看请求类型再次使用findler抓包&#xff0c;查看请求内容使用多线程完成多用户提交的功能使用itchat第三方库实现微信自动回复 将程序部署到服务器中使用scp命令将…
阅读更多...
并行程序设计——OMP编程

并行程序设计——OMP编程

并行程序设计——OMP编程 实验一 实验内容 分别实现课件中的梯形积分法的Pthread、OpenMP版本&#xff0c;熟悉并掌握OpenMP编程方法&#xff0c;探讨两种编程方式的异同。 实验代码 OpenMP编程 #include <stdio.h> #include <stdlib.h> #include <omp.h&g…
阅读更多...
利用中国知网快速自动生成参考文献

利用中国知网快速自动生成参考文献

1.打开知网 2.输入引用的文献名称 3.点击文章 4.点击“导出/参考文献” 5.自动生成参考文献
阅读更多...
知网导出外文参考文献格式和下载文章（2019.5）

知网导出外文参考文献格式和下载文章（2019.5）

如果不弹出页面&#xff0c;是网络的原因&#xff0c;等一等
阅读更多...
谷歌学术、中国知网生成参考文献

谷歌学术、中国知网生成参考文献

谷歌学术 step1.登陆谷歌学术 step2.查找需要的文献 step3.点击引用标志 step4.生成相关引用 step5.选择不同的标准复制粘贴 中国知网 step1.在知网搜索需要的论文 step2.点击导出参考文献 step3.生成参考文献引用
阅读更多...
中国知网文献引用导入EndNote9.X，Web of science导入endnote以及谷歌学术导入endnote图文详解，全网最细版本适用EndNote9.x,Endnote20版本

中国知网文献引用导入EndNote9.X，Web of science导入endnote以及谷歌学术导入endnote图文详解，全网最细版本适用EndNote9.x,Endnote20版本

文章目录 一、EndNote导入文献的以下几种格式1.1 中国知网1.2web of science1.3 谷歌学术 一、EndNote导入文献的以下几种格式 all as we konow&#xff0c;引用参考文献也就是如下三个&#xff0c;那么分别导入endnote改怎么使用呢&#xff1f; 1.1 中国知网 在你想要的文献里…
阅读更多...
在CNKI上导出TXT文件

在CNKI上导出TXT文件

在使用CiteSpace之前要先下载数据源&#xff0c;今天就来讲一讲从CNKI上导出txt文件。 1、从学校官网进入中国知网CNKI&#xff0c;单击高级检索 2、输入关键字&#xff0c;可以选择组合输入&#xff0c;单击搜索 3、在每页显示处选择50 4、勾选所有所有记录&#xff08;每次导…
阅读更多...
EndNote20导入知网文献和导出BibTeX于TexStudio

EndNote20导入知网文献和导出BibTeX于TexStudio

第一步&#xff1a;在知网官网中找到一篇论文并点击引号键 第二步&#xff1a;点击EndNote下载该txt文件 第三步&#xff1a;在EndNote20中点击File->Import->File&#xff0c;引入刚刚下载的txt文件 注意Import Option选择的是EndNote Import&#xff0c;点击import导入…
阅读更多...
知网导出之Excel

知网导出之Excel

背景&#xff1a;需要整理知网XX方向的论文&#xff0c;包括题目&#xff0c;内容&#xff0c;发表时间等等信息。但是一个个写也太麻烦了&#xff0c;所以&#xff0c;一些不需要人总结的东西&#xff0c;就直接导出好了。 流程&#xff1a; 选择好文献之后&#xff0c;选择自…
阅读更多...
知网如何快速引用参考文献

知网如何快速引用参考文献

1.在知网搜索界面&#xff0c;搜索自己想搜的内容&#xff0c;然后点击下图引号位置 2.在弹框中选择你要引用的格式 复制粘贴到自己论文中即可
阅读更多...
知网 BibTeX自动生成（使用BibTeX引用中文参考文献）

知网 BibTeX自动生成（使用BibTeX引用中文参考文献）

前言 谷歌学术具备生成英文文献的bibtex文献引用代码的功能&#xff0c;而知网里不具备生成中文文献的bibtex引用代码的功能。因此&#xff0c;本文将生成中文文献bibtex引用代码的操作过程简单记录便于自己再次翻阅&#xff0c;操作方法源自知乎作者 上官无忌 &#xff0c;具…
阅读更多...
知网导出引用文件，插入到Endnote管理文献

知网导出引用文件，插入到Endnote管理文献

直接选endnote是txt格式 改格式也没用&#xff0c;endnote改也不行 这里选择refworks 导出txt之后 选择refworks import即可
阅读更多...
Web of Science如何导出参考文献

Web of Science如何导出参考文献

Web of Science&#xff08;WOS&#xff09;是大型综合性、多学科、核心期刊引文索引数据库。 Endnote是一款可以有效管理参考文献的软件&#xff0c;并且在论文写作时&#xff0c;利用endnote可以很方便的修改引用格式。 1.进入论文界面 2.点击查看PDF,然后点击红圈 3. 可以…
阅读更多...
Endnote导出目标期刊的参考文献的格式

Endnote导出目标期刊的参考文献的格式

Endnote导出目标期刊的参考文献的格式 1、知网文献与endnote的操作流程2、其他的参考文件来源&#xff08;如bib、ris等&#xff09;及导出格式选择 1、知网文献与endnote的操作流程 导出的格式为txt格式&#xff0c;这时候打开endnote&#xff0c;导入choose你的txt文件就可以…
阅读更多...
知网论文参考文献导入到Endnote方法

知网论文参考文献导入到Endnote方法

第一步下载参考文献文件第二步&#xff1a;右键->打开方式->选择endnote 3. 导入结果
阅读更多...
知网获取论文参考文献

知网获取论文参考文献

知网获取论文参考文献 进入知网搜索相应材料普通检索高级检索 选择相应的文献点击右上角左边双引号“凑”参考文献 进入知网 中国知网官方网址&#xff1a;https://www.cnki.net/ 搜索相应材料 搜素一般可分为普通检索和高级检索。 一般而言&#xff0c;普通检索即可完成我…
阅读更多...
推荐文章

Copyright @ 2022~2023