SLAM前端之ndt_omp使用

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>
#i