激光雷达标定教程：lidar2lidar，OpenCali, ROS

提示：

---

前言

在多传感器融合系统中，各个传感器的数据需要在同一个坐标系中进行处理和分析。标定的主要目的是确定不同传感器之间的相对位置和方向，以便将它们的数据准确地整合到一起。

标定是机器人感知开发中的一项基本操作，其核心目的是求解一个变换矩阵T，
$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$
这里，(R) 是3x3旋转矩阵，(t) 是3x1的平移向量。

给定一个点 (p) 在激光雷达A的坐标系下的坐标 (p_A)，在激光雷达B的坐标系下的坐标 (p_B) 可以通过矩阵 (T) 相互转换：

$$p_B=T\cdot p_A$$

---

提示：本文中将以两个单线激光之间的标定作为案例，通过ROS进行数据获取发布，将点云信息转换成pcd文件格式，使用OpenCali进行手工标定。

一、标定流程

标定的整个流程及注意事项，可以总结为

• 初步估计
  通过几何关系，手动计算一个变换矩阵$T^{\prime }$，作为标定的初始值。一般建议测量一下两个lidar之间基座的平移关系，初步估计旋转角度就可以。

• 数据采集与处理
  采集（多个）同一时刻个激光雷达的点云数据，且确保他们有共同的可视区域，并且该区域处于这些lidar的低误差区间。某些lidar,如单线激光的点云数量较少不易识别特征，建议在重叠可视区域中放置一块特征明显的标定板。
  完成原始数据采集后，视标定方法的情况处理数据，方便输入。

• 标定
  采取手工或自动算法，获取变换矩阵$T$，即标定。我推荐使用OpenCali。
  不建议自己编译这个源码，有些依赖不好搞定，建议直接docker，大约11G整个环境。
  关于docker下载慢，请查阅这个。

https://github.com/hyaline-wang/MyClashShell/blob/master/doc/%E5%9C%A8docker%E4%B8%AD%E4%BD%BF%E7%94%A8.md

二、具体操作

以放置在长方形机器人底盘对角线的两个激光雷达A和B为例，

2.1 初步估计

通过拉皮尺测量， A基座坐标系往B坐标系变换的过程，需要往 X Y Z轴方向平移 0.82 -1.8 0.05米， 且绕Z轴旋转（Yaw）180度。关于传感器坐标轴定义，你需要查阅对应的产品手册。

平移很好处理，旋转矩阵可以通过在计算器中输入角度处理

计算器地址。
提示：注意单位不要选错， 注意绕轴旋转和欧拉角旋转的区别使用。另外，这个工具有个不太符合用户直觉的地方，缺少一个“计算”的按钮，有时候你的输入更新，右侧输出并没有变化。注意自己斟酌和验算。
$$R=\left[\begin{array}{ccc}-1.0& -0.0& 0.0\\ 0.0& -1.0& 0.0\\ 0.0& 0.0& 1.0\end{array}\right]$$

$$t=\left[\begin{array}{c}0.82\\ 1.8\\ 0.05\end{array}\right]$$

$$T=\left[\begin{array}{cccc}-1.0& -0.0& 0.0& 0.82\\ 0.0& -1.0& 0.0& 1.8\\ 0.0& 0.0& 1.0& 0.05\\ 0& 0& 0& 1\end{array}\right]$$

2.2 数据采集及处理

完成多传感器驱动，并且获取了传感器数据。如本案例中两个单线激光数据通过Ros发布，我将数据用rosbag包进行记录，并且在Rviz中可视化

Rosbag 记录

rosbag record   <topic_name1> <topic_name2> <topic_name3>

也可以加 -o(小写): 指定包名前缀。文件默认保存在执行命令的位置。

rosbag record  -o laser_record /front_laser /rear_laser

写一个Launch文件，播放数据包，整合坐标变换发布 和打开rviz

<launch>
    <!-- 使用仿真时间 -->
    <param name="use_sim_time" value="true"/>  

    <!-- 播放ROS bag文件 -->
    <node name="rosbag_play" pkg="rosbag" type="play" output="screen" args="--clock --loop $(find vanjee_mini_lidar)/../../data_bag/new_front_rear_laser_record_cali.bag"/>

    <!-- 添加lidar的静态变换发布 -->
    <node name="front_scanner_to_laser" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0 0 0 0 front_scanner front_laser"/>
    <node name="rear_scanner_to_laser" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0 0 0 0 rear_scanner rear_laser"/>

    <!-- 发布map到robot的静态变换 -->
    <node name="map_to_robot" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0 0 0 0 map robot"/>

    <!-- 发布robot到front_scanner的静态变换 -->
    <node name="robot_to_front_scanner" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0.0 0 0 0 robot front_scanner"/>

    <!-- 发布robot到rear_scanner的静态变换 -->
    <node name="robot_to_rear_scanner" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0.0 0 0 0 robot rear_scanner"/>


    <!-- RViz 可视化 -->
    <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find vanjee_mini_lidar)/../../data_bag/visual_cali.rviz"/>
</launch>

两个传感器数据叠加到一个场景中，视觉上杂乱无章。
我将固定坐标系（Fixed Frame）的坐标轴显示了出来，不过这个坐标系和两个雷达没什么关系，主要是为了好看。
红圈内是同一个标定板在两个传感器视角下的样子，这也是为什么要记录连续数据并播放，并且拿一个大标定物体的原因，方便辨认。

由于OpenCali工具箱中lidar2lidar工具需要输入PCD文件

./run_lidar2lidar <target_pcd_path> <source_pcd_path> <extrinsic_json>

所以我们需要把从数据包中解析成pcd格式
在我的案例中，lidar发布的不是sensor_msgs::PointCloud2类型，而是传感器类型sensor_msgs::LaserScan，所以需要先完成ROS类型转换，再完成ROS->Pcd文件。

如果你的传感器驱动比较方便，你也可以用别的方式产生pcd文件。后续使用中注意保证两个传感器数据时间上是同步的。

这部分代码可以参考以下：

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <laser_geometry/laser_geometry.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <boost/filesystem.hpp>
#include <iostream>

void savePointCloudToPCD(const pcl::PointCloud<pcl::PointXYZI> &cloud, const std::string &filename)
{
    pcl::io::savePCDFileASCII(filename, cloud);
}

void convertLaserScanToPointCloud(const sensor_msgs::LaserScan::ConstPtr &scan, pcl::PointCloud<pcl::PointXYZI> &cloud)
{
    laser_geometry::LaserProjection projector;
    sensor_msgs::PointCloud2 cloud_msg;
    projector.projectLaser(*scan, cloud_msg);

    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(cloud_msg, pcl_pc2);

    pcl::PointCloud<pcl::PointXYZI> temp_cloud;
    pcl::fromPCLPointCloud2(pcl_pc2, temp_cloud);

    // Resize the cloud to accommodate intensity data
    cloud.resize(temp_cloud.size());

    // Copy point data and intensity information
    for (size_t i = 0; i < temp_cloud.size(); ++i)
    {
        cloud.points[i].x = temp_cloud.points[i].x;
        cloud.points[i].y = temp_cloud.points[i].y;
        cloud.points[i].z = temp_cloud.points[i].z;
        cloud.points[i].intensity = scan->intensities[i];
    }
}

int main(int argc, char **argv)
{
    if (argc != 2)
    {
        std::cerr << "Usage: " << argv[0] << " <bag_file>" << std::endl;
        return -1;
    }

    std::string bag_file = argv[1];
    if (!boost::filesystem::exists(bag_file))
    {
        std::cerr << "Error: Bag file " << bag_file << " does not exist." << std::endl;
        return -1;
    }

    boost::filesystem::create_directory("front");
    boost::filesystem::create_directory("rear");

    rosbag::Bag bag;
    bag.open(bag_file, rosbag::bagmode::Read);

    std::vector<std::string> topics;
    topics.push_back("/front_scan");
    topics.push_back("/rear_scan");

    rosbag::View view(bag, rosbag::TopicQuery(topics));

    int front_index = 0;
    int rear_index = 0;

    for (const rosbag::MessageInstance &m : view)
    {
        sensor_msgs::LaserScan::ConstPtr scan = m.instantiate<sensor_msgs::LaserScan>();
        if (scan != nullptr)
        {
            pcl::PointCloud<pcl::PointXYZI> cloud;
            convertLaserScanToPointCloud(scan, cloud);

            std::string filename;
            if (m.getTopic() == "/front_scan")
            {
                filename = "front/front_" + std::to_string(front_index++) + ".pcd";
            }
            else
            {
                filename = "rear/rear_" + std::to_string(rear_index++) + ".pcd";
            }

            savePointCloudToPCD(cloud, filename);
        }
    }

    bag.close();
    std::cout << "Conversion complete." << std::endl;

    return 0;
}

最终我们获得了两个传感器对应的每一帧数据。
.
├── front
├── rear
├── laserScan2pcd #上述代码产生的执行文件
├── pcl_viewer # 来自pcl库的工具，你可以用它可视化pcd文件是否解析成功

pcl_viewer相关教程可以看这位博主：

https://blog.csdn.net/qq_42994487/article/details/130443844

2.3 开始标定

OpenCali安装

最简安装方式

git clone https://github.com/PJLab-ADG/SensorsCalibration.git

cd SensorsCalibration

# pull docker image 
sudo docker pull scllovewkf/opencalib:v1

# 这个脚本会将工具根目录和docker环境共享，同时配置NVIDIA环境
sudo ./run_docker.sh

环境安装好后，

进入对应工具包进行编译，以手动标定为例

cd lidar2lidar/manual_calib
mkdir build
cmake .. 
make -j4      
cd ../

编译完成后可执行文件在bin目录
Four input files:

./bin/run_lidar2lidar <target_pcd_path> <source_pcd_path> <extrinsic_json>

• target_pcd_path: 目标坐标系 PCD 文件
• source_pcd_path: 原始坐标系 PCD 文件
• extrinsic_json: 初始外参文件

前两个文件视你的需求，将某一个传感器作为基准坐标系。
对于外参究竟是 target2source 还是source2target，很容易搞混，这里我提供工具的源码片段给大家解析

// 将 source 与外参矩阵相乘，获得其在target坐标系下的结果
// 红色的是target
void ProcessSourceFrame(const pcl::PointCloud<pcl::PointXYZI>::Ptr cloudLidar,
                        const Eigen::Matrix4d &extrinsic,
                        const bool &diaplay_mode)
{

//省略 ...
  for (int ipt = 0; ipt < pointsNum; ipt++)
  {
    Eigen::Vector4d pointPos(cloudLidar->points[ipt].x,
                             cloudLidar->points[ipt].y,
                             cloudLidar->points[ipt].z, 1.0);
    Eigen::Vector4d trans_p = extrinsic * pointPos; // 左乘矩阵，完成坐标系变换
    dataUpdate[ipt * 3 + 0] = trans_p.x();
    dataUpdate[ipt * 3 + 1] = trans_p.y();
    dataUpdate[ipt * 3 + 2] = trans_p.z();
  }
//省略 ...
  std::cout << "Process target lidar frame!\n";
}

ProcessSourceFrame(source_cloud, calibration_matrix_, display_mode_);

即
$$p_{target}=Extrinsic\cdot p_{source}$$

第三个文件，初始的外参文件，是以json形式存在的。把之前通过几何计算获得的初始值输入进data字段。尤其注意这个是按行来组织的，你的数据别填错了。

{
    "left_lidar-to-top_center_lidar-extrinsic": {
        "param_type": "extrinsic",
        "sensor_name": "front",
        "target_sensor_name": "rear",
        "device_type": "relational",
        "param": {
            "sensor_calib": {
                "data": [
                    [
                        -0.9984212,
                        -0.0561700,
                        0.0,
                        -1.865
                    ], #变换矩阵第一行
                    [
                        0.0561700,
                        -0.9984212,
                        0.0,
                        -0.634
                    ], #变换矩阵第二行
                    [
                        0,
                        0,
                        1,
                        0
                    ],
                    [
                        0,
                        0,
                        0,
                        1
                    ]
                ],
                "continuous": true,
                "rows": 4,
                "type": 6,
                "cols": 4
            },
            "time_lag": 0
        }
    }
}

数据准备完成，执行程序开始手工标定。

./bin/run_lidar2lidar front/front_120.pcd  rear/rear_120.pcd front-to-rear-extrinsic.json`

点击各种按钮将两个lidar的点云重合，

以下是官方文档的翻译
校准窗口由左侧的手动校准控制面板和右侧的点云界面组成。用户可以通过点击面板中的相应按钮或使用键盘输入来调整外部参数，以检查两个点云是否对齐。当两个点云对齐时，校准结束，点击保存按钮以保存结果。

外部参数调整	键盘输入	外部参数调整	键盘输入
+x 角度	q	-x 角度	a
+y 角度	w	-y 角度	s
+z 角度	e	-z 角度	d
+x 平移	r	-x 平移	f
+y 平移	t	-y 平移	g
+z 平移	y	-z 平移	h

强度颜色: 激光雷达强度记录为激光束的返回强度，部分基于激光脉冲击中的物体的反射率。此按钮可以将显示模式更改为强度图显示模式。这有助于检查地面车道线是否对齐。

角度步长 平移步长: 这两个按钮更改每次点击或键盘输入的调整步长。

点大小: 调整投影图像中激光雷达点的大小。

重置: 按下按钮重置所有手动调整。

保存结果: 如果按下此按钮，结果将默认存储在运行目录 ~./manual_calib/：

Extrinsic:
R:
0.0121008 -0.999863 -0.0112902
0.0133341 0.0114512 -0.999846
0.999838 0.0119484 0.0134707
t: 0.0134153 -0.352602 -0.575013

************* json format *************
Extrinsic:
[0.0121008,-0.999863,-0.0112902,0.0134153],[0.0133341,0.0114512,-0.999846,-0.352602],[0.999838,0.0119484,0.0134707,-0.575013],[0,0,0,1]

自此，完成lidar2lidar的标定。
一般来讲我都会选择手动标定，尤其是像两个单线这种点云数量少的情况。
后续我再在这个博客补充OpenCali中自动标定怎么使用。你已了解基本原理了，也可以自己去试试。

---