autoware.universe源码略读(3.15)--perception:object_merger

Overview

这个包的目的是为了通过数据关联，来进行检测到的物体之间的融合。可以看到针对这里的算法，应该是用到上一个里面使用到的SSP算法了

连续最短路径算法用于解决数据关联问题（最小成本流问题）。成本由两个对象之间的距离计算得出，门函数用于重置成本，包括最大距离、最大面积和最小面积。

所以这里的data_association和multi_object_tracker中的是一样的

node

（enum）MSG_COV_IDX

这里是把所有变量之间的协方差对应的索引都列出来了

（Class）ObjectAssociationMergerNode

这里的结构相对简单不少，直接就是这么一个节点类

（Func）isUnknownObjectOverlapped

这里是对UnkownObject先进行了一个单独处理，输入一个known_object，然后计算两者之间的距离，根据距离、precision、recall和IoU几种指标判断，是否是和已知的物体重叠的

（Func）convertListToClassMap

根据distance_threshold_list生成了一个map，把距离阈值和class_label对应了起来，比如上一个函数就需要这个东西作为输入

（mFunc）ObjectAssociationMergerNode::ObjectAssociationMergerNode

这里需要注意的是，每次只订阅了两个话题，就是每次输入的需要融合的两个物体

Name	Type	Description
input/object0	autoware_auto_perception_msgs::msg::DetectedObjects	detection objects
input/object1	autoware_auto_perception_msgs::msg::DetectedObjects	detection objects

然后加载的参数有

Name	Type	Description
can_assign_matrix	double	Assignment table for data association
max_dist_matrix	double	Maximum distance table for data association
max_area_matrix	double	Maximum area table for data association
min_area_matrix	double	Minimum area table for data association
max_rad_matrix	double	Maximum angle table for data association
base_link_frame_id	double	association frame
distance_threshold_list	std::vector<double>	Distance threshold for each class used in judging overlap. The class order depends on ObjectClassification.
generalized_iou_threshold	double	Generalized IoU threshold

这里首先用到了sync_这样一个变量，目的应该是为了进行一个简单的同步

sync_(SyncPolicy(10), object0_sub_, object1_sub_)
// Create publishers and subscribers
using std::placeholders::_1;
using std::placeholders::_2;
sync_.registerCallback(std::bind(&ObjectAssociationMergerNode::objectsCallback, this, _1, _2));

然后里面也是实例化了一个DataAssociation类

overlapped_judge_param_.distance_threshold_map =
  convertListToClassMap(declare_parameter<std::vector<double>>("distance_threshold_list"));

（mFunc）ObjectAssociationMergerNode::objectsCallback

先是把两个对象都转换到base_link下，然后直接调用数据关联

std::unordered_map<int, int> direct_assignment, reverse_assignment;
const auto & objects0 = transformed_objects0.objects;
const auto & objects1 = transformed_objects1.objects;
Eigen::MatrixXd score_matrix =
  data_association_->calcScoreMatrix(transformed_objects1, transformed_objects0);
data_association_->assign(score_matrix, direct_assignment, reverse_assignment);

接下来针对找到匹配的那些objects，根据设置的模式PriorityMode来决定输出的object

for (size_t object0_idx = 0; object0_idx < objects0.size(); ++object0_idx) {
  const auto & object0 = objects0.at(object0_idx);
  if (direct_assignment.find(object0_idx) != direct_assignment.end()) {  // found and merge
    const auto & object1 = objects1.at(direct_assignment.at(object0_idx));
    switch (priority_mode_) {
      case PriorityMode::Object0:
        output_msg.objects.push_back(object0);
        break;
      case PriorityMode::Object1:
        output_msg.objects.push_back(object1);
        break;
      case PriorityMode::Confidence:
        if (object1.existence_probability <= object0.existence_probability)
          output_msg.objects.push_back(object0);
        else
          output_msg.objects.push_back(object1);
        break;
    }
  } else {  // not found
    output_msg.objects.push_back(object0);
  }
}

然后再把没有匹配的情况下的objects1的对象也加到输出中

for (size_t object1_idx = 0; object1_idx < objects1.size(); ++object1_idx) {
  const auto & object1 = objects1.at(object1_idx);
  if (reverse_assignment.find(object1_idx) != reverse_assignment.end()) {  // found
  } else {                                                                 // not found
    output_msg.objects.push_back(object1);
  }
}

最后还对Unknown类型的物体进行了下处理，就是遍历用上边提到的isUnknownObjectOverlapped函数，最后把重叠的位置对象都给删除了，没什么好说的