Camera TTC估计

Camera系列文章

传感器融合是将多个传感器采集的数据进行融合处理，以更好感知周围环境；这里首先介绍Camera的相关内容，包括摄像头及图像知识基本介绍，OpenCV图像识别（特征提取，目标分类等），融合激光点云和图像进行TTC估计。

系列文章目录
1. 摄像头基础及校准
2. Lidar TTC估计
3. Camera TTC估计

---

前言

本章我们将介绍使用Camera估计与前车的TTC（Time To Collision）。首先我们将展示如何基于Camera连续帧中车辆尺寸变化计算TTC；然后我们使用神经网络标记车辆在图像中的位置信息（Bounding Box）；由于用Bounding Box来计算TTC不是很准确，我们将讨论使用关键点检测（keypoint detection）来准确的估计TTC。

相比Lidar TTC估计，基于Camera估计TTC相对难一些：首先，Camera获取的是2D图像（舍弃Z轴距离信息）；其次，Camera需要更可靠、更准确的识别车辆以提取相对速度信息。

---

一、Camera TTC估计方法

如上所述，单目摄像头无法测量距离信息。不像Lidar可以直接TOF法测量距离信息，Camera是被动元器件，依赖于环境光线遇物体反射进入Camera镜头。双面摄像头可以通过对比固定间距的两个摄像头获取的图像差异直接估计距离信息，但由于工艺要求高，尺寸较大，价格较高，计算量较大，目前已经基本退出ADAS应用市场中。

尽管如此，我们还是可以从单目摄像头中提取距离信息。我们还是以CVM（Constant Velocity Model）固定相对速度运动模型来考虑如何无需测速即可计算出TTC。

如上图针孔摄像头几何模型中可推导出几何公式(1)，基于此公式我们可以根据连续帧间目标物相对高度尺寸变化推算出TTC：

二、高度比估算方法

下图中，通过神经网络获取车辆在图像中位置，并用Bounding Box框出车辆位置，包括宽度和高度信息。理论上来说可以用来计算车辆在前后两帧图像中的高度比，但通过对比可以发现，Bounding Box无法准确反映车辆尺寸信息，会出现比较明显的错误估计。

在工程中，依赖于单一测量或特征通常不够可靠，对于安全相关的产品尤其如此，因此，我们需要进一步提取车辆特征，即基于纹理特征点（Texture Keypoints）来估计尺寸变化。
如果我们可定位连续帧上车辆关键点的信息，我们就可以通过这些关键点间的距离信息估计Camera TTC估计公式里的高度比。如下图所示，(a)中我们检出7个关键点（keypoints）；(b)中我们通过匹配连续帧中的关键点获取4个关键点。这样我们就可以使用距离比的中值或均值替代高度比。

高速场景下前车中提取的关键点叠加在2D图像上进行关键点相对距离估算图示如下。

Camera TTC估计实例

如上讨论可知，使用Camera TTC估计主要需要以下四步：
1.通过神经网络等方法框出前车在图像中的位置（Bounding BOX）；
2.提取图像中一系列的关键点（Keypoints）；
3.匹配连续帧中的关键点，建立连接；
4.根据一系列关键点距离的变化估计TTC。

步骤1~3包括较多的图像识别，深度学习内容，在本章暂不讨论，以下实例主要介绍如何根据匹配的关键点估计TTC。

以下实例中，匹配的关键点打包至OpenCV数据结构cv::DMatch中，为提取连续帧中匹配好的关键点，需要使用queryIdx和trainIdx。为获取稳定的高度比，使用medianDistRatio替代meanDistRatio。

// Compute time-to-collision (TTC) based on keypoint correspondences in successive images
void computeTTCCamera(std::vector<cv::KeyPoint> &kptsPrev, std::vector<cv::KeyPoint> &kptsCurr,
                      std::vector<cv::DMatch> kptMatches, double frameRate, double &TTC)
{
    // compute distance ratios between all matched keypoints
    vector<double> distRatios; // stores the distance ratios for all keypoints between curr. and prev. frame
    for (auto it1 = kptMatches.begin(); it1 != kptMatches.end() - 1; ++it1)
    { // outer kpt. loop

        // get current keypoint and its matched partner in the prev. frame
        cv::KeyPoint kpOuterCurr = kptsCurr.at(it1->trainIdx);
        cv::KeyPoint kpOuterPrev = kptsPrev.at(it1->queryIdx);

        for (auto it2 = kptMatches.begin() + 1; it2 != kptMatches.end(); ++it2)
        { // inner kpt.-loop

            double minDist = 100.0; // min. required distance

            // get next keypoint and its matched partner in the prev. frame
            cv::KeyPoint kpInnerCurr = kptsCurr.at(it2->trainIdx);
            cv::KeyPoint kpInnerPrev = kptsPrev.at(it2->queryIdx);

            // compute distances and distance ratios
            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)
            { // avoid division by zero

                double distRatio = distCurr / distPrev;
                distRatios.push_back(distRatio);
            }
        } // eof inner loop over all matched kpts
    }     // eof outer loop over all matched kpts

    // only continue if list of distance ratios is not empty
    if (distRatios.size() == 0)
    {
        TTC = NAN;
        return;
    }


    // STUDENT TASK (replacement meanDistRatio with medianDistRatio)
	// compute camera-based TTC from distance ratios
	
	//meanDistRatio
    //double meanDistRatio = std::accumulate(distRatios.begin(), distRatios.end(), 0.0) / distRatios.size();
	
    std::sort(distRatios.begin(), distRatios.end());
    long medIndex = floor(distRatios.size() / 2.0);
    // compute median dist. ratio to remove outlier influence
    double medDistRatio = distRatios.size() % 2 == 0 ? (distRatios[medIndex - 1] + distRatios[medIndex]) / 2.0 : distRatios[medIndex]; 

    dT = 1 / frameRate;
    TTC = -dT / (1 - medDistRatio);
    // EOF STUDENT TASK
}