多相机坐标转换_使用KITTI数据集实现坐标转换

本文的主要内容，是讨论如何将KITTI激光雷达的点云数据，映射到图像中去。

KITTI数据集是怎么采集的

下面这幅图是车的俯视图，可以看到，用到了2个彩色相机，2个灰度相机，1个激光雷达。1个GPS/IMU。

图1 KITTI车俯视图

KITTI标系是怎么定义的

比较值得注意的是，蓝字部分标注的Velodyne激光雷达的坐标系中， X坐标是向前的。也就意味着激光雷达数据中的X数据，给的是物体的距离信息。这点在我的代码中的时候会有体现。

KITTI车传感器坐标定义

KITTI怎么从点云映射到图像？

我看了Kitti官方论文《Vision meets Robotics: The KITTI Dataset》，要想把点云映射到图像，核心的公式就是

图2 点云坐标x映射到图像坐标y

先说说映射流程流程。

首先经过

矩阵映射。经过这一步，点云坐标被映射到了0号相机坐标系里。
然后经过
矩阵映射。这一步我也没太搞懂。比较奇怪的是，不管像映射到哪个编号的相机，似乎都需要左乘

矩阵。而且这个

矩阵看上去仅仅是一个内参矩阵……
最后经过
矩阵映射，被变换到了i号相机所在的图像坐标系。

再说说各部分长什么样。

是点云数据的齐次坐标形式，一个4行1列的列向量，

。
是4*4的矩阵。它并非是平移矩阵，而是标定得到的激光雷达和相机的外参矩阵。包括了旋转矩阵

和平移矩阵

两部分, 如下

= |

是4*4的矩阵，最后一行为

,是从3*3矩阵扩展来的。
是3*4的矩阵。
是最终映射得到的图像坐标，是一个3行1列的列向量，

。为了变换成齐次坐标的形式，需要做一个归一化，除以

。

KITTI怎么获取上述映射矩阵？

公式中：

为点云数据，使用.bin二进制文件储存，可以使用c++读取。里面包括了三维坐标信息和反射率。

对于几个转换矩阵，KITTI提供了三类标定文件，分别叫

calib_cam_to_cam.txt： 相机之间的坐标转换矩阵
calib_imu_to_velo.txt： IMU和激光雷达的坐标转换
calib_velo_to_cam.txt： 相机和激光雷达之间的坐标转换

本文中要将激光雷达的点云数据，映射到图像中去。因此用到的是calib_cam_to_cam.txt和calib_velo_to_cam.txt。

就是calib_cam_to_cam.txt中的P_rect_0i。

是由calib_cam_to_cam.txt的3*3的R_rect_00矩阵扩展一个维度而来。

由calib_velo_to_cam.txt中的R和T，扩展出来的。

在这些标定txt文件中，所有的数据都是按行存储的，也就是P_rect的1-4个数据，是矩阵

的第一行。

怎么显示出映射后融合效果？

通过上述公式，我们已经可以把3维点云数据映射至图像坐标了。可以直接在图像上标记出这些点。

但是，为了更酷炫的显示这个效果，我们可以根据点云的x坐标给这些点上色。用x坐标的原因，也就是刚刚上面提到的因为激光雷达的坐标系x轴是朝前的，实际上坐标x就是物体距离。

一般使用各大编程语言的colormap里的JET色彩就能实现渐变效果，距离远的点是蓝色的，距离近的点是红色的。

映射后的效果图如下：

Kitti官方提供了 MATLAB代码run_demoVelodyne.m。这份代码里，为了显示融合效果，用到了几个tricks：

把x<5的点去掉了。物理意义就是，距离小于5的范围可能处于相机的盲区，拍不到。去掉这些点可以提高速度。
点云中，每5个点取一个点。也是为了提高计算速度。
在实现远距离是蓝色，近距离是红色时，kitti官方代码将距离映射成0-63的整数，每个整数对应一个颜色，实现方式很有趣。

col_idx = round(64*5/velo(i,1));

这里velo(i,1)就是距离。由于他把x<5的点去掉了，所以5/velo(i,1)范围在(0,1]之间。64*5/velo(i,1)就到了(0,64]之间。取整之后，就变成了0-63的整数。

c++代码

依据官方代码，我实现了c++版本的。

https://gitee.com/cuiry/learn3D/tree/master/learnKittigitee.com

里面有1张kitti彩色图像，和对应的点云bin文件、标定txt文件。

代码实现了bin文件和标定txt数据的读取、从点云坐标转换成图像坐标、以及融合效果的显示。

代码运行在Ubuntu18.04，需要opencv4.4.0和PCL库。

参考资料

kitti数据集坐标转换_冯子材的博客-CSDN博客blog.csdn.net

https://github.com/ZouCheng321/fusion_kitti/blob/master/run_demoVelodyne.mgithub.com