R3LIVE项目实战(3) —双目相机与激光雷达 lidar_camera_calib联合标定

3.1 lidar_camera_calib简介

3.2 环境准备

3.3 编译

源码地址：https://github.com/hku-mars/livox_camera_calib

注意： lidar_camera_calib和livox_camera_lidar_calibration为两个不同的标定程序，两个标定代码都是基于livox的激光雷达，代码重合的较高，应该为同一团队成果, lidar_camera_calib标定的自标定过程较为简单，精度也随之较低。本文的介绍 lidar_camera_calib，同时也借鉴了livox_camera_lidar_calibration的( 投影点云到照片 projectCloud.cpp和点云着色color_lidar_display.cpp代码)

3.1 lidar_camera_calib简介

lidar_camera_calib是一个在无目标环境中，用于高分辨率LiDAR（例如Livox）和摄像机之间准确的外部标定工具。我们的算法可以在室内和室外场景中运行，并且只需要场景中的边缘信息。如果场景适合，我们可以实现类似于或甚至超过基于目标的方法的像素级准确度。

我们使用标定的外部参数给点云上色，并与实际图像进行比较。A和C是点云的局部放大视图。B和D是与A和C中的点云对应的摄像机图像的部分。

lidar_camera_calib支持多场景标定（更准确和稳定）相关论文相关论文可以在arxiv上找到：像素级无目标环境下高分辨率LiDAR和摄像机的外部自标定。

3.2 环境准备

(1) Ubuntu和ROS

需要Ubuntu 64位16.04或18.04。ROS Kinetic或Melodic。ROS的安装及其额外的ROS包

sudo apt-get install ros-XXX-cv-bridge ros-xxx-pcl-conversions

(2) Eigen

// 安装
cd eigen-git-mirror
mkdir build
cd build
cmake ..
sudo make install

// 安装后 头文件安装在/usr/local/include/eigen3/

// 移动头文件
sudo cp -r /usr/local/include/eigen3/Eigen /usr/local/include

备注：在很多程序中 include 时经常使用 #include <Eigen/Dense> 而不是使用 #include <eigen3/Eigen/Dense> 所以要做下处理

(3) Ceres Solver

(4) PCL

3.3 编译

克隆代码库并进行catkin_make

cd ~/catkin_ws/src 
git clone https://github.com/hku-mars/livox_camera_calib.git 
cd ../ 
catkin_make 
source ~/catkin_ws/devel/setup.bash

编译报错1： 编译时会出现如下报错，报错原因是ceres版本太高的原因，将ceres2.1.0改为ceres2.2.0后运行正常

/home/zjlab/perception/Calib/calib_ws/src/livox_camera_calib-master/src/lidar_camera_multi_calib.cpp:312:14: error: ‘LocalParameterization’ is not a member of ‘ceres’
  312 |       ceres::LocalParameterization *q_parameterization =
      |              ^~~~~~~~~~~~~~~~~~~~~
/home/zjlab/perception/Calib/calib_ws/src/livox_camera_calib-master/src/lidar_camera_multi_calib.cpp:312:37: error: ‘q_parameterization’ was not declared in this scope
  312 |       ceres::LocalParameterization *q_parameterization =
      |                                     ^~~~~~~~~~~~~~~~~~
/home/zjlab/perception/Calib/calib_ws/src/livox_camera_calib-master/src/lidar_camera_multi_calib.cpp:313:15: error: expected type-specifier
  313 |           new ceres::EigenQuaternionParameterization();
      |               ^~~~~
/home/zjlab/perception/Calib/calib_ws/src/livox_camera_calib-master/src/lidar_camera_calib.cpp: In function ‘int main(int, char**)’:
/home/zjlab/perception/Calib/calib_ws/src/livox_camera_calib-master/src/lidar_camera_calib.cpp:311:14: error: ‘LocalParameterization’ is not a member of ‘ceres’
  311 |       ceres::LocalParameterization *q_parameterization =
      |              ^~~~~~~~~~~~~~~~~~~~~
/home/zjlab/perception/Calib/calib_ws/src/livox_camera_calib-master/src/lidar_camera_calib.cpp:311:37: error: ‘q_parameterization’ was not declared in this scope
  311 |       ceres::LocalParameterization *q_parameterization =
      |                                     ^~~~~~~~~~~~~~~~~~
/home/zjlab/perception/Calib/calib_ws/src/livox_camera_calib-master/src/lidar_camera_calib.cpp:312:15: error: expected type-specifier
  312 |           new ceres::EigenQuaternionParameterization();

编译报错2：opencv版本冲突，系统自带的opencv4.5.4和ros下的opencv4.2.0冲突。

/usr/bin/ld: warning: libopencv_features2d.so.4.2, needed by /usr/lib/aarch64-linux-gnu/libopencv_calib3d.so.4.2.0, may conflict with libopencv_features2d.so.4.5
/usr/bin/ld: warning: libopencv_imgproc.so.4.5, needed by /usr/lib/aarch64-linux-gnu/libopencv_imgcodecs.so.4.5.4, may conflict with libopencv_imgproc.so.4.2
[ 91%] Built target lidar_camera_calib
/usr/bin/ld: warning: libopencv_features2d.so.4.2, needed by /usr/lib/aarch64-linux-gnu/libopencv_calib3d.so.4.2.0, may conflict with libopencv_features2d.so.4.5
/usr/bin/ld: warning: libopencv_imgproc.so.4.5, needed by /usr/lib/aarch64-linux-gnu/libopencv_imgcodecs.so.4.5.4, may conflict with libopencv_imgproc.so.4.24

修改cv_bridge配置：

ros默认安装的opencv路径在/usr/include,/usr/lib,/usr/share三个目录。从opencv官网源码编译安装的，opencv会默认安装到usr/local下对应的三个子目录。

#if(NOT "include;/usr/include/opencv4 " STREQUAL " ")
#  set(cv_bridge_INCLUDE_DIRS "")
#  set(_include_dirs "include;/usr/include/opencv4")
if(NOT "include;/usr/local/include;/usr/local/include/opencv4" STREQUAL " ")
  set(cv_bridge_INCLUDE_DIRS "")
  set(_include_dirs "include;/usr/local/include;/usr/local/include/opencv4;/usr/include") 

set(libraries "cv_bridge;/usr/lib/aarch64-linux-gnu/libopencv_calib3d.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_dnn.so.4.2.0;........)

3.4 运行数据集

官方提供的数据集可以从OneDrive和BaiduNetDisk（百度网盘）下载。

百度网盘：https://pan.baidu.com/s/1oz3unqsmDnFvBExY5fiBJQ?pwd=i964#list/path=%2F

(1) 单场景标定

将我们的pcd和image文件下载到本地路径，并将calib.yaml文件中的文件路径更改为您的数据路径。然后直接运行：

source ./devel/setup.bash
roslaunch livox_camera_calib calib.launch

将得到以下结果。

make[2]: *** No rule to make target '/usr/lib/aarch64-linux-gnu/libopencv_aruco.so.4.5.4', needed by '/home/zjlab/perception/Calib/calib_ws/devel/lib/livox_camera_calib/bag_to_pcd'.  Stop.
make[1]: *** [CMakeFiles/Makefile2:2054: livox_camera_calib/CMakeFiles/bag_to_pcd.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
make[2]: *** No rule to make target '/usr/lib/aarch64-linux-gnu/libopencv_aruco.so.4.5.4', needed by '/home/zjlab/perception/Calib/calib_ws/devel/lib/livox_camera_calib/lidar_camera_calib'.  Stop.
make[1]: *** [CMakeFiles/Makefile2:488: livox_camera_calib/CMakeFiles/lidar_camera_calib.dir/all] Error 2
make[2]: *** No rule to make target '/usr/lib/aarch64-linux-gnu/libopencv_aruco.so.4.5.4', needed by '/home/zjlab/perception/Calib/calib_ws/devel/lib/livox_camera_calib/lidar_camera_multi_calib'.  Stop.

运行报错1：Can not load image。

[ERROR] [1689831836.845248880]: Can not load image from /home/zjlab/perception/Calib/calib_ws/src/livox_camera_calib-master/data/calib_dataset/single_scene_calibratio/0.png
[lidar_camera_calib-1] process has died [pid 18393, exit code 255, cmd /home/zjlab/perception/Calib/calib_ws/devel/lib/livox_camera_calib/lidar_camera_calib __name:=lidar_camera_calib __log:=/home/zjlab/.ros/log/d53e9cfa-26ac-11ee-b53d-48b02d3db6e3/lidar_camera_calib-1.log].
log file: /home/zjlab/.ros/log/d53e9cfa-26ac-11ee-b53d-48b02d3db6e3/lidar_camera_calib-1*.log

查看calib.yaml文件中的文件路径是否更改准确，更改后运行正常.

运行报错2：process has died.系统自带的opencv4.5.4和ros下的opencv4.2.0冲突

[lidar_camera_calib-1] process has died [pid 25608, exit code -11, cmd /home/zjlab/perception/Calib/calib_ws/devel/lib/livox_camera_calib/lidar_camera_calib __name:=lidar_camera_calib __log:=/home/zjlab/.ros/log/d53e9cfa-26ac-11ee-b53d-48b02d3db6e3/lidar_camera_calib-1.log].
log file: /home/zjlab/.ros/log/d53e9cfa-26ac-11ee-b53d-48b02d3db6e3/lidar_camera_calib-1*.log

init.png

在指定的yaml指定的文件目录下生成的标定结果如下：

-0.00261333,-0.999901,-0.0138569,0.014096
-0.00334576,0.0138656,-0.999898,0.0574687
0.999991,-0.0025667,-0.00338166,-0.0518364
0,0,0,1

(2) 多场景标定

将我们的pcd和image文件下载到本地路径，并将multi_calib.yaml文件中的文件路径更改为您的数据路径。然后直接运行：

roslaunch livox_camera_calib multi_calib.launch

使用初始外部参数得到的投影图像。（传感器套件：Livox Horizon + MVS相机）

多场景标定的示例。

residual可视化结果

rviz可视化结果

通过初始外参获得的投影图像。（传感器套件：Livox Horizon + MVS相机）

多场景校准的一个例子。通过初始外参获得的投影图像。

粗略校准用于处理不好的外参。

粗略校准后获得的外参获得的投影图像。

最终优化后，我们最终获得了一个精确的外参。

精确校准后获得的外参获得的投影图像。

在指定的yaml指定的文件目录下生成的标定结果如下：

0.00943777,-0.999902,-0.0103278,-0.0507875
-0.0434641,0.00990826,-0.999006,0.0851625
0.99901,0.00987727,-0.0433664,-0.0231513
0,0,0,1

3.5 使用自己的传感器数据标定

3.5.1 采集相机图片和雷达bag数据

(1) 通过按键‘T’采集相机图片，通过下面的脚本实现，脚本为ZED相机自带，在/usr/local/zed/samples/tutorials/tutorial2_image_capture目录下修改。

#include </usr/local/zed/include/sl/Camera.hpp>
#include <iostream>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace sl;

int main(int argc, char **argv) {

    // Create a ZED camera object
    Camera zed;

    // Set configuration parameters
    InitParameters init_parameters;
    init_parameters.camera_resolution = RESOLUTION::HD1080; // Use HD1080 video mode
    init_parameters.camera_fps = 30; // Set fps at 30

    // Open the camera
    auto returned_state = zed.open(init_parameters);
    if (returned_state != ERROR_CODE::SUCCESS) {
        cout << "Error " << returned_state << ", exit program." << endl;
        return EXIT_FAILURE;
    }

    // Capture 50 frames and stop
    // int i = 0;
    int i = 0;
    sl::Mat image;
    cout << "Please 'T' or 't' capture picture, 'E' or 'e' Exit!" << endl;

    while (true){
        
        // Grab an image
        returned_state = zed.grab();
        // A new image is available if grab() returns ERROR_CODE::SUCCESS
        if (returned_state == ERROR_CODE::SUCCESS) {
            
            char key;
            cin >> key;

            if(key == 'E' || key == 'e'){
                break;
            }
            if(key == 'T' || key == 't'){
                
                // Get the left image
                cout << "Capture picture" << to_string(i) << endl;           
                zed.retrieveImage(image, VIEW::LEFT);
                string savePath = "../data/image/" + to_string(i) + ".bmp" ;
                image.write(savePath.c_str());
                
                // Display the image resolution and its acquisition timestamp
                cout<<"Image resolution: "<< image.getWidth()<<"x"<<image.getHeight() <<" || Image timestamp: "<<image.timestamp.data_ns<< endl << endl;
                i++;
            }
            
        }
    }

    // while(i < 10){
    //     returned_state
    // }

    // Close the camera
    zed.close();
    return EXIT_SUCCESS;

(2) 采集雷达bag数据，要通过roslaunch livox_ros_driver livox_lidar_msg.launch命令启动雷达，bag的类型是livox_ros_driver2/CustomMsg，不然bag的格式为sensor_msgs/PointCloud2，不方便后续程序的处理。

roslaunch livox_ros_driver livox_lidar_msg.launch # 启动雷达
# 在根目录下逐级创建用于保存 bag 文件的文件夹
mkdir -p src/livox_camera_lidar_calibration/data/lidar
rosbag record -O src/livox_camera_lidar_calibration/data/lidar/0.bag /livox/lidar # 录制点云，输出到指定文件夹下，便于后续操作，文件名从序号 0 开始标

3.5.2 使用多场景标定

在multi_calib.yaml中更改参数，将图像文件和pcd文件从0到（数据总数-1）进行命名。

[ INFO] [1690958506.262467995]: Loading the rosbag /home/zjlab/perception/Calib/calib_ws/src/livox_camera_calib/mydata/bag/0.bag
[ERROR] [1690958506.437747485]: LOADING BAG FAILED: Bag unindexed
[bag_to_pcd-2] process has died [pid 6316, exit code 255, cmd /home/zjlab/perception/Calib/calib_ws/devel/lib/livox_camera_calib/bag_to_pcd __name:=bag_to_pcd __log:=/home/zjlab/.ros/log/a4eaa69a-30ff-11ee-99c8-48b02d3db6e3/bag_to_pcd-2.log].
log file: /home/zjlab/.ros/log/a4eaa69a-30ff-11ee-99c8-48b02d3db6e3/bag_to_pcd-2*.log

报错：错具体信息为：

rosbag.bag.ROSBagUnindexedException: Unindexed bag

问题解决：

遇到这个问题，我们需要去看一下bag，使用命令：

rosbag info 0.bag

此时会报错：

ERROR bag unindexed: 0.bag. Run rosbag reindex.

依次执行记录的bag：

rosbag reindex 0.bag

等待执行完毕，我们再输入，如果能正常输出，则代表可以正常读入bag了

zjlab@zjlab-zjrobot:~/perception/Calib/calib_ws/src/livox_camera_calib/mydata/bag$ rosbag info 0.bag 
ERROR bag unindexed: 0.bag.  Run rosbag reindex.
zjlab@zjlab-zjrobot:~/perception/Calib/calib_ws/src/livox_camera_calib/mydata/bag$ Run rosbag reindex
bash: Run: command not found
zjlab@zjlab-zjrobot:~/perception/Calib/calib_ws/src/livox_camera_calib/mydata/bag$ rosbag reindex 0.bag 
 0.bag                                       100%             44.1 MB 00:00    
zjlab@zjlab-zjrobot:~/perception/Calib/calib_ws/src/livox_camera_calib/mydata/bag$ rosbag info 0.bag 
path:        0.bag
version:     2.0
duration:    10.5s
start:       Jul 28 2023 16:37:17.00 (1690533438.00)
end:         Jul 28 2023 16:37:28.50 (1690533448.50)
size:        43.7 MB
messages:    106
compression: none [53/53 chunks]
types:       sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181]
topics:      /livox/lidar   106 msgs    : sensor_msgs/PointCloud2

3.5.3 相机内参获取

内参矩阵的格式如下图所示，一般在(0,0);(0,2);(1,1);(1,2)四个位置有对应的值。

在/usr/local/zed/settings目录文件下获取相机内参

[LEFT_CAM_2K]
fx=1055.98
fy=1054.75
cx=1109.22
cy=618.272
k1=-0.0436089
k2=0.0133655
p1=-0.000636837
p2=0.000189454
k3=-0.00610127

报错：相机内参和外参要满足格式要求，不然会报如下的错误。

process[projectCloud-1]: started with pid [25817]
Get the parameters from the launch file
[ INFO] [1691026996.688365859]: Start to load the rosbag /home/zjlab/perception/Calib/calib_ws/src/livox_camera_calib/mydata/bag/0.bag

Can not convert a string to double
[projectCloud-1] process has finished cleanly

解决办法：修改内外参文件，按照要求排列，数字之间要有空格

报错：Failed to load module "canberra-gtk-module"

Gtk-Message: 11:34:35.337: Failed to load module "canberra-gtk-module"
[projectCloud-1] process has finished cleanly
log file: /home/zjlab/.ros/log/83ec06a2-311b-11ee-a1d3-024273a4022a/projectCloud-1*.log
all processes on machine have died, roslaunch will exit

解决办法：重新安装一下吧。

sudo apt-get install libcanberra-gtk-module

3.5.4 运行标定程序

运行后外参结果将会保存在指定目录下

roslaunch  livox_camera_calib multi_calib.launch

3.5.5 验证结果

投影点云到照片和点云着色可以参考livox_camera_lidar_calibration，只需修改launch文件下的路径即可验证。

(1) 投影点云到照片

roslaunch livox_camera_calib projectCloud.launch

(2) 点云着色：

roslaunch livox_camera_calib colorLidar.launch