基于多线程ndt配准的定位

   一、前言

        对于室外无人车而言，如果在运行过程中了解自身在真实物理世界中的位姿是智能驾驶导航系统的先决条件，定位精度会直接影响到智能驾驶导航精度。

        本文主要讲讲述了如何利用NDT算法和一张已经构建的点云地图去实时定位小车的当前位姿。对于NDT在PCL点云库有现成的实现可以调用，但是经过本人在是用32线激光雷达定位，定位的速度是比较差强人意的，不太稳定有时候会超过100ms，也就是说定位的帧率有时候会慢于激光雷达的帧率。好在koide3/ndt_omp。这个包提供了一个从pcl派生的OpenMP增强的NDT算法，使用多线程的方法加快NDT配准的速度，它可以比pcl中的原始版本快10倍。参考:GitHub - koide3/ndt_omp: Multi-threaded and SSE friendly NDT algorithm。

        接下来就大概记录一下如何利用ndt-omp构建一个基于多线激光雷达的定位程序。

二、安装

        ndt-omp是一个依赖与PCL的标准ROS功能包，也就是实际上配置起来非常简单，就放到自己的ROS工作空间里一起catkin_make就可以了。

三、实战

1.cmake引入ndt-omp程序库：

find_package(catkin REQUIRED COMPONENTS
ndt_omp
)

catkin_package(
CATKIN_DEPENDS
ndt_omp
)

2.package.xml引入依赖关系：

<build_depend>ndt_omp</build_depend>
<exec_depend>ndt_omp</exec_depend>

3.程序编写：

头文件： 

#include "pclomp/ndt_omp.h"

ndt配准对象参数设置：

//omp-ndt对象创建
auto ndt = boost::make_shared<pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>>();

//两次变换之间允许的最大值,用于判断是否收敛,作为迭代计算完成的阈值,设为0.05
ndt->setTransformationEpsilon(trans_epsilon_);

//最大迭代步长，设为0.1
ndt->setStepSize(step_size_);

//分辨率，设为2.0
ndt->setResolution(resolution_);

//最大迭代速度，设为100
ndt->setMaximumIterations(max_iterations_);

//配准方法，设为pclomp::DIRECT7
ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);

输入配准目标点云：

ndt->setInputTarget(cloud_map);

//这边要先align一次,不然后面会卡一下，没搞懂为什么
auto output_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
ndt->align(*output_cloud, Eigen::Matrix4f::Identity());

在激光雷达点云数据的回调函数里进行实时的配准定位：

//降采样输入激光点云，分辨率设为1.0
pcl::VoxelGrid<pcl::PointXYZ> sor;
auto cloud_filtered = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
sor.setInputCloud(cloud);
sor.setLeafSize(voxel_grid_, voxel_grid_, voxel_grid_);
sor.filter(*cloud_filtered);

//把输入点云的坐标转化为车体坐标
auto base_to_sensor_affine = tf2::transformToEigen(TF_base_to_sensor_);
auto base_to_sensor_matrix = base_to_sensor_affine.matrix().cast<float>();
auto transform_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
pcl::transformPointCloud(*cloud_filtered, *transform_cloud, base_to_sensor_matrix);
    transform_cloud_ = transform_cloud;

//点云地图配准
Eigen::Affine3d initial_pose_affine;
tf2::fromMsg(*initial_pose_, initial_pose_affine);
Eigen::Matrix4f initial_pose_matrix = initial_pose_affine.matrix().cast<float>();

ndt->setInputSource(transform_cloud_);
auto output_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
ndt->align(*output_cloud, initial_pose_matrix);

//获取结果
auto result_pose_matrix = ndt->getFinalTransformation();
Eigen::Affine3d result_pose_affine;
result_pose_affine.matrix() = result_pose_matrix.cast<double>();
initial_pose_ = boost::make_shared<geometry_msgs::Pose>(tf2::toMsg(result_pose_affine));

//配准精度
double transform_probability = ndt->getTransformationProbability();