2D激光雷达的多传感器拼接

0.简介

为了保证激光雷达的360°环境覆盖，我们常常需要用到多传感器的拼接，如果我们单纯的取读取激光雷达的信息会出现如下图的情况，两个激光雷达会发生重叠，这就需要我们去对激光雷达进行标定。

<arg name="device_ip1" default="192.168.1.200" />
<arg name="device_ip2" default="192.168.1.200" />

<arg name="msop_port1" default="2368" />
<arg name="difop_port1" default="2369" />
<arg name="msop_port2" default="2370" />
<arg name="difop_port2" default="2371" />
<arg name="return_mode" default="1" />
<arg name="time_synchronization" default="true" />

1. ICP和NDT

NDT：相当于是栅格化的ICP

优点：

• 栅格化可以去除噪点的影响
• NDT算法便于用GPU加速

缺点；

• 对于结构化点云，NDT对一块栅格高斯分布的假设不成立，效果不好，反而点线ICP(2D)或点面ICP效果更好一些。
• 对于非结构大规模点云，NDT速度快一些，初值鲁棒性取决于栅格大小，越大，精度越差，但对初值鲁棒性好一些，反之，对初值更依赖，类似于ICP对应的min_distance最小匹配距离作用。
• 如果想让点云匹配对初值不那么敏感，可以考虑CSM+ICP的方法，CSM确定一个初始范围，再通过ICP精确化。



• 
  

两种方法的详细数学推导部分可以参照​​这篇文章​​，一般我们在PCL库中即可直接使用

2. 多激光雷达标定

这里推荐一个非常好的博主​​AdamShan​​，其文章对于多激光标定部分写了非常详细的内容。同时也有在Github上存在有​​多激光雷达标定​​​以及​​激光雷达与摄像头​​的标定程序，这里就不贴代码了（写这一段是为了方便后续自我回顾以及查找）。而在室内机器人中同样会面临使用多传感器标定的问题（多机器人编队中地图与地图之间的外参匹配也会有这样问题），这里给出基于2D雷达的匹配方案。

multi_lidar_calibration.h

#include <iostream>
#include <vector>

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/TransformStamped.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <tf2/utils.h>
#include <tf2_ros/transform_listener.h>

#include <pcl_ros/point_cloud.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>

class MultiLidarCalibration
{
public:
    MultiLidarCalibration(ros::NodeHandle &n);
    ~MultiLidarCalibration();

    // 函数处理
    void Run();

private:
    // 订阅的激光话题
    std::string source_lidar_topic_str_;
    std::string target_lidar_topic_str_;

    // 激光雷达坐标系
    std::string source_lidar_frame_str_;
    std::string target_lidar_frame_str_;

    // icp匹配得分
    float icp_score_;

    //  在base_link坐标系下main_laser_link的坐标
    float main_to_base_transform_x_;
    float main_to_base_transform_y_;
    float main_to_base_transform_row_;
    float main_to_base_transform_yaw_;

    // 第一次运行时进行矩阵赋值
    bool is_first_run_;

    // 两个激光间的transfrom，通过tf2获得
    Eigen::Matrix4f transform_martix_;
    // 主激光到base_link的TF
    Eigen::Matrix4f front_to_base_link_;

    ros::NodeHandle nh_;
    // 纠正激光输出，类型pointcloud2
    ros::Publisher final_point_cloud_pub_;
    // 进行激光时间同步和数据缓存
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::LaserScan, sensor_msgs::LaserScan> SyncPolicyT;
    message_filters::Subscriber<sensor_msgs::LaserScan> *scan_front_subscriber_, *scan_back_subscriber_;
    message_filters::Synchronizer<SyncPolicyT> *scan_synchronizer_;

    // pcl格式的激光数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr main_scan_pointcloud_;
    // 标定雷达数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr sub_scan_pointcloud_;
    // 通过tf2转换后的待标定的激光数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr sub_scan_pointcloud_init_transformed_;

    /**
     * @brief icp 
     * @param final_registration_scan_ 通过icp处理把待标定数据，转换到目标坐标系下的激光点云数据
     */
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr final_registration_scan_;

    // 两个激光坐标系间初始坐标变换
    void GetFrontLasertoBackLaserTf();

    // 订阅main雷达和sub雷达两个激光数据
    void ScanCallBack(const sensor_msgs::LaserScan::ConstPtr &in_main_scan_msg, const sensor_msgs::LaserScan::ConstPtr &in_sub_scan_msg);

    // 极坐标转换到笛卡尔坐标，sensor_msgs::LaserScan to pcl::PointCloud<pcl::PointXYZ> (方法很多不限于这一种)
    pcl::PointCloud<pcl::PointXYZ> ConvertScantoPointCloud(const sensor_msgs::LaserScan::ConstPtr &scan_msg);

    // 标定后的激光点云进行发布，发布的数据格式是sensor_msgs::PointCloud2
    void PublishCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &in_cloud_to_publish_ptr);

    // 对main雷达和sub雷达的激光点云进行icp匹配
    bool ScanRegistration();

    // 发布标定结果
    void PrintResult();

    // 可视化
    void View();
};