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

之前两篇文章记录了lidar感知相关笔记，这篇记录视觉感知以及视觉数据和lidar数据的简单融合。直接上干货。文章主要记录两部分：
1.lidar数据的处理
2.camera与lidar数据的融合

关于视觉部分的基础知识这里不再记录，主要包括图像特征点的提取，关键点、描述子、角点的定义。图像特征点的匹配等等。

1.Lidar点云处理
对于第一部分，要达到的效果就是对lidar数据的过滤处理，同时离本车近的数据点我们红红色表示，离本车远的数据点，我们用绿色表示，中间依次过渡。先放结果图：
图中一个蓝色线格代表2米的距离。
接下来，我们要对lidar点云进行过滤，将属于障碍物的点云保留，数据路面的点云滤除。先放结果：
代码实现部分：
代码部分较为简单，整体思路大致可分为几个部分，
第一部分为创建空白图像，设置图像大小、分辨率；
第二部分为读取lidar数据并将其画在刚刚创建的图线上，这部分涉及世界坐标到图像坐标系的一个转换；
第三部分是将不同距离的lidar点标记为不同的颜色，从近及远用红色到绿色逐渐过渡。
第四部分就是加入蓝色的表示距离的格子线。

（1）创建空白图像

    cv::Size worldSize(10.0, 20.0); // width and height of sensor field in m
    cv::Size imageSize(1000, 2000); // corresponding top view image in pixel
    float maxVal = worldSize.height;
    // create topview image
    cv::Mat topviewImg(imageSize, CV_8UC3, cv::Scalar(0, 0, 0));

（2）加载lidar数据

   for (auto it = lidarPoints.begin(); it != lidarPoints.end(); ++it)
    {
        float xw = (*it).x; // world position in m with x facing forward from sensor
        float yw = (*it).y; // world position in m with y facing left from sensor

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

（3）标记为不同颜色，去除路面点

double zw = (*it).z;
        double minZ = -1.42;//去除路面点的阈值
        if(zw > minZ) {
            double val = (*it).x;
            int red = min(255, (int)(255 * abs((val-maxVal) / maxVal)));//red和green两个值的公式可自行定，只要满足近处到远处由红变绿即可
            int green = min(255, (int)(255 * (1 - abs((val - maxVal) / maxVal))));
            
            cv::circle(topviewImg, cv::Point(x,y), 5, cv::Scalar(0, green, red), -1);//将点云画在图像上
        }

（4）画距离标线

float lineSpacing = 2.0; // 每2米一条线
    int nMarkers = floor(worldSize.height / lineSpacing);
    for (size_t i = 0; i < nMarkers; ++i)
    {
        int y = (-(i * lineSpacing) * imageSize.height / worldSize.height) + imageSize.height;
        cv::line(topviewImg, cv::Point(0, y), cv::Point(imageSize.width, y), cv::Scalar(255, 0, 0));//画线
    }

接下来会写camera与lidar数据的融合的相关内容，也是此片博客的下半文。有什么问题欢迎大家留言讨论。也可关注小博主，私聊。