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

multi_lidar_calibration.cpp

#include <chrono>
#include "multi_lidar_calibration.h"

MultiLidarCalibration::MultiLidarCalibration(ros::NodeHandle &n) : nh_(n)
{
    ROS_INFO_STREAM("\033[1;32m----> Multi Lidar Calibration Use ICP...\033[0m");

    nh_.param<std::string>("/multi_lidar_calibration_node/source_lidar_topic", source_lidar_topic_str_, "/sick_back/scan");
    nh_.param<std::string>("/multi_lidar_calibration_node/target_lidar_topic", target_lidar_topic_str_, "/sick_front/scan");
    nh_.param<std::string>("/multi_lidar_calibration_node/source_lidar_frame", source_lidar_frame_str_, "sub_laser_link");
    nh_.param<std::string>("/multi_lidar_calibration_node/target_lidar_frame", target_lidar_frame_str_, "main_laser_link");
    nh_.param<float>("/multi_lidar_calibration_node/icp_score", icp_score_, 5.5487);
    nh_.param<float>("/multi_lidar_calibration_node/main_to_base_transform_x", main_to_base_transform_x_, 0.352);
    nh_.param<float>("/multi_lidar_calibration_node/main_to_base_transform_y", main_to_base_transform_y_, 0.224);
    nh_.param<float>("/multi_lidar_calibration_node/main_to_base_transform_row", main_to_base_transform_row_, -3.1415926);

    nh_.param<float>("/multi_lidar_calibration_node/main_to_base_transform_yaw", main_to_base_transform_yaw_, 2.35619);

    // 发布转换后的激光点云
    final_point_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/final_point_cloud", 10);

    // 订阅多个激光话题
    scan_front_subscriber_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, target_lidar_topic_str_, 1);
    scan_back_subscriber_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, source_lidar_topic_str_, 1);
    scan_synchronizer_ = new message_filters::Synchronizer<SyncPolicyT>(SyncPolicyT(10), *scan_front_subscriber_, *scan_back_subscriber_);
    scan_synchronizer_->registerCallback(boost::bind(&MultiLidarCalibration::ScanCallBack, this, _1, _2));

    // 参数赋值
    is_first_run_ = true;

    // 在main_laser_link下sub_laser_link的坐标
    transform_martix_ = Eigen::Matrix4f::Identity(); //4 * 4 齐次坐标
    // 在base_link坐标系下main_laser_link的坐标
    front_to_base_link_ = Eigen::Matrix4f::Identity();

    //点云指针赋值
    main_scan_pointcloud_ = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>());
    sub_scan_pointcloud_ = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>());
    final_registration_scan_ = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>());
    // 使用在main_laser_link下sub_laser_link的坐标,把sub_laser_link下的激光转换到main_laser_link下
    sub_scan_pointcloud_init_transformed_ = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>());
}

MultiLidarCalibration::~MultiLidarCalibration() {}

/**
 * @brief 获取激光雷达间的坐标变换
 * 
 * @param transform_martix_ 激光雷达间的转换矩阵
 * @param front_to_base_link_ 在main_laser_link下sub_laser_link的坐标
 */
void MultiLidarCalibration::GetFrontLasertoBackLaserTf()
{
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener tfl(buffer);

    ros::Time time = ros::Time::now();
    ros::Duration timeout(0.1);

    geometry_msgs::TransformStamped tfGeom;
    try
    {
        tfGeom = buffer.lookupTransform(source_lidar_frame_str_, target_lidar_frame_str_, ros::Time::now(), ros::Duration(3.0));
    }
    catch (tf2::TransformException &e)
    {
        ROS_ERROR_STREAM("Lidar Transform Error ... ");
    }

    // tf2矩阵转换成Eigen::Matrix4f
    Eigen::Quaternionf qw(tfGeom.transform.rotation.w, tfGeom.transform.rotation.x, tfGeom.transform.rotation.y, tfGeom.transform.rotation.z); //tf 获得的四元数
    Eigen::Vector3f qt(tfGeom.transform.translation.x, tfGeom.transform.translation.y, tfGeom.transform.translation.z);                        //tf获得的平移向量
    transform_martix_.block<3, 3>(0, 0) = qw.toRotationMatrix();
    transform_martix_.block<3, 1>(0, 3) = qt;

    // 绝对标定的前向激光到base_link的坐标转换
    Eigen::Vector3f rpy(main_to_base_transform_row_, 0, main_to_base_transform_yaw_);
    Eigen::Matrix3f R;
    R = Eigen::AngleAxisf(rpy[0], Eigen::Vector3f::UnitX()) *
        Eigen::AngleAxisf(rpy[1], Eigen::Vector3f::UnitY()) *
        Eigen::AngleAxisf(rpy[2], Eigen::Vector3f::UnitZ());
    Eigen::Vector3f t(main_to_base_transform_x_, main_to_base_transform_y_, 0.242);

    front_to_base_link_.block<3, 3>(0, 0) = R;
    front_to_base_link_.block<3, 1>(0, 3) = t;
    ROS_INFO_STREAM("main_laser_link in base_link matrix=\n"
                    << front_to_base_link_);
}

/**
  * @brief 激光雷达发布点云
  * @param in_cloud_to_publish_ptr 输入icp转换后的激光点云数据
  */
void MultiLidarCalibration::PublishCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &in_cloud_to_publish_ptr)
{
    sensor_msgs::PointCloud2 cloud_msg;
    pcl::toROSMsg(*in_cloud_to_publish_ptr, cloud_msg);
    cloud_msg.header.frame_id = target_lidar_frame_str_;
    final_point_cloud_pub_.publish(cloud_msg);
}

/**
 * @brief 激光雷达消息类型转换 sensor_msg::Laser to pcl::PointCloud<pcl::PointXYZ>
 * 
 * @param scan_msg 输入sensor_msgs
 * @return pcl::PointCloud<pcl::PointXYZ> 输出pcl格式点云
 */
pcl::PointCloud<pcl::PointXYZ> MultiLidarCalibration::ConvertScantoPointCloud(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    pcl::PointCloud<pcl::PointXYZ> cloud_points;
    pcl::PointXYZ points;

    for (int i = 0; i < scan_msg->ranges.size(); ++i)
    {
        float range = scan_msg->ranges[i];
        if (!std::isfinite(range))
        {
            continue;
        }

        if (range > scan_msg->range_min && range < scan_msg->range_max)
        {
            float angle = scan_msg->angle_min + i * scan_msg->angle_increment;
            points.x = range * cos(angle);
            points.y = range * sin(angle);
            points.z = 0.0;
            cloud_points.push_back(points);
        }
    }
    return cloud_points;
}

/**
 * @brief 多个激光雷达数据同步
 * 
 * @param in_main_scan_msg 激光雷达topic 1
 * @param in_sub_scan_msg 激光雷达topic 2
 */
void MultiLidarCalibration::ScanCallBack(const sensor_msgs::LaserScan::ConstPtr &in_main_scan_msg, const sensor_msgs::LaserScan::ConstPtr &in_sub_scan_msg)
{
    main_scan_pointcloud_ = ConvertScantoPointCloud(in_main_scan_msg).makeShared();
    sub_scan_pointcloud_ = ConvertScantoPointCloud(in_sub_scan_msg).makeShared();
}

/**
 * @brief 两个激光雷达数据进行icp匹配
 * 
 */
bool MultiLidarCalibration::ScanRegistration()
{
    if (0 == main_scan_pointcloud_->points.size() || 0 == sub_scan_pointcloud_->points.size())
    {
        return false;
    }

    // 对点云进行初始化旋转,back_link to front_link
    pcl::transformPointCloud(*sub_scan_pointcloud_, *sub_scan_pointcloud_init_transformed_, transform_martix_);

    // 最大欧式距离差值
    icp_.setMaxCorrespondenceDistance(0.1);
    // 迭代阈值,当前变换矩阵和当前迭代矩阵差异小于阈值,认为收敛
    icp_.setTransformationEpsilon(1e-10);
    // 均方误差和小于阈值停止迭代
    icp_.setEuclideanFitnessEpsilon(0.01);
    // 最多迭代次数
    icp_.setMaximumIterations(100);

    icp_.setInputSource(sub_scan_pointcloud_init_transformed_);
    icp_.setInputTarget(main_scan_pointcloud_);

    icp_.align(*final_registration_scan_);

    if (icp_.hasConverged() == false && icp_.getFitnessScore() > 1.0)
    {
        ROS_WARN_STREAM("Not Converged ... ");
        return false;
    }
    return true;
}

/**
 * @brief 打印结果 
 * 
 */
void MultiLidarCalibration::PrintResult()
{
    if (icp_.getFitnessScore() > icp_score_)
    {
        return;
    }

    // sub激光雷达到main雷达的icp的计算结果
    Eigen::Matrix4f T = Eigen::Matrix4f::Identity();
    T = icp_.getFinalTransformation();
    Eigen::Matrix3f R3 = T.block<3, 3>(0, 0);
    Eigen::Vector3f t3 = T.block<3, 1>(0, 3);

    // main激光到base_link的坐标变换
    Eigen::Matrix3f R1 = front_to_base_link_.block<3, 3>(0, 0);
    Eigen::Vector3f t1 = front_to_base_link_.block<3, 1>(0, 3);

    // 在main激光雷达坐标系下的sub雷达的坐标位置,两个激光对称放置
    Eigen::Matrix3f R4;
    Eigen::Vector3f t4;
    R4 << -1, 0, 0, 0, -1, 0, 0, 0, 1;
    t4 << -0.704, -0.448, 0;

    // 变换结果是以base_link坐标系下的sub激光雷达的坐标
    Eigen::Matrix3f R2 = R4 * R1 * R3;
    Eigen::Vector3f t2 = R1 * t3 + t1 + t4;

    // 输出转换关系
    Eigen::Vector3f eulerAngle = R2.eulerAngles(0, 1, 2);
    ROS_INFO_STREAM("eulerAngle=\n"
                    << eulerAngle);
    ROS_INFO_STREAM("transform vector=\n"
                    << t2);
}

/**
 * @brief  点云可视化
 * 
 */
void MultiLidarCalibration::View()
{
    // 点云可视化
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0); //背景色设置

    // 显示源点云
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(main_scan_pointcloud_, 0, 255, 0);
    viewer->addPointCloud<pcl::PointXYZ>(main_scan_pointcloud_, source_color, "source");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source");

    // 显示目标点云
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(sub_scan_pointcloud_, 255, 0, 255);
    viewer->addPointCloud<pcl::PointXYZ>(sub_scan_pointcloud_, target_color, "target");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "target");

    // 显示变换后的源点云
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_trans_color(final_registration_scan_, 255, 255, 255);
    viewer->addPointCloud<pcl::PointXYZ>(final_registration_scan_, source_trans_color, "source trans");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "source trans");

    // 保存变换结果
    pcl::io::savePLYFile("final_registration_scan.pcd", *final_registration_scan_, false);
    viewer->spin();
}

/**
 * @brief 运行主函数
 * 
 */
void MultiLidarCalibration::Run()
{
    if (is_first_run_)
    {
        GetFrontLasertoBackLaserTf();
        is_first_run_ = false;
        return;
    }

    // 进行icp匹配,匹配失败返回
    if (!ScanRegistration())
    {
        return;
    }

    PublishCloud(final_registration_scan_);

    PrintResult();

    // View();
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "multi_lidar_calibration");
    ros::NodeHandle n;

    MultiLidarCalibration multi_lidar_calibration(n);
    ros::Rate rate(10);
    while (ros::ok())
    {
        // update laserscan
        ros::spinOnce();

        multi_lidar_calibration.Run();

        rate.sleep();
    }

    return 0;
}

请添加图片描述
本质上工作就是通过ICP匹配(NDT也可)来实现点云匹配,从而得到外参的R,t值。

3. 参考链接

https://adamshan.blog.csdn.net/article/details/105736726
https://zhuanlan.zhihu.com/p/96908474
https://github.com/AbangLZU/multi_lidar_calibration