Livox_automatic_calibration 多Mid-360激光雷达标定

前言

        地址：https://github.com/Livox-SDK/Livox_automatic_calibration

        该方法主要依靠几何一致性假设，即多个雷达扫描出来的局部三维模型是一致的，通过对基准雷达(LiDAR0)进行移动建图，然后将其余雷达数据对LiDAR0的重建地图不断进行迭代配准与计算，依靠一致性假设不断减少匹配误差，直到算法收敛并且满足标定矩阵刚性不变特性（六条平行线），最后用一致性算法得出最终标定矩阵（外参）。

一、bug修改

        1.mapping文件夹和calibration文件夹下的PointCloudMapper.cpp中的InsertPoints函数中的

      if (!map_octree_->isVoxelOccupiedAtPoint(p)) {
        map_octree_->addPointToCloud(p, map_data_);
        incremental_points->push_back(p);
      }

        修改为：

      double min_x, min_y, min_z, max_x, max_y, max_z;
      map_octree_->getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
      bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);

      if (!isInBox || !map_octree_->isVoxelOccupiedAtPoint(p)) {
        map_octree_->addPointToCloud(p, map_data_);
        incremental_points->push_back(p);
      }

        2. calibration文件夹下的calibration.cpp中的

viewer_final->addPointCloud<pcl::PointXYZ>(H_LiDAR_Map, match_color, "match cloud"); //display the match cloud

        修改为：

pcl::PointCloud<pcl::PointXYZ>::Ptr Frame_Map(new pcl::PointCloud<pcl::PointXYZ>);
viewer_final->addPointCloud<pcl::PointXYZ>(Frame_Map, match_color, "match cloud"); //display the match cloud

二、编译和运行

        1.编译

cd Livox_automatic_calibration
mkdir build
cd build
cmake ..
make

编译生成三个文件，分别是mapping, calibration, fitline　

• mapping: 可视化建图工具（生成 T_Matrix.txt 文件）　　

• calibration: 可视化自动标定工具（生成 calib_data.txt 文件）　　

• fitline:　拟合标定参数，计算最终参数矩阵　

        2.运行

          首先准备双雷达标定pcd数据：　　

• 基准雷达数据放在 data/Base_LiDAR_Frames/.pcd (以 100000.pcd 作为第一帧文件名，后续帧往上累加)

• 待标定雷达数据放在 data/Target-LiDAR-Frames/.pcd (以 100000.pcd 作为第一帧文件名，后续帧往上累加)

• 粗配准外参矩阵放在 data/Init_Matrix.txt （这里为 T_base_target）

注意：基准雷达数据和待标定雷达数据需要尽可能在时间上同步，文件名和时间戳为同步对应。

cd Livox_automatic_calibration
cp run.sh build/
cd build
sh run.sh

        run.sh脚本首先启动可视化建图程序mapping，用基准雷达建立子地图，自动生成待匹配地图数据H-LiDAR-Map-data.pcd文件。建立完成后启动标定程序calibration，完成标定参数估计工作，最后启动 参数拟合器fitline，完成最终参数矩阵计算。

#!/bin/bash 
./mapping
./calibration
./fitline

        启动标定程序calibration时如下。红色点云：基准雷达建图结果；绿色点云: 自动标定中的目标雷达点云

        数据的自动标定结果输出如下： 

注意： 

• 必须保证双雷达数据同步
• 基准雷达建图必须尽可能准确
• 采集车、平台运动必须尽可能缓慢，数据建议进行运动畸变修正以保证最终的精度
• 粗配准外参数矩阵不必要很精准，但需要大致能够对齐，尽可能保证最终的精度。
• 对于Mid-40, Horizon型号，转换的PCD文件参照100ms为一帧。 

三、标定数据采集要求

        1.在移动采集数据期间，需要保证基准雷达LiDAR0在移动的时间段t内扫描的区域（重建的地图）能够被其余雷达所探测，需要保证最小的探测盲区。建议采用原地旋转或者”0”字形轨迹运动进行完全的场景扫描。

        2.采集数据的环境，尽量选用宽敞的室内或者地下车库，能够保证有较好的地图重建精度，地图完整度与特征丰富度（需注意Livox的探测盲区问题）。避免选用室外空旷，无明显几何结构特征且移动目标（车辆行人）较多的场景。采集数据时候避免四周有移动物体（注意切勿有人在围观或者跟随运动）。否则容易导致建图失败或者建图误差过大，影响后续的配准标定。

        3.移动采集时尽量保证运动的缓慢，特别时是转弯处，遵循慢速、大半径过弯原则，最小化运动畸变。

        4.传感器数据要保证同步，建议采用Livox HUB进行数据的获取。

        5.雷达的安装不局限于有重叠可视区域，可以任意安装，只要能够保证获取外参初值即可。

        6.采集格式无要求，可以是rosbag或者lvx，只需要能够按每帧100ms转换为多帧pcd文件即可。

四、参考标定过程

        1.找到合适的场地，确定标定的路线，软硬件的准备。因为我们需要提供一个外参的初值（重要！），有两种方式，第一是直接把外参初值写在雷达中然后采数据，这样获得的点云数据直接是带有外参初值的，程序中使用的初值提供单位矩阵即可；第二是把雷达中的外参清空后采数据，这样点云数据是雷达自身坐标系下的，然后在程序中对应的地方提供初值外参矩阵即可，这里外参初值获取可以用机械设计参考值或者手工测量值。

• 方式一：在采集数据前提供外参初值

  • 在雷达中设置外参初值：在进行数据采集之前，直接将外参初值设置到雷达中。也就是说，雷达在采集点云数据时，点云数据已经被转换到了一个参考坐标系下，而这个参考坐标系与其他传感器的相对位置已经由外参初值定义。

  • 使用单位矩阵作为程序中的初值：因为点云数据已经带有外参初值（即转换到了相应的坐标系下），在程序中不再需要提供额外的外参初值。此时，程序中可以直接使用单位矩阵作为外参初值，表示传感器已经在同一坐标系下。

• 方式二：在采集数据后提供外参初值

  • 清空雷达中的外参：在数据采集前，不在雷达中设置任何外参（即清空外参），这样雷达采集的点云数据将保持在它自己的坐标系下，而没有转换到任何其他坐标系。

  • 在程序中提供外参初值：此时，程序需要提供一个外参初值矩阵，这个初值可以通过机械设计参考值或者手工测量获得。该初值将用来将雷达坐标系下的点云数据转换到全局坐标系或者其他传感器的坐标系。

        2.开始录制对应的rosbag或者lvx，注意不同的雷达数据需要保存不同的topic。所以在录制rosbag或者在lvx转rosbag的时候，需要在对应的launch文件（比如录制是livox_hub_rviz.launch；转换是lvx_to_rosbag.launch）里面把 multi_topic 参数设置成1，这样可以把不同的雷达数据保存成不同的topic，或者把一个lvx文件中不同的雷达保存成不同的topic。开始录制后开始缓慢的走一圈标定路线，走完后结束录制即可。

        3.将雷达数据转化成对应的一帧一帧pcd文件（lvx可以通过lvx_to_rosbag.launch转换，参考 GitHub - Livox-SDK/livox_ros_driver: Livox device driver under ros, support Lidar Mid-40, Mid-70, Tele-15, Horizon, Avia. ），注意两个雷达之间需要时间同步，同样名称的pcd文件需要对应同一个时刻的数据。然后根据readme文档里面的要求把对应的pcd文件放在不同的文件夹下然后执行程序即可。

        4.编译，运行代码，输出最终标定结果。

五、标定结果的验证

有两种验证方式可以参考：

        1.两雷达存在重叠区域（overlap）情况，可以在标定结束，参数写入后，用livox viewer观察overlap区域是否存在错层，如果错层较大，说明标定结果错误，建议修改外参初值，确认地图建立的精度以后，重新进行标定和验证。

        2.两雷达无overlap情况，将target雷达的点云使用标定结果外参旋转后，融合到base雷达点云（merge操作）。将融合后的点云存为pcd，运行mapping节点进行建图，观察建图结果。如果建图结果存在错层，说明标定结果错误，建议修改外参初值，确认地图建立的精度以后，重新进行标定和验证。

六、全流程

        1.mapping部分

        使用 Base LiDAR Frames 中的点云数据建图，保存每一帧点云的位姿到 T_Matrix.txt 文件中，点云位姿即 T_map_frame-n 

        2.calibration部分

        ①加载 Target LiDAR Frames 中的 target(frame-n) 坐标系下的点云数据，保存到 pcl::PointCloud<pcl::PointXYZ>::Ptr 类型变量 frames 中；加载 Init_Matrix.txt 文件中的数据，得到 init_guess 即 外参矩阵 T_base_(frame-n)_target(frame-n)。

        ②通过 init_guess 将 target(frame-n) 坐标系下的点云数据 frames 转移为 base_(frame-n) 坐标系下点云数据 trans_output_cloud。

        ③使用 mapping 部分得到的 T_map_frame-n 将 base_(frame-n) 坐标系下点云数据 trans_output_cloud 转移为 map 坐标系下的点云数据 final_output_cloud。

        ④从 map 中找到Target LiDAR Frames 中当前帧点云在 map下的近邻点组成的点云 neighbors_L 。

        ⑤使用 T_Matrix_Inverse 即 T_frame-n_map 将 map 坐标系下的点云 neighbors_L 转移为 base_(frame-n) 坐标系下的点云数据 neighbors_trans。

        ⑥设置 gicp 的源点云和目标点云，匹配得到准确的 Tiny_T 即外参矩阵 T_base_(frame-n)_target(frame-n)。

             gicp.InputSource(trans_output_cloud)  源点云

             gicp.InputTarget(neighbors_trans)  目标点云

        ⑦获取 GICP 的 Fitness，fitness 的含义参考 pcl和open3d中的fitness参数的区别 。如果GICP的配准得分（FitnessScore）大于1，说明配准效果不好。在这种情况下，程序跳过这次迭代，并且将 init_guess 重置为初始值 init_guess_0，以避免使用不准确的结果；当 GICP 得分低于1 时，说明配准效果好，此时更新外参 init_guess = Tiny_T * init_guess（为什么这么更新？）；当 GICP 得分低于 0.1 时，说明配准效果非常好，记录 id, fitness, x, y, z, roll, pitch, yaw 到 calib_data.txt 文件中。

        3.数据获取

        rosrun pcl_ros bag_to_pcd 命令用于将 ROS bag 文件中的每一帧点云数据（只适用于sensor_msgs/PointCloud2类型点云）提取并保存为 PCD 格式的文件，该命令会将pcd文件以时间戳命名。

• 当 Mid-360 录制的 bag 包中雷达点云的类型为 livox_ros_driver2/CustomMsg时无法处理，此时对 ros 中 bag_to_pcd.cpp 源文件进行修改，使其适用于 livox_ros_driver/CustomMsg 类型点云。

rosrun pcl_ros bag_to_pcd <*.bag> <topic> <output_directory>
# <*.bag>：指定你要处理的 ROS bag 文件。可以使用通配符（如 *.bag）来选择多个 bag 文件。
# <topic>：指定要从 bag 文件中提取的点云数据的主题（topic）。
# <output_directory>：指定输出 PCD 文件的目录，程序会将提取的点云数据保存到该目录下。

        bag_to_pcd.cpp 源代码见pcl_ros: bag_to_pcd.cpp Source File ，bag_to_pcd.cpp 经过修改后如下：

 #include <boost/filesystem.hpp>
 #include <rosbag/bag.h>
 #include <rosbag/view.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include "pcl_ros/transforms.h"
 #include <tf/transform_listener.h>
 #include <tf/transform_broadcaster.h>

 #include "livox_ros_driver2/CustomMsg.h"
  
 typedef sensor_msgs::PointCloud2 PointCloud;
 typedef PointCloud::Ptr PointCloudPtr;
 typedef PointCloud::ConstPtr PointCloudConstPtr;
 
 int i = 100000;

 int main (int argc, char** argv)
 {
   ros::init (argc, argv, "bag_to_pcd");
   if (argc < 4)
   {
     std::cerr << "Syntax is: " << argv[0] << " <file_in.bag> <topic> <output_directory> [<target_frame>]" << std::endl;
     std::cerr << "Example: " << argv[0] << " data.bag /laser_tilt_cloud ./pointclouds /base_link" << std::endl;
     return (-1);
   }
  
   // TF
   tf::TransformListener tf_listener;
   tf::TransformBroadcaster tf_broadcaster;
  
   rosbag::Bag bag;
   rosbag::View view;
   rosbag::View::iterator view_it;
  
   try
   {
     // 打开指定的.bag文件，读取模式为只读
     bag.open (argv[1], rosbag::bagmode::Read);
   } 
   catch (const rosbag::BagException&) 
   {
     std::cerr << "Error opening file " << argv[1] << std::endl;
     return (-1);
   }
  
   // check that target topic exists in the bag file:
   // 检查ROS Bag文件中是否包含指定的topic，且该topic的数据类型为sensor_msgs/PointCloud2
   rosbag::View topic_list_view(bag); //透过rosbag::View对象来创建对ROS Bag文件的视图，该对象可以用来查询Bag文件中的各种话题（topic）
   std::string target_topic;
   std::map<std::string, std::string> topic_list; //保存Bag文件中所有话题及其对应的数据类型。键是话题的名称，值是话题的数据类型
   for(rosbag::ConnectionInfo const *ci: topic_list_view.getConnections() ) // getConnections()返回所有话题的连接信息列表。每个话题连接信息（ConnectionInfo）包含话题名称和数据类型。
   {
       topic_list[ci->topic] = ci->datatype;
       if (ci->topic == argv[2])
       {
                   std::cout <<"ci->datatype: " << ci->datatype << std::endl;
                   if (ci->datatype == std::string("sensor_msgs/PointCloud2") || ci->datatype == std::string("livox_ros_driver/CustomMsg"))
                   {
                           target_topic = std::string (argv[2]);
                           view.addQuery (bag, rosbag::TopicQuery (target_topic)); //将用户指定的目标话题添加到ROS Bag文件的查询视图（rosbag::View）中，指定该话题为后续要读取的内容。
                   }
                   else
                   {
                           std::cerr << "Provided topic '" << argv[2] << "' is in the bag file, but is not of type sensor_msgs/PointCloud2 (type: " << ci->datatype << ")" << std::endl;
                   }
       }
   }
   if (target_topic.empty())
   {
       std::cerr << "Could not find a sensor_msgs/PointCloud2 type on topic '" << argv[2] << "' in bag file " << argv[1] << std::endl;
       std::cerr << "Topics found in the bag file:" << std::endl;
       for (std::map<std::string, std::string>::iterator it=topic_list.begin(); it!=topic_list.end(); ++it)
           std::cout << "    " << it->first << " (" << it->second << ")" << std::endl;
       return (-1);
   }
  
   view.addQuery (bag, rosbag::TypeQuery ("tf/tfMessage"));
   view.addQuery (bag, rosbag::TypeQuery ("tf2_msgs/TFMessage"));
   view_it = view.begin ();

   // 创建输出目录
   std::string output_dir = std::string (argv[3]);
   boost::filesystem::path outpath (output_dir);
   if (!boost::filesystem::exists (outpath))
   {
     if (!boost::filesystem::create_directories (outpath))
     {
       std::cerr << "Error creating directory " << output_dir << std::endl;
       return (-1);
     }
     std::cerr << "Creating directory " << output_dir << std::endl;
   }
  
   // Add the PointCloud2 handler
   std::cerr << "Saving recorded sensor_msgs::PointCloud2/livox_ros_driver::CustomMsg messages on topic " << target_topic << " to " << output_dir << std::endl;
  
   PointCloud cloud_t;
   ros::Duration r (0.001);
   // Loop over the whole bag file
   while (view_it != view.end ())
   {
     // Handle TF messages first
     // 如果当前消息是TF消息（tf::tfMessage），则通过tf_broadcaster.sendTransform()发布该TF变换。
     tf::tfMessage::ConstPtr tf = view_it->instantiate<tf::tfMessage> ();
     if (tf != NULL)
     {
       tf_broadcaster.sendTransform (tf->transforms);
       ros::spinOnce ();
       r.sleep ();
     }
     else
     {
       PointCloudConstPtr cloud;
       // 思路：先判断是不是 livox 点云，在判断是不是 ros 点云，如果都不是则跳过当前迭代
       livox_ros_driver2::CustomMsgConstPtr livox_cloud = view_it->instantiate<livox_ros_driver2::CustomMsg> ();
       if(livox_cloud != NULL){
         pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
         for(size_t i = 0; i < livox_cloud->point_num; ++i){
            pcl::PointXYZ pt;
            pt.x = livox_cloud->points[i].x;
            pt.y = livox_cloud->points[i].y;
            pt.z = livox_cloud->points[i].z;

            pcl_cloud.push_back(pt);
         }
         sensor_msgs::PointCloud2 pcl_ros_cloud;
         pcl::toROSMsg(pcl_cloud, pcl_ros_cloud);
         pcl_ros_cloud.header = livox_cloud->header;
         cloud = boost::make_shared<const sensor_msgs::PointCloud2>(pcl_ros_cloud);
       }
       else{
         // Get the PointCloud2 message
         // instantiate<PointCloud>() 方法尝试将当前消息转换为类型 sensor_msgs::PointCloud2，如果转换成功，则 cloud 会指向该消息；如果不是，则返回 NULL。
         cloud = view_it->instantiate<PointCloud> ();
         // 如果消息为空（cloud == NULL），则跳过当前迭代
         if (cloud == NULL)
         {
           ++view_it;
           continue;
         }
       }
       // If a target_frame was specified
       // 坐标变换（如果指定了目标坐标系）
       if(argc > 4)
       {
         // Transform it
         if (!pcl_ros::transformPointCloud (argv[4], *cloud, cloud_t, tf_listener))
         {
          ++view_it;
          continue;
         }
       }
       else
       {
         // Else, don't transform it
         cloud_t = *cloud;
       }

       std::cerr << "Got " << cloud_t.width * cloud_t.height << " data points in frame " << cloud_t.header.frame_id << " on topic " << view_it->getTopic() << " with the following fields: " << pcl::getFieldsList (cloud_t) << std::endl;
       
       // 生成点云保存路径，文件名基于点云的时间戳cloud_t.header.stamp
      //  std::stringstream ss;
      //  ss << output_dir << "/" << cloud_t.header.stamp << ".pcd";
      //  std::cerr << "Data saved to " << ss.str () << std::endl;

       // 生成点云保存路径，文件名为 100000.pcd 依次递增
       std::stringstream ss;
       ss << output_dir << "/" << i << ".pcd";
       ++i;
       std::cerr << "Data saved to " << ss.str () << std::endl;

       // 保存为 .pcd 文件
       pcl::io::savePCDFile (ss.str (), cloud_t, Eigen::Vector4f::Zero (),
                             Eigen::Quaternionf::Identity (), true);
     }
     // Increment the iterator
     ++view_it;
   }
  
   return (0);
 }

        编译完成后，使用如下命令得到每一帧点云：

rosrun livox_2_pcd my_node /home/wu/Project/LivoxCloud_2_pcd_ws/src/bag/scans.bag /livox/lidar /home/wu/Project/LivoxCloud_2_pcd_ws/src/data

        结果如下，转移成功

• 当 Mid-360 录制的 bag 包中雷达点云的类型为 sensor_msgs/PointCloud2 时，需要对文件进行重命名。重命名脚本如下：

#!/usr/bin/env python
import os
import shutil

def rename_and_copy_pcd_files(source_path, target_path):
    # 获取源文件夹中的所有 .pcd 文件
    files = [f for f in os.listdir(source_path) if f.endswith('.pcd')]
    
    # 按文件名（时间戳）排序
    files.sort()

    # 确保目标路径存在
    if not os.path.exists(target_path):
        os.makedirs(target_path)

    # 重命名并复制文件
    start_num = 100000
    for i, file in enumerate(files):
        old_file_path = os.path.join(source_path, file)
        new_file_name = f"{start_num + i}.pcd"
        new_file_path = os.path.join(target_path, new_file_name)
        
        # 复制并重命名文件
        shutil.copy(old_file_path, new_file_path)
        print(f"Copied and Renamed: {old_file_path} -> {new_file_path}")

# 设置源文件夹和目标文件夹路径
source_path = "/media/wu/"
target_path = "/media/wu/"
rename_and_copy_pcd_files(source_path, target_path)

        转移完成后将 pcd 文件移动到 Base_LiDAR_Frames 和 Target-LiDAR-Frames 中，继续进行多雷达的标定。

七、使用 LivoxViewer2 手动标定

        打开 LivoxViewer2 软件，为了便于手动标定，首先将 FramTime 调整为 1000ms。

        打开外参标定工具，调整 r, p, y, x, y, z，使多个雷达的点云对齐。

参考：       

        多激光雷达外参自动标定算法开源（Livox）

        镭神ch32外参标定&&Livox_automatic_calibration（Livox）

        激光感知算法--多激光雷达的静态标定

        多激光雷达标定multi_LiDAR_calibration