OpenCV学习问题记录（二）

避撞检测——TTC计算
1、Lidar碰撞检测

#include <iostream>
#include <numeric>
#include <opencv2/core.hpp>
#include "dataStructures.h"
#include "structIO.hpp"

using namespace std;

void computeTTCLidar(std::vector<LidarPoint> &lidarPointsPrev,
                     std::vector<LidarPoint> &lidarPointsCurr, double &TTC)
{
    // auxiliary variables
    double dT = 0.1;        // time between two measurements in seconds
    double laneWidth = 4.0; // assumed width of the ego lane

    // find closest distance to Lidar points within ego lane
    double minXPrev = 1e9, minXCurr = 1e9;
    for (auto it = lidarPointsPrev.begin(); it != lidarPointsPrev.end(); ++it)
    {
        if (it->y >= -1 * laneWidth / 2 && it->y <= laneWidth / 2){
            minXPrev = minXPrev > it->x ? it->x : minXPrev;
        }
    }

    for (auto it = lidarPointsCurr.begin(); it != lidarPointsCurr.end(); ++it)
    {
        if (it->y >= -1 * laneWidth / 2 && it->y <= laneWidth / 2){
            minXCurr = minXCurr > it->x ? it->x : minXCurr;
        }
    }

    // compute TTC from both measurements
    TTC = minXCurr * dT / (minXPrev - minXCurr);
}

int main()
{

    std::vector<LidarPoint> currLidarPts, prevLidarPts;
    readLidarPts("E:/传感器融合/SensorFusion/所有代码/所有代码/3_Camera/03_Engineering a Collision Detection System/TTC-Lidar/dat/C22A5_currLidarPts.dat", currLidarPts);
    readLidarPts("E:/传感器融合/SensorFusion/所有代码/所有代码/3_Camera/03_Engineering a Collision Detection System/TTC-Lidar/dat/C22A5_prevLidarPts.dat", prevLidarPts);


    double ttc;
    computeTTCLidar(prevLidarPts, currLidarPts, ttc);
    cout << "ttc = " << ttc << "s" << endl;
}