1 基于平面的约束2D激光雷达和相机的联合标定(2D Laser and Camera Calibration )原理
这是旷视
做的一个关于2D激光雷达和相机的联合标定算法
,在看这个标定算法之前,你可以先看下关于相机的内参标定原理
该方法基于 ROS 的单线激光和相机外参数自动标定代码。标定原理如下图所示,相机通过二维码估计标定板平面在相机坐标系下的平面方程,由于激光点云落在平面上,将点云通过激光坐标系到相机坐标系的外参数
T
c
l
T_{cl}
Tcl 转换到相机坐标系,构建点到平面的距离
作为误差
,使用非线性最小二乘
进行求解。
- 基于平面约束的2D激光雷达和相机标定原理:https://zhuanlan.zhihu.com/p/137501892
- 基于平面约束的2D激光雷达和相机标定原理:https://heyijia.blog.csdn.net/article/details/85000943
- 论文内容:https://blog.csdn.net/Zkangsen/article/details/95224935
- 对CamLaserCalibraTool修改的版本代码:https://blog.csdn.net/HERO_CJN/article/details/87644816
- 论文:Extrinsic Calibration of a Camera and Laser Range Finder (improves camera calibration):http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.80.7118&rep=rep1&type=pdf
2 CamLaserCalibraTool标定代码使用
- 标定代码:https://github.com/MegviiRobot/CamLaserCalibraTool
- 另外一个修改的版本博客:https://blog.csdn.net/HERO_CJN/article/details/87644816
- 修改版本代码:https://github.com/cjn0608/CamLaserCalibraTool
下面是具体 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:https://github.com/MegviiRobot/CamLaserCalibraTool/blob/master/doc/april_6x6_80x80cm_A0.pdf
- apriltag.pdf标定板图片下载:https://github.com/MegviiRobot/CamLaserCalibraTool/blob/master/doc/apriltags1-20.pdf
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激光雷达和相机标定的测试包文件
,下载地址:https://drive.google.com/file/d/1HHwLWwtUrqIOWTG7QFNbwzq9Wq_Ezk7l/view
作者录制的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包上标定的很好,但是我自己实际录制的包怎么测试结果都很差,不知道是我自己晃动的标定板的方式有问题,还是什么问题,难受香菇
!
参考:https://yongqi.blog.csdn.net/article/details/105154394 # 相机的内参标定原理