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>
#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::DIRECT7
  ndt_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米时，数据进入multiPoint
  if((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 ();
}

效果