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 ();
}

效果

SLAM前端之ndt_omp使用相关推荐

激光SLAM 前端数据预处理--剔除坏点方法总结
激光SLAM 前端数据预处理--剔除坏点方法总结距离处理去除掉非常近的点 Code 测试结果分析去除掉非常近的点 Code 测试去除NaN的点 Code 反射率处理 Code 测试结果去 ...
SLAM前端中的视觉里程计和回环检测
1. 通常的惯例是把 VSLAM 分为前端和后端.前端为视觉里程计和回环检测,相当于是对图像数据进行关联:后端是对前端输出的结果进行优化,利用滤波或非线性优化理论,得到最优的位姿估计和全局一致性地图. ...
视觉SLAM前端特征检测与跟踪的思考
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达就目前视觉SLAM的引用来区分,分为基于特征法的和直接法的视觉SL ...
SLAM前端 ---------特征提取之ORB（ORB与SIFT与SURF）
ORB 论文翻译: 一种特征匹配替代方法:对比SIFT或SURF 1.ORB特征简介 ORB是Oriented FAST and Rotated BRIEF(oFAST and rBRIEF)的简称 ...
SLAM前端知识汇总
1.常用特征点汇总 1.1 Fast特征点 <SLAM14讲>中LK光流中用的特征点就是Fast特征点,然后对这些特征点进行光流追踪. 在8.3.2节的LK光流代码,第1帧提取的就是Fas ...
视觉SLAM前端——ICP
当我们知道空间点在两个坐标系下的位置时,可以通过ICP来求解其相对位姿.ICP最初常用在激光SLAM中,因为激光可以获得3D位置,而不像视觉还需要经过相机的投影变换,但是从激光雷达获得的三维点云中,我 ...
视觉SLAM前端——LK光流法
目录: LK光流介绍单层LK光流多层LK光流 LK光流 LK光流是一种描述图像运动的方法,利用LK光流可以实现对图像的追踪,从而求解图像运动的位姿.其基本思想如下: img1,img2分别 ...
SLAM前端：PnP（一）DLS、P3P
PnP是一类问题的统称,是指通过多对点的3D位置及2D投影坐标,来估计相机位姿R.t. 场景一:视觉slam中在初始化后可以知道空间中一些点在世界坐标系下的坐标,在下一帧图像进行特征点匹配后,利用这些 ...
[SLAM前端系列]——一文读懂ICP
今天突然发现这篇博客写的并不详细,配不上一文读懂这四个字,特此回来更新. 我们知道SLAM可以用2D/3D雷达或者相机实现(视觉SLAM).由于笔者是激光SLAM工程师,所以本文的ICP都是在激光雷达 ...
视觉SLAM前端——PnP
如果我们已知多对匹配好的2D/3D特征点,我们可以用PnP方法去求得相机的运动.对于单目相机来说,我们可以通过利用对极约束或者单应性求得相机的运动,然后三角化获得3D点:对于双目可以通过视差恢复深度信 ...

SLAM前端之ndt_omp使用