1. Collision detection model

恒定速度模型与恒定加速度模型

2. Estimating TTC with a Camera

恒速模型TTC计算

边界框检测问题

改用纹理关键点

3. Estimating TTC with Lidar

struct LidarPoint { // single lidar point in space
    double x, y, z; // point position in m
    double r; // point reflectivity in the range 0-1
};

void computeTTCLidar(std::vector &lidarPointsPrev,
                     std::vector &lidarPointsCurr, double &TTC)
{
    // auxiliary variables
    double dT = 0.1; // time between two measurements in seconds

    // find closest distance to Lidar points
    double minXPrev = 1e9, minXCurr = 1e9;
    for(auto it=lidarPointsPrev.begin(); it!=lidarPointsPrev.end(); ++it) {
        minXPrev = minXPrev>it->x ? it->x : minXPrev;
    }

    for(auto it=lidarPointsCurr.begin(); it!=lidarPointsCurr.end(); ++it) {
        minXCurr = minXCurr>it->x ? it->x : minXCurr;
    }

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

4. Camera based 2D feature detection

5、Object Detection with YOLO

YOLOv3工作流程

• “confThreshold”用于删除分数值较低的所有边界框

• “ nmsThreshold”将非最大抑制NMS应用于剩余的边界框。

• “ inpWidth”和“ inpHeight”，YOLO作者所建议，将其设置为416。其他值例如可以是320（更快）或608（更准确）。
  

// load image from file
cv::Mat img = cv::imread("./images/img1.png");

// load class names from file
string yoloBasePath = "./dat/yolo/";
string yoloClassesFile = yoloBasePath + "coco.names";
string yoloModelConfiguration = yoloBasePath + "yolov3.cfg";
string yoloModelWeights = yoloBasePath + "yolov3.weights";

vector classes;
ifstream ifs(yoloClassesFile.c_str());
string line;
while (getline(ifs, line)) classes.push_back(line);

// load neural network
cv::dnn::Net net = cv::dnn::readNetFromDarknet(yoloModelConfiguration, yoloModelWeights);
net.setPreferableBackend(cv::dnn::DNN_BACKEND_OPENCV);
net.setPreferableTarget(cv::dnn::DNN_TARGET_CPU);

• 从文件加载的图像通过blobFromImage函数传递，然后转换为神经网络的输入块。像素值以1/255的缩放因子缩放到0到1的目标范围。它还可以将图像的大小调整为指定的大小（416、416、416），而不会进行裁切。

 // generate 4D blob from input image
    cv::Mat blob;
    double scalefactor = 1/255.0;
    cv::Size size = cv::Size(416, 416);
    cv::Scalar mean = cv::Scalar(0,0,0);
    bool swapRB = false;
    bool crop = false;
    cv::dnn::blobFromImage(img, blob, scalefactor, size, mean, swapRB, crop);　

输出Blob将作为输入传递到网络。然后，将执行前向传递以获得从网络输出的预测边界框的列表。这些框经过一个后处理步骤，以过滤掉具有低置信度值的那些框.



通过网络运行正向传递



我们必须将刚创建的Blob作为输入传递给网络。然后，我们运行OpenCV的转发功能，以执行通过网络的单个转发。为此，我们需要确定网络的最后一层，并为该功能提供相关的内部名称。这可以通过使用OpenCV函数“ getUnconnectedOutLayers”来完成，该函数给出所有未连接输出层的名称，这些输出层实际上是网络的最后一层。

 // Get names of output layers
    vector names;
    vector outLayers = net.getUnconnectedOutLayers(); // get indices of output layers, i.e. layers with unconnected outputs
    vector layersNames = net.getLayerNames(); // get names of all layers in the network

    names.resize(outLayers.size());
    for (size_t i = 0; i < outLayers.size(); ++i) // Get the names of the output layers in names
    {
        names[i] = layersNames[outLayers[i] - 1];
    }

    // invoke forward propagation through network
    vector netOutput;
    net.setInput(blob);
    net.forward(netOutput, names);

前向传递的结果以及网络的输出是大小为C的向量，每个类中的前四个元素表示中心x，y，相关边界框的宽度和高度。第五个元素表示各个边界框实际上包围对象的信任度或置信度。矩阵的其余元素是与coco.cfg文件中包含的每个类相关联的置信度。

// Scan through all bounding boxes and keep only the ones with high confidence
    float confThreshold = 0.20;
    vector classIds;
    vector confidences;
    vector boxes;
    for (size_t i = 0; i < netOutput.size(); ++i)
    {
        float* data = (float*)netOutput[i].data;
        for (int j = 0; j < netOutput[i].rows; ++j, data += netOutput[i].cols)
        {
            cv::Mat scores = netOutput[i].row(j).colRange(5, netOutput[i].cols);
            cv::Point classId;
            double confidence;

            // Get the value and location of the maximum score
            cv::minMaxLoc(scores, 0, &confidence, 0, &classId);
            if (confidence > confThreshold)
            {
                cv::Rect box; int cx, cy;
                cx = (int)(data[0] * img.cols);
                cy = (int)(data[1] * img.rows);
                box.width = (int)(data[2] * img.cols);
                box.height = (int)(data[3] * img.rows);
                box.x = cx - box.width/2; // left
                box.y = cy - box.height/2; // top

                boxes.push_back(box);
                classIds.push_back(classId.x);
                confidences.push_back((float)confidence);
            }
        }
    }

• 网络输出的后处理
  

• OpenCV库提供了一种现成的函数，用于抑制重叠的边界框。此功能称为NMSBoxes，可以按以下简短代码示例所示使用它：
  

// perform non-maxima suppression
    float nmsThreshold = 0.4;  // Non-maximum suppression threshold
    vector indices;
    cv::dnn::NMSBoxes(boxes, confidences, confThreshold, nmsThreshold, indices);

• 应用非最大抑制后，多余的边界框将被成功删除。下图显示了结果，其中绿色表示保留的边界框，而红色的边界框在NMS期间已删除。

6. Flow schematic

• 在第一部分（黄色框）中，我们已经完成了编写有关关键点检测和关键点匹配的代码。黄色矩形是其程序流程。首先我们加载图像并使用环形缓冲区，然后检测图像关键点，提取关键点描述符，最后匹配相邻两帧图像的关键点描述符。虽然关键点匹配存在于每个图像中，但是它们没有聚类并绑定到任何对象。

• 在第二部分中（绿色框），我们使用Yolov3完成了对对象的检测和分类，这为我们提供了一组感兴趣对象边界框。对于相机图像，我们可以将对象上的关键点关联到对应的边界框。而对于激光雷达，我们也可以将激光雷达3D点云投影到2D图像平面，使用对象检测的边界框对点云进行聚类，到第四步时该边界框变量既包含有关对象位置的信息，还包括车辆在图像中的关键点信息，以及对应的3D激光雷达点云。

• 接下来我们需要时刻跟踪3D对象边界框。因此，我们需要通过查看边界框内的关键点对应关系来关联相邻帧之间的边界框。

• 在完成第八步之后，我们可以计算相机及激光雷达传感器正前方物体的TTC。值得注意的是，我们将只关注本车道线正前方最近的对象。

Code Implementation

if (dataBuffer.size() > dataBufferSize) {
	dataBuffer.erase(dataBuffer.begin());
	}
dataBuffer.push_back(frame);

float confThreshold = 0.2;
float nmsThreshold = 0.4;
detectObjects((dataBuffer.end() - 1)->cameraImg, (dataBuffer.end() - 1)->boundingBoxes, confThreshold,nmsThreshold,yoloBasePath, yoloClassesFile, yoloModelConfiguration, yoloModelWeights, bVis);

// remove Lidar points based on distance properties
float minZ = -1.5, maxZ = -0.9, minX = 2.0, maxX = 20.0, maxY = 2.0, minR = 0.1; // focus on ego lane
cropLidarPoints(lidarPoints, minX, maxX, maxY, minZ, maxZ, minR);

(dataBuffer.end() - 1)->lidarPoints = lidarPoints;

// associate Lidar points with camera-based ROI
float shrinkFactor = 0.20; // shrinks each bounding box by the given percentage to avoid 3D object merging at the edges of an ROI
clusterLidarWithROI((dataBuffer.end()-1)->boundingBoxes, (dataBuffer.end() - 1)->lidarPoints, shrinkFactor, P_rect_00, R_rect_00, RT);

// choose keypoint detector
detKeypointsModern(keypoints, imgGray, detectorType, false);
// choose keypoint descriptor
descKeypoints((dataBuffer.end() - 1)->keypoints, (dataBuffer.end() - 1)->cameraImg, descriptors,descriptorType);
// choose keypoint matcher and match corresponding kepoints
matchDescriptors((dataBuffer.end() - 2)->keypoints, (dataBuffer.end() - 1)->keypoints,
(dataBuffer.end() - 2)->descriptors, (dataBuffer.end() - 1)->descriptors,matches, descriptorclass, matcherType, selectorType);
// store matches in current data frame
(dataBuffer.end() - 1)->kptMatches = matches;

map bbBestMatches;
matchBoundingBoxes(matches, bbBestMatches, *(dataBuffer.end() - 2), *(dataBuffer.end() -1)); // associate bounding boxes between current and previous frame using keypoint matches
// store matches in current data frame
(dataBuffer.end() - 1)->bbMatches = bbBestMatches;

for (auto it1 = (dataBuffer.end() - 1)->bbMatches.begin();it1 != (dataBuffer.end() - 1)->bbMatches.end(); ++it1) {

    // find bounding boxes associates with current match
    BoundingBox *prevBB, *currBB;
    for (auto it2 = (dataBuffer.end() - 1)->boundingBoxes.begin();it2 != (dataBuffer.end() - 1)->boundingBoxes.end(); ++it2) {
        if (it1->second == it2->boxID) // check wether current match partner corresponds to this BB
            {
            currBB = &(*it2);
            }
        }

    for (auto it2 = (dataBuffer.end() - 2)->boundingBoxes.begin();it2 != (dataBuffer.end() - 2)->boundingBoxes.end(); ++it2) {
        if (it1->first == it2->boxID) // check wether current match partner corresponds to this BB
            {
            prevBB = &(*it2);
            }
    	}

    // compute TTC for current match
    if (currBB->lidarPoints.size() > 0 &&prevBB->lidarPoints.size() > 0)
    // only compute TTC if we have Lidar points
        {
        // compute time-to-collision based on Lidar data (implement -> computeTTCLidar)
        double ttcLidar;
        computeTTCLidar(prevBB->lidarPoints, currBB->lidarPoints, sensorFrameRate, ttcLidar);

        // assign enclosed keypoint matches to bounding box (implement -> clusterKptMatchesWithROI)
        // compute time-to-collision based on camera (implement -> computeTTCCamera)
        double ttcCamera;
        clusterKptMatchesWithROI(*currBB, (dataBuffer.end() - 2)->keypoints,(dataBuffer.end() - 1)->keypoints, (dataBuffer.end() - 1)->kptMatches);
        computeTTCCamera((dataBuffer.end() - 2)->keypoints, (dataBuffer.end() - 1)->keypoints,currBB->kptMatches, sensorFrameRate, ttcCamera);
        currBB->kptMatches, sensorFrameRate, ttcCameraCAM,TmPrev);
        }
   　}

8. Conclusion