单目针孔相机与激光雷达联合标定

1 针孔相机各坐标系与激光雷达坐标系的介绍

1.1 像素坐标系

像素坐标系是选择在图像的左上角位置作为原点的，单位为pixel，（大概是由于在图像转换为矩阵进行索引时，全为正值更方便吧？） 图像坐标系与像素坐标系之间的转换和像元大小有很大的关系，像元pix_x(单位：物理尺寸 mm/像素) 为在x方向上一个pixel所代表的实际物理尺寸（mm）；同理pix_y为在y方向上一个pixel所代表的实际物理尺寸（mm） 。相机标定所得的内参矩阵的fx和fy理论上即为实际的焦距f/pix_x 和f/pix_y。其中1/pix_x = dx ,1/pix_y = dy

1.2 图像坐标系

图像坐标系单位为mm，其坐标原点为图像中点，为摄像机光轴与图像物理坐标系的交点位置。

1.3 相机坐标系

相机坐标系：以摄像机光心为原点（在针孔模型中也就是针孔为光心），z轴与光轴重合也就是z轴指向相机的前方，x轴与y轴的正方向与图像坐标系平行，其中上图中的f为摄像机的焦距。单位m。

1.4 雷达坐标系（世界坐标系）

雷达坐标系：激光雷达坐标系可以描述物体与激光雷达的相对位置，表示为[X, Y, Z]，其中原点为激光雷达几何中心，X轴水平向前，Y轴水平向左，Z轴竖直向上，符合右手坐标系规则。
进行激光雷达与相机的标定，即等价于求解像素坐标系（或者相机坐标系）相对于雷达坐标系的位姿（相反亦可）。
因此需要实现的即 像素坐标系->图像坐标系->相机坐标系->雷达坐标系。

2 各坐标系之间的相互转换

2.1 像素坐标系->图像坐标系

2.2 图像坐标系->相机坐标系

2.3 像素坐标系->雷达坐标系

可以得出，像素坐标系转换为雷达坐标系可以通过内参和外参矩阵来进行逐层的转换。

当然这是第一次做雷达和相机标定时的思路，后来对该思路进行了优化和简化。

本质上从像素坐标系转换至雷达坐标系也只是一个3D到2D点对的一个运动方法。因此，引出了此次标定的主角，PnP算法。PnP算法就是在已知世界坐标系下N个空间点的真实坐标以及这些空间点在图像上的投影，如何计算相机所在世界坐标系下的位姿。 此时大家便可以进行代入，世界坐标系即激光雷达坐标系。 此时的PnP问题显然变成了已知激光坐标系下的真实空间点和这些点对应图像上的投影，来求解相机位于世界坐标系下的位姿。

3 标定工作的进行

3.1 标定板的选择和图像角点提取

使用1m*0.7m每个棋盘格边长大小为0.1m的标定板进行标定工作，（友情提醒，标定板存在正反关系，图示摆放方向为正）。
通过使用opencv的棋盘格角点检测进行标定板角点的提取，得到54组点的像素坐标，存于数组中。

3.2 激光雷达点云角点提取

对点云数据进行处理，分割出整个标定板的点云数据（可以选择景深较大的空间，便于标定板的提取，实在不行手动分割出来，去除噪点数据，由于房间空间有限，只能这样摆放，个人使用cloudcompare软件进行了标定板的手动分割提取点云数据），对分割出的数据进行棋盘格角点的拟合工作，毕竟已经可以得到较为准确的标定板的4个角点在雷达坐标系下的xyz坐标了，本人通过写一个简单的空间线性插值代码完成了标定板中间54组棋盘格角点的拟合工作，以从左到右、从上到下的顺序将其写入txt文本中，便于后续pnp操作时直接读取数据。

至此，得到了联合标定所需的2D像素点和3D空间点，对其进行PnP算法。（PnP算法不详细介绍了）编写脚本将点云数据附着上rgb数据，通过ros节点发布出来，通过rviz显示效果如下：

完结撒花！！！！