Udacity传感器融合笔记（三）Camera与lidar数据融合（下）

本篇博客为Camera与Lidar数据融合的下部分，主要记录如何将Lidar点云数据映射到camera图像数据。

在介绍如何映射之前，先要了解一下什么是相机的外参和内参：
1.相机的外参矩阵 包含两部分 ：旋转矩阵R和平移矩阵T。描述的是世界坐标系和相机坐标系的相互位置关系。告诉我们如何将世界坐标系的一个三维点通过外参的坐标变换，变换到相机坐标系上。
2.相机的内参矩阵K，描述的是相机坐标系和图像坐标系的位置关系。告诉我们如何将步骤1相机坐标系上的坐标转换为像素坐标系上的坐标
3.畸变矩阵，由于镜头误差等因素，我们的最后所呈的图像会带有一定的畸变，通常被我们叫做桶形畸变和枕型畸变，畸变矩阵就是告诉我们为什么上述的那个像素点并没有落在理论位置上。

此部分代码为相机的参数设置：

void loadCalibrationData(cv::Mat &P_rect_00, cv::Mat &R_rect_00, cv::Mat &RT)
{
    RT.at<double>(0,0) = 7.533745e-03; RT.at<double>(0,1) = -9.999714e-01; RT.at<double>(0,2) = -6.166020e-04; RT.at<double>(0,3) = -4.069766e-03;
    RT.at<double>(1,0) = 1.480249e-02; RT.at<double>(1,1) = 7.280733e-04; RT.at<double>(1,2) = -9.998902e-01; RT.at<double>(1,3) = -7.631618e-02;
    RT.at<double>(2,0) = 9.998621e-01; RT.at<double>(2,1) = 7.523790e-03; RT.at<double>(2,2) = 1.480755e-02; RT.at<double>(2,3) = -2.717806e-01;
    RT.at<double>(3,0) = 0.0; RT.at<double>(3,1) = 0.0; RT.at<double>(3,2) = 0.0; RT.at<double>(3,3) = 1.0;
    
    R_rect_00.at<double>(0,0) = 9.999239e-01; R_rect_00.at<double>(0,1) = 9.837760e-03; R_rect_00.at<double>(0,2) = -7.445048e-03; R_rect_00.at<double>(0,3) = 0.0;
    R_rect_00.at<double>(1,0) = -9.869795e-03; R_rect_00.at<double>(1,1) = 9.999421e-01; R_rect_00.at<double>(1,2) = -4.278459e-03; R_rect_00.at<double>(1,3) = 0.0;
    R_rect_00.at<double>(2,0) = 7.402527e-03; R_rect_00.at<double>(2,1) = 4.351614e-03; R_rect_00.at<double>(2,2) = 9.999631e-01; R_rect_00.at<double>(2,3) = 0.0;
    R_rect_00.at<double>(3,0) = 0; R_rect_00.at<double>(3,1) = 0; R_rect_00.at<double>(3,2) = 0; R_rect_00.at<double>(3,3) = 1;
    
    P_rect_00.at<double>(0,0) = 7.215377e+02; P_rect_00.at<double>(0,1) = 0.000000e+00; P_rect_00.at<double>(0,2) = 6.095593e+02; P_rect_00.at<double>(0,3) = 0.000000e+00;
    P_rect_00.at<double>(1,0) = 0.000000e+00; P_rect_00.at<double>(1,1) = 7.215377e+02; P_rect_00.at<double>(1,2) = 1.728540e+02; P_rect_00.at<double>(1,3) = 0.000000e+00;
    P_rect_00.at<double>(2,0) = 0.000000e+00; P_rect_00.at<double>(2,1) = 0.000000e+00; P_rect_00.at<double>(2,2) = 1.000000e+00; P_rect_00.at<double>(2,3) = 0.000000e+00;

}

通过上述几个矩阵，我们就可以将一个3维空间点的坐标X映射到二维平面点Y上，所用公式如下：
这里的P_rect_xx代表的就是相机的内参，R_rect_00代表的是一个3x3的旋转修正矩阵，在使用双目摄像头时，用于使两个摄像头的图像共面。如果使用单目摄像头，则不需要该矩阵。（R|T）就是相机的外参矩阵R和T。

在课程的代码中，将lidar的3D点云数据映射到2维平面中经历了以下几个步骤：
1.读取点云数据，并将其转换为齐次坐标，保存在向量X中：

for(auto it = lidarPoints.begin(); it != lidarPoints.end(); ++it) //遍历点云数据

 cv::Mat X(4,1,cv::DataType<double>::type);//用于储存lidar的齐次坐标点

 //保存齐次坐标
 X.at<double>(0,0) = (*it).x;
        X.at<double>(1,0) = (*it).y;
        X.at<double>(2,0) = (*it).z;
        X.at<double>(3,0) = 1;

2.应用映射公式，将三维点云数据映射到2维平面上，保存在Y中：

Y = P_rect_00 * R_rect_00 * RT * X;//映射公式

3.将Y中的齐次坐标变为像素坐标将结果保存在pt中。

 double scale = Y.at<double>(0,2);//变换系数

变换为像素坐标
 cv::Point pt;
        pt.x = Y.at<double>(0,0) / scale;
        pt.y = Y.at<double>(1,0) / scale;

接下来就是这篇博客的上半部分的内容，将2维平面数据用opnecv画在图像上，并按按照由近到远用红色和绿色表示
可以回到上篇博客去回顾：
Camera与lidar数据融合（上）
这样，我们就可以得到类似上篇博客的结果。

第三步就是将上面所得到的结果与图像数据进行映射。映射使用的是opencv中的功能函数：addWeighted（）
函数有6个参数，其意义分别为：
参数1.映射的前景图像
参数2：前景图像融合权重
参数3：背景图像
参数4：背景图像权重
参数5：参数1与参数2作和后添加的数值，默认为0
参数6：输出图片

 cv::addWeighted(overlay, opacity, visImg, 1 - opacity, 0, visImg);//addWeighted的原理可查看opencv源码

这样就可以得到如下结果：
可以看到，点云数据已经映射到原图像上了。但是结果中存在大量无用的点云数据，我们可以将其滤除，对滤除后的点云数据再做映射。
1.可以将x方向（汽车前进方向）设置阈值 25和0 。对于距离大于25和小于0的点云数据滤除。
2.可以将汽车左右方向大于6m的点云数据滤除
3.可以将传感器下方1.42米以下的点云滤除
4.可以将反射绿小于0.01的点云滤除

 if((*it).x > maxX || (*it).x < 0.0 || abs((*it).y) > maxY || (*it).z < minZ || (*it).r<0.01 )
        {
            continue;
        }

经过上面的点云滤除，再做映射。得到结果如下：
欢迎大家评论区留言讨论！