在gazebo仿真环境下对相机和激光雷达的标定

相机和激光雷达的标定主要是为了得到两者之间的参数，包括相机的内参和雷达到相机的外参。这样便可以完成点云到图像的投影，从而完成信息融合。
实际上gazebo中这些参数都是真值，是不需要标定的：

• 相机的内参可以从相机发布的消息：/camera/camera_info中获得
• Lidar到相机的外参可以从gazebo中设置的两个传感器之间的位姿得到

以我的小车为例，首先看一下我的gazebo小车图片（忽略掉悬空的合理性）

相机和激光雷达都是朝向小车的前侧，相机高于激光雷达0.3m。
理论上来讲直到这些参数是可以得到雷达到相机的外参的：
雷达坐标系：x向前，y向左，z向上
相机坐标系：x向右，y向下，z向前
根据坐标系之间的旋转平移变换应该可以得到外参（旋转矩阵+平移向量=欧式变换）

因为坐标系转来转去令我头疼，我还使用了Eigen库来求解，就是得不到正确答案（这里的判断依据是我把点云和图像融合为彩色点云，结果颜色没有对上）。所以一气之下选择了标定。

标定过程也是异常坎坷，主要参考了这篇文章、这篇文章。

主要使用了Autoware工具箱，但整个Autoware代码编译器来巨麻烦，环境太多。还好第一篇文章的大佬整理出来了其中必要的部分，并放在了github上。我编译一遍就通过了，真的太感谢了。

gazebo中的标定实际上和现实中的标定没有太大的区别。都是先要录制一个包含有相机和点云消息的包，然后播放出来。只是gazebo中需要自己搭建一个标定的环境，还要有标定板，这里我放出来我用到的world和里面的模型，可供大家使用。
网盘链接：https://pan.baidu.com/s/1H314s6Hn-bY1qFphinnNrg?pwd=2022
提取码：2022
大概长这个样子：（弄这么多灰色的背景是为了让标定程序更快速的检测到我们的标定板。）

具体的操作细节这两篇文章里面都有，主要说一下文章中没有提到注意点：

• gazebo中拖动标定板比较困难，可以选择点击板子，直接在左侧设置pose
• 标定位置一共选择6个，每个位置选择5个姿态，可以参考这里
• 注意标定板转动角度不需要太大，太大更容易产生误差，0.3即可
• 注意在录制包的时候，不仅打开自带的相机内参标定看看是否能找到，还需要打开rviz看看点云中能否看到整个板子，千万不要只看到板子的一半就认为是整个板子
• 相机内参是不需要单独标定的，这个程序可以得到内参和外参
• 得到的外参是相机到雷达的，点云到图像要求个逆

经过标定我得到的参数文件内容为：

%YAML:1.0
---
CameraExtrinsicMat: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ -1.9487212737283599e-03, 1.2995338606212603e-03,
       9.9999725684480878e-01, 2.8479359727213497e-02,
       -9.9999693499389153e-01, 1.5247177575343773e-03,
       -1.9507020743178560e-03, 5.7692333353345825e-03,
       -1.5272485783945355e-03, -9.9999799322173888e-01,
       1.2965586276114660e-03, 2.6759754689266252e-01, 0., 0., 0., 1. ]
CameraMat: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 3.3710092264812880e+02, 0., 3.2016449479134496e+02, 0.,
       3.3730878718418813e+02, 2.3885430457718860e+02, 0., 0., 1. ]
DistCoeff: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [ -1.3310624142978161e-02, 4.3596514912675008e-02,
       -3.0808506470280403e-04, 5.1425393407627498e-04,
       -3.8541526166501847e-02 ]
ImageSize: [ 640, 480 ]
ReprojectionError: 1.3258493620696266e-01

上面的就是外参矩阵(相机到点云)，下面的是内参矩阵。
结合我们的真值可以知道，实际上外参矩阵就是
0 ， 0， 1， 0
-1， 0， 0， 0
0， -1 ， 0， 0.3
0， 0， 0， 1
内参就使用相机消息传出来的。
到此，我们就完成了gazebo方针环境下对相机和激光雷达的标定。

为了验证标定是否正确，可以运行点云和图像融合为彩色点云的代码，这些可以参考我的另一篇文章：
ROS中点云学习（七）：激光点云和图像的融合