本文已参与「新人创作礼」活动，一起开启掘金创作之路。

💐 兄dei，点个赞呗！ 💐

🍀 兄dei，你的随手一赞，就是对我莫大的鼓励！ 🍀

🌺 兄dei，每日一赞，发大财，行大运哦！ 🌺

1 基于平面的约束2D激光雷达和相机的联合标定（2D Laser and Camera Calibration ）原理

这是旷视做的一个关于2D激光雷达和相机的联合标定算法，在看这个标定算法之前，你可以先看下关于相机的内参标定原理

该方法基于 ROS 的单线激光和相机外参数自动标定代码。标定原理如下图所示，相机通过二维码估计标定板平面在相机坐标系下的平面方程，由于激光点云落在平面上，将点云通过激光坐标系到相机坐标系的外参数 $T_{cl}$ 转换到相机坐标系，构建点到平面的距离作为误差，使用非线性最小二乘进行求解。

• 基于平面约束的2D激光雷达和相机标定原理：zhuanlan.zhihu.com/p/137501892
• 基于平面约束的2D激光雷达和相机标定原理：heyijia.blog.csdn.net/article/det…
• 论文内容：blog.csdn.net/Zkangsen/ar…
• 对CamLaserCalibraTool修改的版本代码：blog.csdn.net/HERO_CJN/ar…
• 论文：Extrinsic Calibration of a Camera and Laser Range Finder (improves camera calibration)：citeseerx.ist.psu.edu/viewdoc/dow…

2 CamLaserCalibraTool标定代码使用

• 标定代码：github.com/MegviiRobot…
• 另外一个修改的版本博客：blog.csdn.net/HERO_CJN/ar…
• 修改版本代码：github.com/cjn0608/Cam…

下面是具体 CamLaserCalibraTool项目代码的具体使用流程：

2.1 下载编译项目

按照如下流程进行编译即可：

mkdir  -p  LaserCameraCal_ws/src
cd LaserCameraCal_ws/src
git clone https://github.com/MegviiRobot/CamLaserCalibraTool
cd ../..
catkin_make -DCMAKE_BUILD_TYPE=Release
source devel/setup.bash 

注意：

如果你开启一个新的终端，要在终端中重新source devel/setup.bash 一下，否则会报错！

2.2 运行仿真数据，先对工具进行简单测试

强烈建议: 先用仿真数据试试这个标定系统，系统的可观性问题在仿真代码里都能找到验证，这样就能指导你如何采集数据。

1、编译准备好第一步的环境之后，执行如下的命令运行仿真数据：

rosrun lasercamcal_ros simulation_lasercamcal_node

2、运行结果如下：

root@zhihui-mint:~/dataset/CamLaserCalibraTool_ws# ls
build  devel  src
root@zhihui-mint:~/dataset/CamLaserCalibraTool_ws# rosrun lasercamcal_ros simulation_lasercamcal_node
============= Ground Truth of Rotation and translation: Rlc, tlc =============== 
 0  0  1
-1  0  0
 0 -1  0
0.1
0.2
0.3
============= End =============== 
obs size: 50

Solver Summary (v 1.14.0-eigen-(3.2.92)-lapack-suitesparse-(4.4.6)-cxsparse-(3.1.4)-eigensparse-openmp-no_tbb)

                                     Original                  Reduced
Parameter blocks                            1                        1
Parameters                                  7                        7
Effective parameters                        6                        6
Residual blocks                          5529                     5529
Residuals                                5529                     5529

Minimizer                        TRUST_REGION

Dense linear algebra library            EIGEN
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver                        DENSE_QR                 DENSE_QR
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                        1

Cost:
Initial                          4.593626e-01
Final                            0.000000e+00
Change                           4.593626e-01

Minimizer iterations                       10
Successful steps                           10
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                         0.000357

  Residual only evaluation           0.004045 (9)
  Jacobian & residual evaluation     0.010192 (10)
  Linear solver                      0.002103 (9)
Minimizer                            0.039448

Postprocessor                        0.000062
Total                                0.039867

Termination:                      CONVERGENCE (Gradient tolerance reached. Gradient max norm: 3.063522e-13 <= 1.000000e-10)

----- H singular values--------:
 216.067
 42.1002
 34.8389
 18.0096
 2.64307
0.526374

recover chi2: 1.29318e-27

----- Transform from Camera to Laser Tlc is: -----

 6.10623e-16 -3.10862e-15            1          0.1
          -1  6.10623e-16  3.33067e-16          0.2
-3.33067e-16           -1 -2.83107e-15          0.3
           0           -0           -0            1

----- Transform from Camera to Laser, euler angles and translations are: -----

   roll(rad): -1.5708 pitch(rad): 4.996e-16 yaw(rad): -1.5708
or roll(deg): -90 pitch(deg): 2.8625e-14 yaw(deg): -90
       tx(m): 0.1  ty(m): 0.2   tz(m): 0.3

2.3 录制自己的2D激光雷达和相机图像数据的bag包

2.3.1 准备标定板

作者给出了两种类型的标定板：

• kalibr_tag.pdf：github.com/MegviiRobot…
• apriltag.pdf标定板图片下载：github.com/MegviiRobot…

1、april_6x6_80x80cm_A0标定板

注意：

在这个标定april_6x6_80x80cm_A0图片的左下角有一行字体：6x6 tags, size=8.8cm and spacing=2.64cm，下面说明一下具体含义：

• 6x6 tags：表示这个标定图片中有7x7，即7行和7列的大的正方形，之所以说是6x6是因为标定的时候取的是内部正方形角点的交点，如果还不明白看下图就可以一目了然了，下图就是一个9x6的棋方格，虽然它的正方形数是10x7的！
• size=8.8cm：表示红色框正方形的边长
• spacing=2.64cm：表示绿色框正方形的边长

当然，你在制作自己标定板的时候并不一定要求打印和上面一样大小的标定板，比如你是在A4纸上打印的，你只需要用尺子测量一下tag size和tag spacing的具体长度即可！我使用的标定板是april_6x6_80x80cm_A0，即tag_size=8.8cm， tag_spacing=2.64cm，这是我们公司在外面专门定制的！

2、apriltags1-20标定板

2.3.2 准备录制激光雷达和相机的图像的bag包数据

做好准备工作，启动你的2D激光雷达、相机驱动以及rviz工具

1、查看启动后2D激光雷达、相机的topic

• /LiDAR/LD06：2D激光雷达的topic用的是乐动机器人的2D激光雷达）
• /usb_cam/image_raw：相机的topic

2、开始录制数据

rosbag record /LiDAR/LD06 /usb_cam/image_raw

• 关于如何晃动标定版，作者给出了一个示例视频（要破墙去Google Driver观看）

• 作者也给出了一个它自己录制的2D激光雷达和相机标定的测试包文件，下载地址：drive.google.com/file/d/1HHw…

作者录制的bag的info：

root@zhihui-mint:~/dataset/hyj_cali_laser-cam# rosbag info cali_cam_laser6.bag
path:        cali_cam_laser6.bag
version:     2.0
duration:    50.9s
start:       Oct 29 2019 12:56:34.75 (1572353794.75)
end:         Oct 29 2019 12:57:25.63 (1572353845.63)
size:        991.3 MB
messages:    2291
compression: none [764/764 chunks]
types:       sensor_msgs/Image     [060021388200f6f0f447d0fcd9c64743]
                   sensor_msgs/LaserScan [90c7ef2dc6895d81024acba2ac42f369]
topics:      /camera/fisheye1/image_raw   1527 msgs    : sensor_msgs/Image    
                  /scan                                                       764 msgs    : sensor_msgs/LaserScan
root@zhihui-mint:~/dataset/hyj_cali_laser-cam# 

可以看出作者也是没有对录制的包直接做数据时间同步的，相机的帧率大概是2D激光雷达的二倍！

2.4 修改配置文件

在开始标定之前，需要现在配置文件中修改：

• 保存数据和结果的路径
• april标定板的尺寸
• 2D激光雷达和相机的topic
• 相机的分辨率
• 相机的内参和畸变系数

1、下面是作者自己录制的2D激光雷达和相机标定的测试包文件中calibra_config_fisheye_hyj.yaml 的内容下载地址

root@zhihui-mint:~/dataset/CamLaserCalibraTool_ws/src/CamLaserCalibraTool/config# cat calibra_config_fisheye_hyj.yaml 
%YAML:1.0

#common parameters
savePath: "/root/dataset/hyj_cali_laser-cam/"
bag_path: "/root/dataset/hyj_cali_laser-cam/cali_cam_laser6.bag"

scan_topic_name: "/scan"
img_topic_name: "/camera/fisheye1/image_raw"

# tag info
tag_type: 1 # 1: kalibr_tag, 2: apriltag
tag_size: 0.06 # tag size, unit meter
tag_spacing: 0.3 # tag spacing, only used in kalibr_tag. For details, please see kalibr tag description.
black_border: 2 # if you use kalibr_tag black_boarder = 2; if you use apriltag black_boarder = 1
 
#camera calibration 
model_type: KANNALA_BRANDT
camera_name: cam0
image_width: 848
image_height: 800

projection_parameters:
   mu: 288.03113837228807
   mv: 288.2964766776069
   u0: 420.3999640553491
   v0: 395.05186980787374
   k2: 0.003598587568152557
   k3: 0.01853991048490219
   k4: -0.004391882415095654
   k5: -0.009998750559241121

root@zhihui-mint:~/dataset/CamLaserCalibraTool_ws/src/CamLaserCalibraTool/config# 

2、我自己根据实际情况修改为如下：

root@zhihui-mint:~/dataset/CamLaserCalibraTool_ws/src/CamLaserCalibraTool/config# cat calibra_config_fisheye.yaml 
%YAML:1.0

#common parameters
savePath: "/root/dataset/shl_cali_laser-cam/"
#bag_path: "/root/dataset/shl_cali_laser-cam/2021-04-10-10-37-45.bag"


#bag_path: "/root/dataset/shl_cali_laser-cam/2021-04-09-09-45-13_cut.bag"  # used

#bag_path: "/root/dataset/shl_cali_laser-cam/2021-04-10-10-37-45.bag"
bag_path: "/root/dataset/shl_cali_laser-cam/2021-04-10-10-37-45_cut.bag"

scan_topic_name: "/LiDAR/LD06"
img_topic_name: "/usb_cam/image_raw"

# tag info
tag_type: 1 # 1: kalibr_tag, 2: apriltag
tag_size: 0.088 # tag size, unit meter
tag_spacing: 0.3 # tag spacing, only used in kalibr_tag. For details, please see kalibr tag description.
black_border: 2 # if you use kalibr_tag black_boarder = 2; if you use apriltag black_boarder = 1
 
#camera calibration 
model_type: KANNALA_BRANDT
camera_name: cam0
image_width: 1280
image_height: 720

projection_parameters:
   mu: 642.0576
   mv: 643.79173
   u0: 639.813
   v0: 377.29326
   k2: -0.356689
   k3: 0.099261
   k4: -0.001743
   k5: 0.002325

root@zhihui-mint:~/dataset/CamLaserCalibraTool_ws/src/CamLaserCalibraTool/config# 

注意：

这里配置文件中的tag_spacing=0.3并不是tag_spacing的实际距离大小，而是比例：8.8x0.3=2.64，因此这里的tag_spacing是一个固定值为0.3，不用修改！！！

如下是代码中对tag_spacing的体现，tag_spacing_sz就是一个大正方形的边长+一个小正方形状的边长

• 小正方形的边长 tag_spacing = 大正方形的边长tag_size x 0.3

2.5 运行kalibr检测二维码

1、运行命令

cd LaserCameraCal_ws
source devel/setup.bash
roslaunch lasercamcal_ros kalibra_apriltag.launch 

2、运行过程中的输出内容

root@zhihui-mint:~/dataset/CamLaserCalibraTool_ws# roslaunch lasercamcal_ros kalibra_apriltag.launch 
... logging to /root/.ros/log/1018130a-9bff-11eb-902e-e0d55e449b44/roslaunch-zhihui-mint-3858.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://zhihui-mint:37679/

SUMMARY
========

PARAMETERS
 * /kalibra_detect_node/config_file: /root/dataset/Cam...
 * /kalibra_detect_node/image_transport: compressed
 * /kalibra_detect_node/tag_family: 36h11
 * /rosdistro: kinetic
 * /rosversion: 1.12.14

NODES
  /
    kalibra_detect_node (lasercamcal_ros/kalibra_detect_node)

ROS_MASTER_URI=http://localhost:11311

/opt/ros/kinetic/lib/python2.7/dist-packages/roslib/packages.py:451: UnicodeWarning: Unicode equal comparison failed to convert both arguments to Unicode - interpreting them as being unequal
  if resource_name in files:
process[kalibra_detect_node-1]: started with pid [3867]
config_file
[ INFO] [1618309340.312961830]: Loaded config_file: /root/dataset/CamLaserCalibraTool_ws/src/CamLaserCalibraTool/config/calibra_config_fisheye.yaml
config_file
[ INFO] [1618309340.317754866]: Loaded config_file: /root/dataset/CamLaserCalibraTool_ws/src/CamLaserCalibraTool/config/calibra_config_fisheye.yaml
tag_size: 0.06 type: 1
[ INFO] [1618309340.391768939]: LOAD KANNALA BRANDT CAMERA!
tag size: 0.06 tag space: 0.3
twc: 0.227064 0.222049 0.305548
tag size: 0.06 tag space: 0.3
twc: 0.232086 0.220396 0.305276
tag size: 0.06 tag space: 0.3
......
tag size: 0.06 tag space: 0.3
twc: 0.221955 0.179013 0.48799
tag size: 0.06 tag space: 0.3
twc: 0.235106 0.187759 0.491983
tag size: 0.06 tag space: 0.3
twc: 0.250324 0.196471 0.495506
tag size: 0.06 tag space: 0.3
twc: 0.266691 0.20656 0.498652

3、运行过程中每一帧数据标定板二维码检测结果如下：

4、检测完之后会把检测到每一帧中的april的信息保存到/root/dataset/hyj_cali_laser-cam/apriltag_pose.txt文件中，作者录制的数据包有1527 msgs，最终也是每一帧都检测到了，我自己录制的数据包总会有10帧左右检测不到！

2.6 再运行激光视觉外参数标定代码

1、会自动检测激光在标定板上的线段，并完成标定和保存结果。

roslaunch lasercamcal_ros calibra_offline.launch

激光线条的自动获取如下图所示，其中红色部分为激光检测到的标定板线段，请注意保持激光前方是空旷区域：

2、标定完之后会在终端中输出标定结果：

3、标定结果会保存到result.yaml文件中

root@zhihui-mint:~/dataset/hyj_cali_laser-cam# cat result.yaml 
%YAML:1.0
---
extrinsicTlc: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ -1.6196332831555606e-02, 3.4701504873445888e-01,
       9.3771969945961475e-01, -2.7549345564795952e-02,
       -9.9956040125119339e-01, -2.8911975545042802e-02,
       -6.5652053003503732e-03, 5.0841322379481788e-02,
       2.4833103981628583e-02, -9.3741381130315315e-01,
       3.4733076933196366e-01, 1.1213851989733120e-01, 0., 0., 0., 1. ]
RollPitchYaw: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [ -1.2159589096592847e+00, -2.4835657049251735e-02,
       -1.5869983647855737e+00 ]
txtytz: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [ -2.7549345564795952e-02, 5.0841322379481788e-02,
       1.1213851989733120e-01 ]
root@zhihui-mint:~/dataset/hyj_cali_laser-cam# 

2.7 验证结果，根据标定的外参把2D激光雷达数据投影到图像上

1、运行验证，运行 debug 工具，看激光光条和图像是否对齐的比较好。

roslaunch lasercamcal_ros debug.launch

2、在另外一个终端，播放测试的bag包文件

rosbag play data.bag

3、投影结果

我使用这个工具在作者给的测试bag包上标定的很好，但是我自己实际录制的包怎么测试结果都很差，不知道是我自己晃动的标定板的方式有问题，还是什么问题，难受香菇！

参考：yongqi.blog.csdn.net/article/det… # 相机的内参标定原理