【Apollo解读代码系列之perception】camera模块tracker逻辑分析

camera模块之tracker

　　在apollo-camera模块的tracker中主要包含两个子文件夹：common与omt。其中common文件夹下面主要是状态估计【kalman_filter】与相似特征【similar】文件。

common中状态估计【kalman_filter】文件包含多种滤波器类对视觉感知目标进行滤波，下面的类导图有画出。主要包含均值滤波、卡尔曼滤波、低通滤波等等。

common下的相似特征【similar】文件主要对图像块直接求取相似度，有cpu与cuda加速两个计算方式。

omt文件夹下面主要包含【omt_obstacle_tracker】、【obstacle_reference】、【target】三个文件。另外【track_object】文件只是简单声明追踪的存储数据结构。

omt下的【omt_obstacle_tracker】主要应用类或者成为接口类，包含Predict()、Associate2D()、Assoctate3D()等公有函数。从该类可以看出视觉障碍物追踪在数据关联阶段不仅仅使用2D信息同时也使用3D目标信息进行关联。

omt文件夹下的【target】是具体对目标状态估计与更新函数。上面的【omt_obstacle_tracker】的函数内部具体实现都会调用具体【target】内部的函数。例如：【omt_obstacle_tracker】下面的predict()会调用【target】的predict()函数。

omt文件下的【obstacle_reference】主要是通过结合相机参数与估计地面，随后通过obstacle_template来对目标进行一定程度的修正。

apollo相机模块的tracker单元测试【提前编译好可执行文件】用例：

// path : /apollo/modules/perception/camera/test
./camera_lib_obstacle_tracker_omt_omt_obstacle_tracker_test

camera模块之主要结构图

相机tracker的类导图：

具体tracker之间的代码流程走图：

　　上面基本上对camera的tracker有个简要介绍，可以得到基本了解。下面我们对【omt_obstacle_tracker】稍微有些细化介绍一下。

关于GenerateHypothesis函数是关联模块Associate2D()中核心函数之一，相关解释在注释中进行批注。

void OMTObstacleTracker::GenerateHypothesis(const TrackObjectPtrs &objects) {
  std::vector<Hypothesis> score_list;
  Hypothesis hypo;
  for (size_t i = 0; i < targets_.size(); ++i) {
    ADEBUG << "Target " << targets_[i].id;
    for (size_t j = 0; j < objects.size(); ++j) {
      hypo.target = static_cast<int>(i);
      hypo.object = static_cast<int>(j);
      // 计算targets_ 与 objects_ 之间的similar_map_相似度
      float sa = ScoreAppearance(targets_[i], objects[j]);
      // targets_与objects_之间的高斯motion分布值
      float sm = ScoreMotion(targets_[i], objects[j]);
      // shape之间的差距
      float ss = ScoreShape(targets_[i], objects[j]);
      // IoU
      float so = ScoreOverlap(targets_[i], objects[j]);
      // 加权获取hypo的value
      if (sa == 0) {
        hypo.score = omt_param_.weight_diff_camera().motion() * sm +
                     omt_param_.weight_diff_camera().shape() * ss +
                     omt_param_.weight_diff_camera().overlap() * so;
      } else {
        hypo.score = (omt_param_.weight_same_camera().appearance() * sa +
                      omt_param_.weight_same_camera().motion() * sm +
                      omt_param_.weight_same_camera().shape() * ss +
                      omt_param_.weight_same_camera().overlap() * so);
      }
      int change_from_type = static_cast<int>(targets_[i].type);
      int change_to_type = static_cast<int>(objects[j]->object->sub_type);
      // 这里存在类型转换矩阵,apollo里面有个先验矩阵值
      hypo.score += -kTypeAssociatedCost_[change_from_type][change_to_type];
      ADEBUG << "Detection " << objects[j]->indicator.frame_id << "(" << j
             << ") sa:" << sa << " sm: " << sm << " ss: " << ss << " so: " << so
             << " score: " << hypo.score;

      // 95.44% area is range [mu - sigma*2, mu + sigma*2]
      // don't match if motion is beyond the range
      if (sm < 0.045 || hypo.score < omt_param_.target_thresh()) {
        continue;
      }
      score_list.push_back(hypo);
    }
  }
  // 排序存储满足条件的score_list
  sort(score_list.begin(), score_list.end(), std::greater<Hypothesis>());
  std::vector<bool> used_target(targets_.size(), false);
  for (auto &pair : score_list) {
    if (used_target[pair.target] || used_[pair.object]) {
      continue;
    }
    Target &target = targets_[pair.target];
    auto det_obj = objects[pair.object];
    // 添加到target列表中
    target.Add(det_obj);
    used_[pair.object] = true;
    used_target[pair.target] = true;
    AINFO << "Target " << target.id << " match " << det_obj->indicator.frame_id
          << " (" << pair.object << ")"
          << "at " << pair.score << " size: " << target.Size();
  }
}

　　上述GenerateHypothesis中加权求取时候用到kTypeAssociatedCost_来索引类别转换的概率值，这部分主要在【omt_obstacle_tracker】初始化里面type_change_cost的变量获取如下的矩阵。你会发现对角线都为0主要类别没有进行变化，有些类别转换权重为1.0有些为0.2分别表示类型变化错误的概率。这部分从侧面说明目标检测中类型检出模糊性或者变化的可能性。例如：当前是追踪列表目标为车，关联时候为人的概率，这种变化很大，所以应该给大权重值。反之如果追踪列表目标为轿车，关联时候为SUV的概率，这种变化相对较小，所以可以给予较小的权重值。

0.0 0.0 0.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 0.0
0.0 0.0 0.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 0.0
0.0 0.0 0.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 0.0
1.0 1.0 1.0 0.0 0.0 0.0 0.0 1.0 1.0 0.2 1.0 1.0
1.0 1.0 1.0 0.0 0.0 0.0 0.0 1.0 1.0 0.2 1.0 1.0
1.0 1.0 1.0 0.0 0.0 0.0 0.0 1.0 1.0 0.2 1.0 1.0
1.0 1.0 1.0 0.0 0.0 0.0 0.0 1.0 1.0 0.2 1.0 1.0
1.0 1.0 1.0 1.0 1.0 1.0 1.0 0.0 0.0 0.2 0.1 1.0
1.0 1.0 1.0 1.0 1.0 1.0 1.0 0.0 0.0 0.2 0.1 1.0
1.0 1.0 1.0 0.2 0.2 0.2 0.2 0.2 0.2 0.0 0.2 1.0
1.0 1.0 1.0 1.0 1.0 1.0 1.0 0.1 0.1 0.2 0.0 1.0
0.0 0.0 0.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0 0.0

【omt_obstacle_tracker】关联模块Associate2D()

　　基本上Associate2D函数主要是对图像平面的目标进行关联，首先计算两帧图像直接的相似度。同时也使用相机坐标系下面的3D投影至图像平面框进行计算。关于计算方式主要参考GenerateHypothesis生成对应的阈值。随后，使用CombineDuplicateTargets()函数通过IoU进行重复剔除。这里面有个重点就是：每次在target更新时候，有两个类型需要进行更新UpdateType()与Update2D()是指类型与2D信息。

【omt_obstacle_tracker】关联模块Associate3D()这部分主要对target部分的3D目标信息进行更新。阅读target文件下面的Update3D()函数，主要是对world_center相关变量进行更新，对应状态估计是KalmanFilterConstVelocity。

【omt_obstacle_tracker】预测部分Predict()内部调用target的predict()函数。我们来看下target的predict()函数：

void Target::Predict(CameraFrame *frame) {
  auto delta_t =
      static_cast<float>(frame->timestamp - latest_object->timestamp);
  if (delta_t < 0) {
    return;
  }
  image_center.Predict(delta_t); // image_center的predict
  float acc_variance = target_param_.world_center().process_variance();
  float delta_t_2 = delta_t * delta_t;
  float pos_variance = 0.25f * acc_variance * delta_t_2 * delta_t_2;
  float vel_variance = acc_variance * delta_t_2;
  world_center.process_noise_(0, 0) = pos_variance;
  world_center.process_noise_(1, 1) = pos_variance;
  world_center.process_noise_(2, 2) = vel_variance;
  world_center.process_noise_(3, 3) = vel_variance;
  world_center.Predict(delta_t); // world_center 的predict

  // const position kalman predict
  world_center_const.process_noise_.setIdentity();
  world_center_const.process_noise_(0, 0) = vel_variance * delta_t_2;
  world_center_const.process_noise_(1, 1) =
      world_center_const.process_noise_(0, 0);
  world_center_const.Predict(delta_t); // world_center_const 的predict
}

　　上述代码可以清晰的看出，在target下的predict()函数，内部分别有3种预测方式。它们分别对应不同的状态估计方式，可以看target文件下的声明。同时，针对world_center相关信息变量，也分为运动or静止状态下的估计。

　　阅读到这里，可能你会疑惑？一个kalmanfilter不就可以搞定所有的相机状态变量了，为什么还需要如此多的滤波方式来监测更新呢？这个就是apollo相机模式下追踪的细节工程体现，其实你想想在图像平面下目标物体的wh与中心点center变化规律。然后世界坐标系下whl雨world_center的变化规律是否都变化率近似。答案是否定的，可以想象一下世界坐标系下物体center变化较whl变化更急剧一些，是否运动也会产生不同影响。所以，就有了KalmanFilterConstState与MaxNMeanFilter等。

小结

　　阅读完apollo相机模块下面的追踪代码，给我一个感觉就是细致。它把视觉感知下的目标物体变量拆分出来，根据各自的特性使用不同的状态估计来进行监测与更新。同时，还对相机的每帧成像进行监测与目标物体纠正等。仔细阅读target文件下面的策略，基本上就是相机模块下的tracker流程与技术细节。ok，希望你看过这篇对你阅读代码能够更清楚的梳理了一遍。