【多传感融合】优达学城多传感融合学习笔记（四）——将激光雷达3D点云映射到相机图像（下）

将激光雷达3D点云映射到相机图像（下）——编程实践

KITTI数据集的传感器安装配置

在上一篇笔记中，对激光雷达3D点云映射到相机图像的原理进行了详细分析，下面让我们首先了解一下我们使用的KITTI数据集中的传感器安装配置。在下图中，数据采集车上安装了两个前向摄像头，车顶安装了一个Velodyne 激光雷达和一个惯性测量装置（即IMU，不过本课程中未使用）。

对于KITTI官网下载到的所有数据集合，你都可以找到包含相机内参、外参的校正文件。下面展示了校正文件"calib_velo_to_cam.txt“的部分内容，表明了激光雷达和左摄像头的相对位置关系（适用于我们即将使用的高速公路场景数据）：

calib_time: 15-Mar-2012 11:37:16

R: 7.533745e-03 -9.999714e-01 -6.166020e-04 1.480249e-02 7.280733e-04 -9.998902e-01 9.998621e-01 7.523790e-03 1.480755e-02

T: -4.069766e-03 -7.631618e-02 -2.717806e-01

…

矩阵R和向量T提供给我们了相机的外参。当然，为了完成三维点云到相机图像的映射，我们还需要相机的内参。它们保存在"calib_cam_to_cam.txt"文件中，相关内容如下：

calib_time: 09-Jan-2012 13:57:47

…

R_rect_00: 9.999239e-01 9.837760e-03 -7.445048e-03 -9.869795e-03 9.999421e-01 -4.278459e-03 7.402527e-03 4.351614e-03 9.999631e-01

P_rect_00: 7.215377e+02 0.000000e+00 6.095593e+02 0.000000e+00 0.000000e+00 7.215377e+02 1.728540e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00

…

矩阵R_rect_00是一个3*3的修正旋转矩阵，用于使相机图像共面，例如，对齐立体视觉平台的多个摄像头（对于这辆KITTI数据采集车，有两部摄像头），这可以通过将左侧摄像头的一行像素直接对齐右侧摄像头的另一行像素（而不是两条在两个相机平面交叉的倾斜直线）。由于我们当前主要考虑MONO（单通道）相机，因此我们将不再深究上述对齐操作的基础理论——但是如果你对此比较感兴趣，可以搜索"epipolar geometry"（对极几何）获取更多相关内容。矩阵P_rect_00则包含了相机的内参（我们称之为 $K$）。下列公式展示了如何使用齐次坐标系将KITTI数据集中的激光雷达3D点云X映射到左侧相机的2D相机图像点Y（这里使用了Kitti readme文件中的符号）上。

程序执行主要流程

要实现将KITTI数据集中的激光雷达3D点云X映射到左侧相机的2D相机图像点Y上，在程序中主要需要执行以下几步：

1. 遍历获取激光雷达点云信息时，将每个3D点转换为齐次坐标，并存入4D变量X中。
2. 应用映射公式，将X映射到相机的图像平面，并将结果存储到变量Y中。
3. 将变量Y从齐次坐标系转换回欧几里得坐标系，从而得到对应的图像中的像素位置，并将结果存入变量pt中。

参考代码如下：
加载对应的相机数据和激光雷达点云数据，初始化前面提到的映射相关矩阵（OpenCV形式矩阵）：

初始化用于显示映射结果的图片数据（visImg用于显示原始相机图像，overlay用于显示映射到相机图像上的激光雷达点云），初始化齐次坐标对象X和Y：

遍历激光雷达点云，将其转换到齐次坐标系映射到相机图像平面，再转换回非齐次坐标，得到对应的像素位置pt，并将其绘制在图像overlay中（最近处为红色，最远处为绿色）。

利用OpenCV的cv::addWeighted函数将overlay图像按照一定的不透明度和原始相机图像visImg融合并显示：

融合显示的结果如下图所示，其中映射的激光雷达点云按照纵向距离的远近用不同的颜色进行显示（最近处为红色，最远处为绿色）：

从结果可以看出，对于映射到路面上的点，较近处呈现红色，较远处呈现绿色，与我们的预期一致。但是显然，图像中上半部分的红色点是不对的。为了解决这个问题，我们常常需要对雷达点云信息进行一些过滤操作，请看下一节。

过滤激光雷达点云

在前面第二节的课程中，我们已经掌握了如何将激光雷达点云俯视图映射到二维图像上。这个映射过程实际上假设了我们只对自车前面的点云感兴趣。在代码中，上述映射过程如下所示：

int y = (-xw * imageSize.height / worldSize.height) + imageSize.height;
int x = (-yw * imageSize.height / worldSize.height) + imageSize.width / 2;

那么，由于Velodyne 激光雷达是安装在车顶，且以10Hz的频率进行360度旋转，因此，这就意味着它也能够测量到自车后面的3D点云。这些点云也包含在数据集中。但是当我们将其映射到相机图像，它们也会形成一个有效的映射值，即便这些点云根本无法在该前向摄像头中被看到。这些点云具有负的x值（纵坐标），这直接导致了它们映射到相机图像后呈现为红色。为了避免这个问题，我们需要执行一些点云过滤操作。

下面的代码展示了进行点云过滤的具体操作，滤除掉了一些无用的点云，例如：

1. 位于激光雷达后方，且具有负的x值。
2. 在x轴方向上距离太远以致超出了最大距离上限。
3. 在y轴方向上距离太远以致超出了自车感兴趣的范围。
4. 距离道路表面太近以致于具有负的z值。
5. 反射率接近0，可能具有较大的测量误差。

    for(auto it=lidarPoints.begin(); it!=lidarPoints.end(); ++it) {

        float maxX = 25.0, maxY = 6.0, minZ = -1.4; 
        if(it->x > maxX || it->x < 0.0 || abs(it->y) > maxY || it->z < minZ || it->r<0.01 )
        {
            continue; // skip to next point
        }

在应用了上述过滤操作后，点云映射到图像上的结果会被过滤掉很多的无用点云信息，呈现的结果如下图所示。对于车辆碰撞预警等应用，传感器的测量质量固然是至关重要的，而过滤步骤，除了能够增加处理速度，还可以帮助提高可靠性，也是不可或缺的步骤。