传感器融合-SFND_3D_Object_Tracking源码解读(四)

camFusion_Student.cpp
此部分做的是将激光雷达3D点云映射到相机图像，具体的理论可以参考这篇博客
视觉融合-相机校准与激光点云投影

#include <iostream>
#include <algorithm>
#include <numeric>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "camFusion.hpp"
#include "dataStructures.h"

using namespace std;

// 创建多组激光雷达点，这些激光雷达点的投影到相机的边界框内
//这部分主要是做坐标转换，将世界坐标转换成像素坐标，也就是将激光雷达点投影到图像上
void clusterLidarWithROI(std::vector<BoundingBox> &boundingBoxes, std::vector<LidarPoint> &lidarPoints, float shrinkFactor, cv::Mat &P_rect_xx, cv::Mat &R_rect_xx, cv::Mat &RT)
{
    // 遍历所有激光雷达点并将它们关联到2D边界框
    cv::Mat X(4, 1, cv::DataType<double>::type);
    cv::Mat Y(3, 1, cv::DataType<double>::type);

    for (auto it1 = lidarPoints.begin(); it1 != lidarPoints.end(); ++it1)
    {
        // assemble vector for matrix-vector-multiplication
        X.at<double>(0, 0) = it1->x;
        X.at<double>(1, 0) = it1->y;
        X.at<double>(2, 0) = it1->z;
        X.at<double>(3, 0) = 1;

        // project Lidar point into camera
        Y = P_rect_xx * R_rect_xx * RT * X;
        cv::Point pt;
        pt.x = Y.at<double>(0, 0) / Y.at<double>(0, 2); // 像素坐标X
        pt.y = Y.at<double>(1, 0) / Y.at<double>(0, 2); // 像素坐标Y

        vector<vector<BoundingBox>::iterator> enclosingBoxes; // 指向所有包围当前激光雷达点的边界框的指针
        for (vector<BoundingBox>::iterator it2 = boundingBoxes.begin(); it2 != boundingBoxes.end(); ++it2)
        {
            // 略微缩小当前边界框，以避免边缘周围有太多离群点
            cv::Rect smallerBox;
            /*
			//如果创建一个Rect对象rect(100, 50, 50, 100)，那么rect会有以下几个功能：  
			rect.area();     //返回rect的面积 5000  
			rect.size();     //返回rect的尺寸 [50 × 100]  
			rect.tl();       //返回rect的左上顶点的坐标 [100, 50]  
			rect.br();       //返回rect的右下顶点的坐标 [150, 150]  
			rect.width();    //返回rect的宽度 50  
			rect.height();   //返回rect的高度 100  
			rect.contains(Point(x, y));  //返回布尔变量，判断rect是否包含Point(x, y)点  
			typedef struct CvRect 
		　　 { 
			　　int x; // 方形的左上角的x-坐标 
			　　int y; // 方形的左上角的y-坐标
			　　int width; // 宽 
			　　int height; // 高 
		　　 }
		    */
            smallerBox.x = (*it2).roi.x + shrinkFactor * (*it2).roi.width / 2.0;
            smallerBox.y = (*it2).roi.y + shrinkFactor * (*it2).roi.height / 2.0;
            smallerBox.width = (*it2).roi.width * (1 - shrinkFactor);
            smallerBox.height = (*it2).roi.height * (1 - shrinkFactor);

            // 检查点是否在当前边界框内
            if (smallerBox.contains(pt))
            {
                enclosingBoxes.push_back(it2);
            }

        } // eof在所有边界框上循环

        // 检查点是否被一个或多个框包围
        if (enclosingBoxes.size() == 1)
        { 
            // 添加激光雷达点到boundingbox中
            enclosingBoxes[0]->lidarPoints.push_back(*it1);
        }
    } // eof在所有边界框上循环
}

void show3DObjects(std::vector<BoundingBox> &boundingBoxes, cv::Size worldSize, cv::Size imageSize, bool bWait)
{
    // 创建俯视图像
    cv::Mat topviewImg(imageSize, CV_8UC3, cv::Scalar(255, 255, 255));

    for(auto it1=boundingBoxes.begin(); it1!=boundingBoxes.end(); ++it1)
    {
        // 为当前3D对象创建随机颜色
        cv::RNG rng(it1->boxID);
        cv::Scalar currColor = cv::Scalar(rng.uniform(0,150), rng.uniform(0, 150), rng.uniform(0, 150));

        // 将激光雷达点绘制到顶视图图像中
        int top=1e8, left=1e8, bottom=0.0, right=0.0; 
        float xwmin=1e8, ywmin=1e8, ywmax=-1e8;
        for (auto it2 = it1->lidarPoints.begin(); it2 != it1->lidarPoints.end(); ++it2)
        {
            // 世界坐标
            float xw = (*it2).x; //世界坐标系下的激光雷达点云纵坐标
            float yw = (*it2).y; //世界坐标系下的激光雷达点云横坐标（车身左侧为正）
            xwmin = xwmin<xw ? xwmin : xw;
            ywmin = ywmin<yw ? ywmin : yw;
            ywmax = ywmax>yw ? ywmax : yw;

			// x，y表示图片坐标系下的坐标点，分别对应图像的行索引和列索引。
			//（如相关坐标系对应关系不同，可进行相应调整）
            // top-view coordinates
            int y = (-xw * imageSize.height / worldSize.height) + imageSize.height;
            int x = (-yw * imageSize.width / worldSize.width) + imageSize.width / 2;

            // 找到封闭的矩形
            top = top<y ? top : y;
            left = left<x ? left : x;
            bottom = bottom>y ? bottom : y;
            right = right>x ? right : x;

            // 画出个别点
            cv::circle(topviewImg, cv::Point(x, y), 4, currColor, -1);
        }

        // 画出封闭的矩形框
        cv::rectangle(topviewImg, cv::Point(left, top), cv::Point(right, bottom),cv::Scalar(0,0,0), 2);

        //用一些关键数据扩充对象
        char str1[200], str2[200];
        sprintf(str1, "id=%d, #pts=%d", it1->boxID, (int)it1->lidarPoints.size());
        putText(topviewImg, str1, cv::Point2f(left-250, bottom+50), cv::FONT_ITALIC, 2, currColor);
        sprintf(str2, "xmin=%2.2f m, yw=%2.2f m", xwmin, ywmax-ywmin);
        putText(topviewImg, str2, cv::Point2f(left-250, bottom+125), cv::FONT_ITALIC, 2, currColor);  
    }

    // 绘制距离标记
    float lineSpacing = 2.0; // 距离标记之间的间隔
    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));
    }

    // 展示图像
    string windowName = "3D Objects";
    cv::namedWindow(windowName, 1);
    cv::imshow(windowName, topviewImg);

    if(bWait)
    {
        cv::waitKey(0); // wait for key to be pressed
    }
}


// 将给定的边界框与其包含的关键点相关联
void clusterKptMatchesWithROI(BoundingBox &boundingBox_c, BoundingBox &boundingBox_p, std::vector<cv::KeyPoint> &kptsPrev, std::vector<cv::KeyPoint> &kptsCurr, std::vector<cv::DMatch> &kptMatches)
{
    double dist_mean = 0;
    std::vector<cv::DMatch>  kptMatches_roi;

    float shrinkFactor = 0.15;
    cv::Rect smallerBox_c, smallerBox_p;
    // 略微缩小当前边界框，以避免边缘周围有太多离散点
    smallerBox_c.x = boundingBox_c.roi.x + shrinkFactor * boundingBox_c.roi.width / 2.0;
    smallerBox_c.y = boundingBox_c.roi.y + shrinkFactor * boundingBox_c.roi.height / 2.0;
    smallerBox_c.width = boundingBox_c.roi.width * (1 - shrinkFactor);
    smallerBox_c.height = boundingBox_c.roi.height * (1 - shrinkFactor);

    // 稍微收缩预边界框，以避免边缘周围有太多离散点
    smallerBox_p.x = boundingBox_p.roi.x + shrinkFactor * boundingBox_p.roi.width / 2.0;
    smallerBox_p.y = boundingBox_p.roi.y + shrinkFactor * boundingBox_p.roi.height / 2.0;
    smallerBox_p.width = boundingBox_p.roi.width * (1 - shrinkFactor);
    smallerBox_p.height = boundingBox_p.roi.height * (1 - shrinkFactor);

    //获取curr_boundingBox和pre_boundingBox中的匹配项
    for(auto it=kptMatches.begin(); it != kptMatches.end(); ++ it)
    {
        cv::KeyPoint train = kptsCurr.at(it->trainIdx);
        auto train_pt = cv::Point(train.pt.x, train.pt.y);

        cv::KeyPoint query = kptsPrev.at(it->queryIdx); 
        auto query_pt = cv::Point(query.pt.x, query.pt.y);

        // 检查点是否在当前边界框内
        if (smallerBox_c.contains(train_pt) && smallerBox_p.contains(query_pt)) 
            kptMatches_roi.push_back(*it);           
    }

    //计算边界框内所有匹配项的平均距离
    for(auto it=kptMatches_roi.begin(); it != kptMatches_roi.end(); ++ it)
    {
        dist_mean += cv::norm(kptsCurr.at(it->trainIdx).pt - kptsPrev.at(it->queryIdx).pt); 
    }
    if(kptMatches_roi.size() > 0)
        dist_mean = dist_mean/kptMatches_roi.size();
    else return;

    //保持匹配距离<dist_mean * 1.5
    double threshold = dist_mean*1.5;
    for  (auto it = kptMatches_roi.begin(); it != kptMatches_roi.end(); ++it)
    {
       float dist = cv::norm(kptsCurr.at(it->trainIdx).pt - kptsPrev.at(it->queryIdx).pt);
       if (dist< threshold)
           boundingBox_c.kptMatches.push_back(*it);
    }
    std::cout<<"curr_bbx_matches_size: "<<boundingBox_c.kptMatches.size()<<std::endl;
}

// 根据连续图像中的关键点对应关系计算碰撞时间（TTC）
void computeTTCCamera(std::vector<cv::KeyPoint> &kptsPrev, std::vector<cv::KeyPoint> &kptsCurr, 
                      std::vector<cv::DMatch> kptMatches, double frameRate, double &TTC, cv::Mat *visImg)
{
    vector<double> distRatios; // 存储curr之间所有关键点的距离比率。 和上一个。 帧
    for (auto it1 = kptMatches.begin(); it1 != kptMatches.end() - 1; ++it1)
    {
        cv::KeyPoint kpOuterCurr = kptsCurr.at(it1->trainIdx);
        cv::KeyPoint kpOuterPrev = kptsPrev.at(it1->queryIdx);
       
        for (auto it2 = it1 + 1; it2 != kptMatches.end() - 1; ++it2)
        {  
            double minDist = 100.0; // min. required distance
            cv::KeyPoint kpInnerCurr = kptsCurr.at(it2->trainIdx);
            cv::KeyPoint kpInnerPrev = kptsPrev.at(it2->queryIdx);
            // 计算距离和距离比
            double distCurr = cv::norm(kpOuterCurr.pt - kpInnerCurr.pt);
            double distPrev = cv::norm(kpOuterPrev.pt - kpInnerPrev.pt);
            if (distPrev > std::numeric_limits<double>::epsilon() && distCurr >= minDist)
            { // 避免除数为０
                double distRatio = distCurr / distPrev;
                distRatios.push_back(distRatio);
            }
        }
    }  
    // 仅在距离比列表不为空时继续
    if (distRatios.size() == 0)
    {
        TTC = NAN;
        return;
    }

    std::sort(distRatios.begin(), distRatios.end());

    long medIndex = floor(distRatios.size() / 2.0);
    double medDistRatio = distRatios.size() % 2 == 0 ? (distRatios[medIndex - 1] + distRatios[medIndex]) / 2.0 : distRatios[medIndex]; // 计算中位数距离 消除异常影响的比率
    double dT = 1 / frameRate;
    TTC = -dT / (1 - medDistRatio);
}

void computeTTCLidar(std::vector<LidarPoint> &lidarPointsPrev,
                     std::vector<LidarPoint> &lidarPointsCurr, double frameRate, double &TTC)
{
    int lane_wide = 4;
    //仅考虑本车道的雷达点
    std::vector<float> ppx;
    std::vector<float> pcx;
    for(auto it = lidarPointsPrev.begin(); it != lidarPointsPrev.end() -1; ++it)
    {
        if(abs(it->y) < lane_wide/2) ppx.push_back(it->x);
    }
    for(auto it = lidarPointsCurr.begin(); it != lidarPointsCurr.end() -1; ++it)
    {
        if(abs(it->y) < lane_wide/2) pcx.push_back(it->x);
    }

    float min_px, min_cx;
    int p_size = ppx.size();
    int c_size = pcx.size();
    if(p_size > 0 && c_size > 0)
    {
        for(int i=0; i<p_size; i++)
        {
            min_px += ppx[i];
        }

        for(int j=0; j<c_size; j++)
        {
            min_cx += pcx[j];
        }
    }
    else 
    {
        TTC = NAN;
        return;
    }

    min_px = min_px /p_size;
    min_cx = min_cx /c_size;
    std::cout<<"lidar_min_px:"<<min_px<<std::endl;
    std::cout<<"lidar_min_cx:"<<min_cx<<std::endl;

    float dt = 1/frameRate;
    TTC = min_cx * dt / (min_px - min_cx);
}


void matchBoundingBoxes(std::vector<cv::DMatch> &matches, std::map<int, int> &bbBestMatches, DataFrame &prevFrame, DataFrame &currFrame)
{
	　//注意：调用cv :: DescriptorMatcher :: match函数后，每个DMatch
     //根据要匹配的图像参数的顺序，包含两个关键点索引queryIdx和trainIdx。
     // https://docs.opencv.org/4.1.0/db/d39/classcv_1_1DescriptorMatcher.html#a0f046f47b68ec7074391e1e85c750cba
     // prevFrame.keypoints由queryIdx索引
     // currFrame.keypoints由trainIdx索引
    int p = prevFrame.boundingBoxes.size();
    int c = currFrame.boundingBoxes.size();
    int pt_counts[p][c] = { };
    for (auto it = matches.begin(); it != matches.end() - 1; ++it)     
    {
        cv::KeyPoint query = prevFrame.keypoints[it->queryIdx];
        auto query_pt = cv::Point(query.pt.x, query.pt.y);
        bool query_found = false;

        cv::KeyPoint train = currFrame.keypoints[it->trainIdx];
        auto train_pt = cv::Point(train.pt.x, train.pt.y);
        bool train_found = false;

        std::vector<int> query_id, train_id;
        for (int i = 0; i < p; i++) 
        {
            if (prevFrame.boundingBoxes[i].roi.contains(query_pt))            
             {
                query_found = true;
                query_id.push_back(i);
             }
        }
        for (int i = 0; i < c; i++) 
        {
            if (currFrame.boundingBoxes[i].roi.contains(train_pt))            
            {
                train_found= true;
                train_id.push_back(i);
            }
        }

        if (query_found && train_found) 
        {
            for (auto id_prev: query_id)
                for (auto id_curr: train_id)
                     pt_counts[id_prev][id_curr] += 1;
        }
    }
   
    for (int i = 0; i < p; i++)
    {  
         int max_count = 0;
         int id_max = 0;
         for (int j = 0; j < c; j++)
             if (pt_counts[i][j] > max_count)
             {  
                  max_count = pt_counts[i][j];
                  id_max = j;
             }
          bbBestMatches[i] = id_max;
    } 
}

本节讲解如何将激光雷达点云俯视图（仅考虑水平坐标）映射到二维图像中，其中涉及到激光雷达点云的地面部分滤除，和不同坐标范围下的数据的简单映射。本文主要侧重提供方法的参考，代码仅供参考，可根据实际使用场景的需要进行修改。

本文中采用的激光雷达数据来自KITTI数据集，使用Velodyne HDL-64E激光雷达采集得到，该激光雷达工作频率10Hz，每个周期可以采集大约10万个点。更多信息可参考KITTI数据集的传感器安装部分文档：添加链接描述

在KITTI数据集中，该激光雷达点云数据已经和一个前视摄像头进行了同步，本文使用的激光雷达数据采集到的场景近似于下方图片所示的场景。

实现方法
１． 通过数据文件加载激光雷达点云数据，来自Kitti数据集。
２．初始化特定像素大小的图片（1000*2000）。
３．根据需要显示的激光雷达点云范围和图片像素范围，将激光雷达点云的横纵坐标映射到图片的特定像素中，其中激光雷达所处的世界坐标系遵循右手系，x轴对应前进方向，y轴对应左侧横向；而对于图像坐标系，x,y分别对应其图像的行索引和列索引，且图像左上角为原点。
具体处理方式参考如下代码：

//参考定义：CvSize cvSize( int width, int height );
cv::Size worldSize(10.0,20.0);		//待显示的激光雷达点云在世界坐标系下的范围
cv::Size imageSize(1000.0,2000.0);	//待显示的图片的像素范围（与世界坐标系等比例）

...
float xw=(*it).x;//世界坐标系下的激光雷达点云纵坐标
float yw=(*it).y;//世界坐标系下的激光雷达点云横坐标（车身左侧为正）

// x，y表示图片坐标系下的坐标点，分别对应图像的行索引和列索引。
//（如相关坐标系对应关系不同，可进行相应调整）
int x = (-yw * imageSize.height / worldSize.height) + imageSize.width / 2;
int y = (-xw * imageSize.height / worldSize.height) + imageSize.height;

转换后的显示效果如下图所示：
4. 可根据激光雷达相对地面的安装位置高度，设置恰当的阈值变量doulble minZ=-1.40，只有该阈值以上的激光点云才进行上述映射操作，从而过滤掉激光点云中的地面反射目标。最终映射后的图片显示效果如下所示：

参考代码
实现以图片形式显示激光雷达点云俯视图的相关参考代码：