V2X: add fusion configs (#12393)

* V2X: add fusion

* V2X: Style modification

* V2X: Style modification

* V2X: To -> 2

modules/v2x/conf/v2x_fusion_tracker.conf

+--config_path=/apollo/modules/v2x/data
+--fusion_conf_file=fusion_params.pt
+--input_conf_file=app_config.pt
+--mode=1

modules/v2x/dag/v2x_perception_fusion.dag

+# Define all coms in DAG streaming.
+module_config {
+    module_library : "/apollo/bazel-bin/modules/v2x/fusion/apps/libv2x_fusion_component.so"
+
+    components {
+      class_name : "V2XFusionComponent"
+      config {
+        name : "v2x_fusion"
+        flag_file_path : "/apollo/modules/v2x/conf/v2x_fusion_tracker.conf"
+        readers: [
+          {
+            channel: "/apollo/perception/obstacles"
+          }
+        ]
+      }
+    }
+}

modules/v2x/data/fusion_params.pt

+score_params {
+  prob_scale: 0.125
+  max_match_distance: 10
+  min_score: 0
+  use_mahalanobis_distance: true
+  check_type: false
+  confidence_level: C99P
+}
+

modules/v2x/fusion/apps/BUILD

+load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library", "cc_test")
+load("//tools:cpplint.bzl", "cpplint")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "v2x_fusion_component_lib",
+    srcs = ["v2x_fusion_component.cc"],
+    hdrs = ["v2x_fusion_component.h"],
+    copts = [
+        "-DMODULE_NAME=\\\"v2x_fusion\\\"",
+    ],
+    deps = [
+        "//cyber/common:file",
+        "//modules/common/math",
+        "//modules/v2x/fusion/apps/common:apps_common",
+        "//modules/v2x/fusion/configs:v2x_configs_lib",
+        "//modules/v2x/fusion/libs/common/base",
+        "//modules/v2x/fusion/libs/fusion:v2x_fusion_lib",
+        "@boost",
+    ],
+    alwayslink = True,
+)
+
+cc_binary(
+    name = "libv2x_fusion_component.so",
+    linkshared = True,
+    linkstatic = False,
+    deps = [":v2x_fusion_component_lib"],
+)
+
+cc_test(
+    name = "v2x_fusion_component_test",
+    size = "small",
+    srcs = ["v2x_fusion_component_test.cc"],
+    linkopts = [
+        "-lgomp",
+    ],
+    deps = [
+        ":v2x_fusion_component_lib",
+        "@com_google_googletest//:gtest_main",
+    ],
+)
+
+cpplint()

modules/v2x/fusion/apps/common/BUILD

+load("@rules_cc//cc:defs.bzl", "cc_library")
+load("//tools:cpplint.bzl", "cpplint")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "apps_common",
+    srcs = [
+        "trans_tools.cc",
+    ],
+    hdrs = [
+        "ft_definitions.h",
+        "trans_tools.h",
+    ],
+    copts = ['-DMODULE_NAME=\\"v2x_fusion\\"'],
+    deps = [
+        "//cyber",
+        "//modules/common/math",
+        "//modules/localization/proto:localization_cc_proto",
+        "//modules/perception/proto:perception_camera_cc_proto",
+        "//modules/v2x/fusion/libs/common/base",
+        "//modules/v2x/proto:v2x_obstacles_cc_proto",
+    ],
+)
+
+cpplint()

modules/v2x/fusion/apps/common/ft_definitions.h

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#pragma once
+
+#include <memory>
+
+#include "modules/localization/proto/localization.pb.h"
+#include "modules/perception/proto/perception_obstacle.pb.h"
+#include "modules/v2x/proto/v2x_obstacles.pb.h"
+
+#include "modules/v2x/fusion/libs/common/base/v2x_object.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+using apollo::localization::LocalizationEstimate;
+using apollo::perception::PerceptionObstacle;
+using apollo::perception::PerceptionObstacles;
+using apollo::v2x::V2XObstacle;
+using apollo::v2x::V2XObstacles;
+using apollo::v2x::ft::base::Object;
+
+using PerceptionObstaclesPtr = std::shared_ptr<PerceptionObstacles>;
+using V2XObstaclesPtr = std::shared_ptr<V2XObstacles>;
+using StatusPtr = std::shared_ptr<LocalizationEstimate>;
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/apps/common/trans_tools.cc

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "modules/v2x/fusion/apps/common/trans_tools.h"
+
+#include <iostream>
+#include <limits>
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+void Pb2Object(const PerceptionObstacle &obstacle, base::Object *object,
+                const std::string &frame_id, double timestamp_object) {
+  Eigen::Vector3d value;
+  Eigen::Matrix3d variance;
+  variance.setIdentity();
+  object->type_probs.push_back(0.85);
+  object->sub_type_probs.push_back(0.85);
+  object->type = static_cast<base::ObjectType>(obstacle.type());
+  variance = variance * 1;
+  switch (obstacle.sub_type()) {
+    case PerceptionObstacle::ST_UNKNOWN:
+      object->sub_type = base::ObjectSubType::UNKNOWN;
+      variance = variance * 3;
+      break;
+
+    case PerceptionObstacle::ST_UNKNOWN_MOVABLE:
+      object->sub_type = base::ObjectSubType::UNKNOWN_MOVABLE;
+      variance = variance * 2;
+      break;
+
+    case PerceptionObstacle::ST_CAR:
+      object->sub_type = base::ObjectSubType::CAR;
+      variance = variance * 0.8;
+      break;
+
+    case PerceptionObstacle::ST_VAN:
+      object->sub_type = base::ObjectSubType::VAN;
+      variance = variance * 1;
+      break;
+
+    case PerceptionObstacle::ST_TRUCK:
+      object->sub_type = base::ObjectSubType::TRUCK;
+      variance = variance * 3;
+      break;
+
+    case PerceptionObstacle::ST_BUS:
+      object->sub_type = base::ObjectSubType::BUS;
+      variance = variance * 3;
+      break;
+
+    case PerceptionObstacle::ST_CYCLIST:
+      object->sub_type = base::ObjectSubType::CYCLIST;
+      variance = variance * 0.8;
+      break;
+
+    case PerceptionObstacle::ST_MOTORCYCLIST:
+      object->sub_type = base::ObjectSubType::MOTORCYCLIST;
+      variance = variance * 0.8;
+      break;
+
+    case PerceptionObstacle::ST_TRICYCLIST:
+      object->sub_type = base::ObjectSubType::TRICYCLIST;
+      variance = variance * 0.8;
+      break;
+
+    case PerceptionObstacle::ST_PEDESTRIAN:
+      object->sub_type = base::ObjectSubType::PEDESTRIAN;
+      variance = variance * 0.8;
+      break;
+
+    case PerceptionObstacle::ST_TRAFFICCONE:
+      object->sub_type = base::ObjectSubType::TRAFFICCONE;
+      variance = variance * 0.8;
+      break;
+
+    default:
+      break;
+  }
+  if (obstacle.has_timestamp()) {
+    object->timestamp = obstacle.timestamp();
+  } else {
+    object->timestamp = timestamp_object;
+  }
+  value << obstacle.position().x(), obstacle.position().y(), 0.0;
+  object->position.Set(value, variance);
+  value << obstacle.velocity().x(), obstacle.velocity().y(),
+      obstacle.velocity().z();
+  object->velocity.Set(value, variance);
+  object->theta.Set(obstacle.theta(), 0.5);
+  object->sensor_type = base::SensorType::MONOCULAR_CAMERA;
+  object->track_id = obstacle.id();
+  object->frame_id = frame_id;
+  variance.setIdentity();
+  value << obstacle.length(), obstacle.width(), obstacle.height();
+  object->size.Set(value, variance);
+  std::vector<base::Info3d> polygon_info3d;
+  for (auto &polygon_point : obstacle.polygon_point()) {
+    base::Info3d point;
+    value << polygon_point.x(), polygon_point.y(), polygon_point.z();
+    point.Set(value, variance);
+    polygon_info3d.push_back(point);
+  }
+  object->polygon = polygon_info3d;
+}
+
+void V2xPb2Object(const apollo::v2x::V2XObstacle &obstacle,
+                   base::Object *object, const std::string &frame_id,
+                   double timestamp_object) {
+  Pb2Object(obstacle.perception_obstacle(), object, frame_id,
+             timestamp_object);
+  if (obstacle.has_v2x_info() && obstacle.v2x_info().v2x_type_size() > 0 &&
+      obstacle.v2x_info().v2x_type(0) ==
+          ::apollo::v2x::V2XInformation::ZOMBIES_CAR) {
+    object->v2x_type = base::V2xType::ZOMBIES_CAR;
+  }
+}
+
+base::Object Pb2Object(const PerceptionObstacle &obstacle,
+                        const std::string &frame_id) {
+  base::Object object;
+  Eigen::Vector3d value;
+  Eigen::Matrix3d variance;
+  variance.setIdentity();
+  object.timestamp = obstacle.timestamp();
+  // object
+  value << obstacle.position().x(), obstacle.position().y(),
+      obstacle.position().z();
+
+  object.position.Set(value, variance);
+  value << obstacle.velocity().x(), obstacle.velocity().y(),
+      obstacle.velocity().z();
+  object.velocity.Set(value, variance);
+  object.theta.Set(obstacle.theta(), 0.5);
+  object.sensor_type = base::SensorType::MONOCULAR_CAMERA;
+  object.track_id = obstacle.id();
+  object.frame_id = frame_id;
+  value << obstacle.length(), obstacle.width(), obstacle.height();
+  object.size.Set(value, variance);
+  object.type_probs.push_back(0.85);
+  object.sub_type_probs.push_back(0.85);
+  object.type = static_cast<base::ObjectType>(obstacle.type());
+  switch (obstacle.sub_type()) {
+    case PerceptionObstacle::ST_UNKNOWN:
+      object.sub_type = base::ObjectSubType::UNKNOWN;
+      break;
+
+    case PerceptionObstacle::ST_UNKNOWN_MOVABLE:
+      object.sub_type = base::ObjectSubType::UNKNOWN_MOVABLE;
+      break;
+
+    case PerceptionObstacle::ST_CAR:
+      object.sub_type = base::ObjectSubType::CAR;
+      break;
+
+    case PerceptionObstacle::ST_VAN:
+      object.sub_type = base::ObjectSubType::VAN;
+      break;
+
+    case PerceptionObstacle::ST_TRUCK:
+      object.sub_type = base::ObjectSubType::TRUCK;
+      break;
+
+    case PerceptionObstacle::ST_BUS:
+      object.sub_type = base::ObjectSubType::BUS;
+      break;
+
+    case PerceptionObstacle::ST_CYCLIST:
+      object.sub_type = base::ObjectSubType::CYCLIST;
+      break;
+
+    case PerceptionObstacle::ST_MOTORCYCLIST:
+      object.sub_type = base::ObjectSubType::MOTORCYCLIST;
+      break;
+
+    case PerceptionObstacle::ST_TRICYCLIST:
+      object.sub_type = base::ObjectSubType::TRICYCLIST;
+      break;
+
+    case PerceptionObstacle::ST_PEDESTRIAN:
+      object.sub_type = base::ObjectSubType::PEDESTRIAN;
+      break;
+
+    case PerceptionObstacle::ST_TRAFFICCONE:
+      object.sub_type = base::ObjectSubType::TRAFFICCONE;
+      break;
+
+    default:
+      break;
+  }
+  return object;
+}
+
+PerceptionObstacle Object2Pb(const base::Object &object) {
+  PerceptionObstacle obstacle;
+  // times
+  obstacle.set_timestamp(object.timestamp);
+  // id
+  obstacle.set_id(object.track_id);
+  // position
+  obstacle.mutable_position()->set_x(object.position.x());
+  obstacle.mutable_position()->set_y(object.position.y());
+  obstacle.mutable_position()->set_z(object.position.z());
+  // velocity
+  obstacle.mutable_velocity()->set_x(object.velocity.x());
+  obstacle.mutable_velocity()->set_y(object.velocity.y());
+  obstacle.mutable_velocity()->set_z(object.velocity.z());
+  // yaw
+  obstacle.set_theta(object.theta.Value());
+  // lwh
+  obstacle.set_length(object.size.length());
+  obstacle.set_width(object.size.width());
+  obstacle.set_height(object.size.height());
+  obstacle.set_type(static_cast<PerceptionObstacle::Type>(object.type));
+  switch (object.sub_type) {
+    case base::ObjectSubType::UNKNOWN:
+      obstacle.set_sub_type(PerceptionObstacle::ST_UNKNOWN);
+      break;
+
+    case base::ObjectSubType::UNKNOWN_MOVABLE:
+      obstacle.set_sub_type(PerceptionObstacle::ST_UNKNOWN_MOVABLE);
+      break;
+
+    case base::ObjectSubType::CAR:
+      obstacle.set_sub_type(PerceptionObstacle::ST_CAR);
+      break;
+
+    case base::ObjectSubType::VAN:
+      obstacle.set_sub_type(PerceptionObstacle::ST_VAN);
+      break;
+
+    case base::ObjectSubType::TRUCK:
+      obstacle.set_sub_type(PerceptionObstacle::ST_TRUCK);
+      break;
+
+    case base::ObjectSubType::BUS:
+      obstacle.set_sub_type(PerceptionObstacle::ST_BUS);
+      break;
+
+    case base::ObjectSubType::CYCLIST:
+      obstacle.set_sub_type(PerceptionObstacle::ST_CYCLIST);
+      break;
+
+    case base::ObjectSubType::MOTORCYCLIST:
+      obstacle.set_sub_type(PerceptionObstacle::ST_MOTORCYCLIST);
+      break;
+
+    case base::ObjectSubType::TRICYCLIST:
+      obstacle.set_sub_type(PerceptionObstacle::ST_TRICYCLIST);
+      break;
+
+    case base::ObjectSubType::PEDESTRIAN:
+      obstacle.set_sub_type(PerceptionObstacle::ST_PEDESTRIAN);
+      break;
+
+    case base::ObjectSubType::TRAFFICCONE:
+      obstacle.set_sub_type(PerceptionObstacle::ST_TRAFFICCONE);
+      break;
+
+    default:
+      break;
+  }
+  return obstacle;
+}
+
+void FillObjectPolygonFromBBox3D(PerceptionObstacle *object_ptr) {
+  struct PolygoPoint {
+    double x = 0.0;
+    double y = 0.0;
+    double z = 0.0;
+  };
+
+  if (!object_ptr) {
+    return;
+  }
+  const double length = object_ptr->length();
+  const double width = object_ptr->width();
+  double hl = length / 2;
+  double hw = width / 2;
+  double cos_theta = std::cos(object_ptr->theta());
+  double sin_theta = std::sin(object_ptr->theta());
+  PolygoPoint polygon[4];
+  polygon[0].x = hl * cos_theta - hw * sin_theta + object_ptr->position().x();
+  polygon[0].y = hl * sin_theta + hw * cos_theta + object_ptr->position().y();
+  polygon[0].z = object_ptr->position().z();
+  polygon[1].x = hl * cos_theta + hw * sin_theta + object_ptr->position().x();
+  polygon[1].y = hl * sin_theta - hw * cos_theta + object_ptr->position().y();
+  polygon[1].z = object_ptr->position().z();
+  polygon[2].x = -hl * cos_theta + hw * sin_theta + object_ptr->position().x();
+  polygon[2].y = -hl * sin_theta - hw * cos_theta + object_ptr->position().y();
+  polygon[2].z = object_ptr->position().z();
+  polygon[3].x = -hl * cos_theta - hw * sin_theta + object_ptr->position().x();
+  polygon[3].y = -hl * sin_theta + hw * cos_theta + object_ptr->position().y();
+  polygon[3].z = object_ptr->position().z();
+  for (PolygoPoint point : polygon) {
+    auto polygon_point = object_ptr->add_polygon_point();
+    polygon_point->set_x(point.x);
+    polygon_point->set_y(point.y);
+    polygon_point->set_z(point.z);
+  }
+}
+
+void Object2Pb(const base::Object &object, PerceptionObstacle *obstacle) {
+  // times
+  obstacle->set_timestamp(object.timestamp);
+  // id
+  obstacle->set_id(object.track_id);
+  // position
+  obstacle->mutable_position()->set_x(object.position.x());
+  obstacle->mutable_position()->set_y(object.position.y());
+  obstacle->mutable_position()->set_z(object.position.z());
+  // velocity
+  obstacle->mutable_velocity()->set_x(object.velocity.x());
+  obstacle->mutable_velocity()->set_y(object.velocity.y());
+  obstacle->mutable_velocity()->set_z(object.velocity.z());
+  // yaw
+  obstacle->set_theta(object.theta.Value());
+  // lwh
+  obstacle->set_length(object.size.length());
+  obstacle->set_width(object.size.width());
+  obstacle->set_height(object.size.height());
+  FillObjectPolygonFromBBox3D(obstacle);
+  obstacle->set_type(static_cast<PerceptionObstacle::Type>(object.type));
+  switch (object.sub_type) {
+    case base::ObjectSubType::UNKNOWN:
+      obstacle->set_sub_type(PerceptionObstacle::ST_UNKNOWN);
+      break;
+
+    case base::ObjectSubType::UNKNOWN_MOVABLE:
+      obstacle->set_sub_type(PerceptionObstacle::ST_UNKNOWN_MOVABLE);
+      break;
+
+    case base::ObjectSubType::UNKNOWN_UNMOVABLE:
+      obstacle->set_sub_type(PerceptionObstacle::ST_UNKNOWN_UNMOVABLE);
+      break;
+
+    case base::ObjectSubType::CAR:
+      obstacle->set_sub_type(PerceptionObstacle::ST_CAR);
+      break;
+
+    case base::ObjectSubType::VAN:
+      obstacle->set_sub_type(PerceptionObstacle::ST_VAN);
+      break;
+
+    case base::ObjectSubType::TRUCK:
+      obstacle->set_sub_type(PerceptionObstacle::ST_TRUCK);
+      break;
+
+    case base::ObjectSubType::BUS:
+      obstacle->set_sub_type(PerceptionObstacle::ST_BUS);
+      break;
+
+    case base::ObjectSubType::CYCLIST:
+      obstacle->set_sub_type(PerceptionObstacle::ST_CYCLIST);
+      break;
+
+    case base::ObjectSubType::MOTORCYCLIST:
+      obstacle->set_sub_type(PerceptionObstacle::ST_MOTORCYCLIST);
+      break;
+
+    case base::ObjectSubType::TRICYCLIST:
+      obstacle->set_sub_type(PerceptionObstacle::ST_TRICYCLIST);
+      break;
+
+    case base::ObjectSubType::PEDESTRIAN:
+      obstacle->set_sub_type(PerceptionObstacle::ST_PEDESTRIAN);
+      break;
+
+    case base::ObjectSubType::TRAFFICCONE:
+      obstacle->set_sub_type(PerceptionObstacle::ST_TRAFFICCONE);
+      break;
+
+    default:
+      break;
+  }
+  obstacle->set_source(PerceptionObstacle::HOST_VEHICLE);
+  if (object.v2x_type == base::V2xType::ZOMBIES_CAR) {
+    obstacle->mutable_v2x_info()->add_v2x_type(
+        ::apollo::perception::V2XInformation::ZOMBIES_CAR);
+    obstacle->set_source(PerceptionObstacle::V2X);
+  }
+  if (object.v2x_type == base::V2xType::BLIND_ZONE) {
+    obstacle->mutable_v2x_info()->add_v2x_type(
+        ::apollo::perception::V2XInformation::BLIND_ZONE);
+    obstacle->set_source(PerceptionObstacle::V2X);
+  }
+}
+
+void Object2V2xPb(const base::Object &object, V2XObstacle *obstacle) {
+  PerceptionObstacle perception_obstacle;
+  Object2Pb(object, &perception_obstacle);
+  obstacle->mutable_perception_obstacle()->CopyFrom(perception_obstacle);
+}
+
+double Pbs2Objects(const PerceptionObstacles &obstacles,
+                    std::vector<base::Object> *objects, std::string frame_id) {
+  double timestamp = std::numeric_limits<double>::max();
+  objects->clear();
+  if (frame_id == "") {
+    frame_id = obstacles.header().frame_id();
+  }
+  if (obstacles.header().module_name() == "perception_obstacle") {
+    frame_id = "VEHICLE";
+  }
+  double timestamp_object = 0.0;
+  if (obstacles.perception_obstacle_size() > 0 &&
+      obstacles.perception_obstacle(0).has_timestamp() == false) {
+    if (obstacles.header().has_camera_timestamp() &&
+        obstacles.header().camera_timestamp() > 10.0) {
+      timestamp_object = obstacles.header().camera_timestamp() / 1.0e9;
+    } else {
+      timestamp_object = obstacles.header().lidar_timestamp() / 1.0e9;
+    }
+  }
+  for (int j = 0; j < obstacles.perception_obstacle_size(); ++j) {
+    base::Object object;
+    Pb2Object(obstacles.perception_obstacle(j), &object, frame_id,
+               timestamp_object);
+    objects->push_back(object);
+    if (timestamp > object.timestamp) {
+      timestamp = object.timestamp;
+    }
+  }
+
+  return timestamp;
+}
+
+void CarstatusPb2Object(const LocalizationEstimate &carstatus,
+                         base::Object *object, const std::string &frame_id) {
+  Eigen::Vector3d value;
+  Eigen::Matrix3d variance;
+  variance.setIdentity();
+  object->type_probs.push_back(0.85);
+  object->sub_type_probs.push_back(0.85);
+  object->type = base::ObjectType::VEHICLE;
+  object->sub_type = base::ObjectSubType::CAR;
+  object->v2x_type = base::V2xType::HOST_VEHICLE;
+  variance = variance * 0.8;
+  value << carstatus.pose().position().x(), carstatus.pose().position().y(),
+      0.0;
+  object->position.Set(value, variance);
+  value << carstatus.pose().linear_velocity().x(),
+      carstatus.pose().linear_velocity().y(),
+      carstatus.pose().linear_velocity().z();
+  object->velocity.Set(value, variance);
+  object->theta.Set(carstatus.pose().heading(), 0.5);
+  object->sensor_type = base::SensorType::MONOCULAR_CAMERA;
+  object->track_id = 0;
+  object->frame_id = frame_id;
+  variance.setIdentity();
+  value << 5.02203, 2.13135, 2.17711;
+  object->size.Set(value, variance);
+  object->timestamp = carstatus.header().timestamp_sec();
+}
+
+double CarstatusPb2Objects(const LocalizationEstimate &carstatus,
+                            std::vector<base::Object> *objects,
+                            std::string frame_id) {
+  double timestamp = std::numeric_limits<double>::max();
+  objects->clear();
+  base::Object object;
+  CarstatusPb2Object(carstatus, &object, frame_id);
+  objects->push_back(object);
+  if (timestamp > carstatus.header().timestamp_sec()) {
+    timestamp = carstatus.header().timestamp_sec();
+  }
+  return timestamp;
+}
+
+double V2xPbs2Objects(const V2XObstacles &obstacles,
+                       std::vector<base::Object> *objects,
+                       std::string frame_id) {
+  double timestamp = std::numeric_limits<double>::max();
+  objects->clear();
+  double timestamp_object = 0.0;
+  if (obstacles.v2x_obstacle_size() > 0 &&
+      obstacles.v2x_obstacle(0).perception_obstacle().has_timestamp() ==
+          false) {
+    if (obstacles.header().has_camera_timestamp()) {
+      timestamp_object = obstacles.header().camera_timestamp() / 1000000000.0;
+    } else {
+      timestamp_object = obstacles.header().lidar_timestamp() / 1000000000.0;
+    }
+  }
+  if (frame_id == "") {
+    frame_id = obstacles.header().frame_id();
+  }
+  for (int j = 0; j < obstacles.v2x_obstacle_size(); ++j) {
+    base::Object object;
+    V2xPb2Object(obstacles.v2x_obstacle(j), &object, frame_id,
+                  timestamp_object);
+    objects->push_back(object);
+    if (timestamp > object.timestamp) {
+      timestamp = object.timestamp;
+    }
+  }
+
+  return timestamp;
+}
+
+void Objects2Pbs(const std::vector<base::Object> &objects,
+                  std::shared_ptr<PerceptionObstacles> obstacles) {
+  obstacles->mutable_perception_obstacle()->Clear();
+  if (objects.size() < 1) {
+    return;
+  }
+  // obstacles->mutable_header()->set_frame_id(objects[0].frame_id);
+  for (const auto &object : objects) {
+    PerceptionObstacle obstacle;
+    Object2Pb(object, &obstacle);
+    obstacles->add_perception_obstacle()->CopyFrom(obstacle);
+  }
+}
+
+void Objects2V2xPbs(const std::vector<base::Object> &objects,
+                     std::shared_ptr<V2XObstacles> obstacles) {
+  obstacles->mutable_v2x_obstacle()->Clear();
+  if (objects.size() < 1) {
+    return;
+  }
+  for (const auto &object : objects) {
+    if (object.v2x_type == base::V2xType::HOST_VEHICLE) {
+      continue;
+    }
+    V2XObstacle obstacle;
+    Object2V2xPb(object, &obstacle);
+    obstacles->add_v2x_obstacle()->CopyFrom(obstacle);
+  }
+}
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/apps/common/trans_tools.h

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#pragma once
+
+#include <cmath>
+#include <memory>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include "modules/v2x/fusion/apps/common/ft_definitions.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+base::Object Pb2Object(const PerceptionObstacle &obstacle,
+                        const std::string &frame_id);
+void Pb2Object(const PerceptionObstacle &obstacle, base::Object *object,
+                const std::string &frame_id, double timestamp_object = 0.0);
+
+void V2xPb2Object(const V2XObstacle &obstacle, base::Object *object,
+                   const std::string &frame_id, double timestamp_object);
+
+void CarstatusPb2Object(const LocalizationEstimate &carstatus,
+                         base::Object *object, const std::string &frame_id);
+
+double Pbs2Objects(const PerceptionObstacles &obstacles,
+                    std::vector<base::Object> *objects,
+                    std::string frame_id = "");
+
+double V2xPbs2Objects(const V2XObstacles &obstacles,
+                       std::vector<base::Object> *objects,
+                       std::string frame_id);
+
+double CarstatusPb2Objects(const LocalizationEstimate &carstatus,
+                            std::vector<base::Object> *objects,
+                            std::string frame_id);
+
+PerceptionObstacle Object2Pb(const base::Object &object);
+void FillObjectPolygonFromBBox3D(PerceptionObstacle *object_ptr);
+void Object2Pb(const base::Object &object, PerceptionObstacle *obstacle);
+
+void Object2V2xPb(const base::Object &object, V2XObstacle *obstacle);
+void Objects2Pbs(const std::vector<base::Object> &objects,
+                  std::shared_ptr<PerceptionObstacles> obstacles);
+
+void Objects2V2xPbs(const std::vector<base::Object> &objects,
+                     std::shared_ptr<V2XObstacles> obstacles);
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/apps/v2x_fusion_component.cc

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "modules/v2x/fusion/apps/v2x_fusion_component.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+V2XFusionComponent::~V2XFusionComponent() {}
+
+std::string V2XFusionComponent::Name() const { return FLAGS_v2x_module_name; }
+
+bool V2XFusionComponent::Init() {
+  v2x_obstacles_reader_ =
+      node_->CreateReader<V2XObstacles>(FLAGS_v2x_obstacles_topic, nullptr);
+  localization_reader_ = node_->CreateReader<LocalizationEstimate>(
+      FLAGS_localization_topic, nullptr);
+  perception_fusion_obstacles_writer_ =
+      node_->CreateWriter<PerceptionObstacles>(
+          FLAGS_v2x_fusion_obstacles_topic);
+  return true;
+}
+
+bool V2XFusionComponent::Proc(
+    const std::shared_ptr<PerceptionObstacles>& perception_obstacles) {
+  if (FLAGS_use_v2x) {
+    return V2XMessageFusionProcess(perception_obstacles);
+  }
+  perception_fusion_obstacles_writer_->Write(*perception_obstacles);
+  return true;
+}
+
+bool V2XFusionComponent::V2XMessageFusionProcess(
+    const std::shared_ptr<PerceptionObstacles>& perception_obstacles) {
+  // Read localization info. and call OnLocalization to update
+  // the PoseContainer.
+  localization_reader_->Observe();
+  auto localization_msg = localization_reader_->GetLatestObserved();
+  if (localization_msg == nullptr) {
+    AERROR << "V2X: cannot receive any localization message.";
+    return false;
+  }
+  base::Object hv_obj;
+  CarstatusPb2Object(*localization_msg, &hv_obj, "VEHICLE");
+  v2x_obstacles_reader_->Observe();
+  auto v2x_obstacles_msg = v2x_obstacles_reader_->GetLatestObserved();
+  if (v2x_obstacles_msg == nullptr) {
+    AERROR << "V2X: cannot receive any v2x obstacles message.";
+    perception_fusion_obstacles_writer_->Write(*perception_obstacles);
+  } else {
+    header_.CopyFrom(perception_obstacles->header());
+    std::vector<Object> fused_objects;
+    std::vector<std::vector<Object>> fusion_result;
+    std::vector<Object> v2x_objects;
+    V2xPbs2Objects(*v2x_obstacles_msg, &v2x_objects, "V2X");
+    std::vector<Object> perception_objects;
+    Pbs2Objects(*perception_obstacles, &perception_objects);
+    perception_objects.push_back(hv_obj);
+    fusion_.CombineNewResource(perception_objects, &fused_objects,
+                               &fusion_result);
+    fusion_.CombineNewResource(v2x_objects, &fused_objects, &fusion_result);
+    // auto output_msg = std::make_shared<V2XObstacles>();
+    auto output_msg = std::make_shared<PerceptionObstacles>();
+    SerializeMsg(fused_objects, output_msg);
+    perception_fusion_obstacles_writer_->Write(*output_msg);
+  }
+  return true;
+}
+
+void V2XFusionComponent::SerializeMsg(
+    const std::vector<base::Object>& objects,
+    std::shared_ptr<PerceptionObstacles> output_msg) {
+  static int seq_num = 0;
+  Objects2Pbs(objects, output_msg);
+  apollo::common::Header* header = output_msg->mutable_header();
+  header->set_timestamp_sec(apollo::cyber::Time::Now().ToSecond());
+  header->set_module_name("v2x_fusion");
+  header->set_sequence_num(seq_num++);
+  header->set_lidar_timestamp(header_.lidar_timestamp());
+  header->set_camera_timestamp(header_.camera_timestamp());
+  header->set_radar_timestamp(header_.radar_timestamp());
+  output_msg->set_error_code(apollo::common::ErrorCode::PERCEPTION_ERROR_NONE);
+}
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/apps/v2x_fusion_component.h

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ */
+
+#include <cmath>
+#include <condition_variable>
+#include <limits>
+#include <memory>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include "cyber/component/component.h"
+#include "cyber/cyber.h"
+#include "modules/v2x/fusion/apps/common/trans_tools.h"
+#include "modules/v2x/fusion/configs/ft_config_manager.h"
+#include "modules/v2x/fusion/configs/fusion_tracker_gflags.h"
+#include "modules/v2x/fusion/libs/fusion/fusion.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+class V2XFusionComponent
+    : public apollo::cyber::Component<PerceptionObstacles> {
+ public:
+  ~V2XFusionComponent();
+
+  std::string Name() const;
+
+  bool Init() override;
+
+  bool Proc(const std::shared_ptr<PerceptionObstacles>& perception_obstacles)
+      override;
+
+ private:
+  bool V2XMessageFusionProcess(
+      const std::shared_ptr<PerceptionObstacles>& perception_obstacles);
+  void SerializeMsg(const std::vector<base::Object>& objects,
+                    std::shared_ptr<PerceptionObstacles> obstacles);
+  Fusion fusion_;
+
+  std::shared_ptr<apollo::cyber::Reader<LocalizationEstimate>>
+      localization_reader_;
+  std::shared_ptr<apollo::cyber::Reader<V2XObstacles>> v2x_obstacles_reader_;
+  std::shared_ptr<apollo::cyber::Writer<PerceptionObstacles>>
+      perception_fusion_obstacles_writer_;
+  apollo::common::Header header_;
+};
+
+CYBER_REGISTER_COMPONENT(V2XFusionComponent)
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/apps/v2x_fusion_component_test.cc

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#include "modules/v2x/fusion/apps/v2x_fusion_component.h"
+
+#include "gtest/gtest.h"
+
+#include "cyber/init.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+TEST(V2XFusionComponentTest, Simple) {
+  cyber::Init("v2x_fusion_component_test");
+  V2XFusionComponent v2x_fusion_component;
+  EXPECT_EQ(v2x_fusion_component.Name(), "v2x_fusion");
+}
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/configs/BUILD

+load("@rules_cc//cc:defs.bzl", "cc_library", "cc_test")
+load("//tools:cpplint.bzl", "cpplint")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "v2x_configs_lib",
+    deps = [
+        ":ft_config_manager",
+        ":fusion_tracker_gflags",
+    ],
+)
+
+cc_library(
+    name = "fusion_tracker_gflags",
+    srcs = [
+        "fusion_tracker_gflags.cc",
+    ],
+    hdrs = [
+        "fusion_tracker_gflags.h",
+    ],
+    deps = [
+        "@com_github_gflags_gflags//:gflags",
+    ],
+)
+
+cc_library(
+    name = "ft_config_manager",
+    srcs = [
+        "ft_config_manager.cc",
+    ],
+    hdrs = [
+        "ft_config_manager.h",
+    ],
+    deps = [
+        ":fusion_tracker_gflags",
+        "//cyber",
+        "//modules/v2x/proto:fusion_params_cc_proto",
+    ],
+)
+
+cc_test(
+    name = "ft_config_manager_test",
+    size = "small",
+    srcs = ["ft_config_manager_test.cc"],
+    deps = [
+        ":v2x_configs_lib",
+        "@com_google_googletest//:gtest_main",
+    ],
+)
+
+cpplint()

modules/v2x/fusion/configs/ft_config_manager.cc

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "modules/v2x/fusion/configs/ft_config_manager.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+FTConfigManager::FTConfigManager() {}
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/configs/ft_config_manager.h

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#pragma once
+
+#include <map>
+#include <string>
+
+#include <boost/function.hpp>
+
+#include "modules/v2x/proto/fusion_params.pb.h"
+
+#include "cyber/cyber.h"
+#include "modules/v2x/fusion/configs/fusion_tracker_gflags.h"
+
+#define PLUGIN_PARAMS(classname, conf_file, prototype)                       \
+  class classname {                                                          \
+   public:                                                                   \
+    classname() {                                                            \
+      file_path_ = FLAGS_config_path + "/" + conf_file;                      \
+      cyber::common::GetProtoFromFile(file_path_, &params);                  \
+    }                                                                        \
+    ~classname() { cyber::common::SetProtoToASCIIFile(params, file_path_); } \
+    prototype params;                                                        \
+                                                                             \
+   private:                                                                  \
+    std::string file_path_;                                                  \
+  };
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+PLUGIN_PARAMS(FusionParams, FLAGS_fusion_conf_file, fusion::FusionParams)
+
+class FTConfigManager {
+ public:
+  DECLARE_SINGLETON(FTConfigManager)
+ public:
+  FusionParams fusion_params_;
+};
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/configs/ft_config_manager_test.cc

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#include "modules/v2x/fusion/configs/ft_config_manager.h"
+
+#include "gtest/gtest.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+TEST(FTConfigManager, read_and_write) {
+  FTConfigManager* ft_config_manager_ptr = FTConfigManager::Instance();
+  auto& fusion_params = ft_config_manager_ptr->fusion_params_.params;
+  EXPECT_FALSE(fusion_params.score_params().check_type());
+  EXPECT_EQ(fusion_params.score_params().prob_scale(), 0.125);
+}
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/configs/fusion_tracker_gflags.cc

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * File: fusion_gflags.cpp
+ * Brief: Global flags definition.
+ */
+
+#include "modules/v2x/fusion/configs/fusion_tracker_gflags.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+// config manager
+DEFINE_string(config_manager_path, "./conf", "The ModelConfig config paths.");
+DEFINE_string(work_root, "", "Project work root direcotry.");
+DEFINE_string(config_path, "/apollo/modules/v2x/data",
+              "ADU shared data path, including maps, routings...");
+DEFINE_string(v2x_module_name, "v2x_fusion", "name");
+DEFINE_string(localization_topic, "/apollo/localization/pose",
+              "localization topic name");
+DEFINE_string(v2x_obstacles_topic, "/apollo/v2x/original_obstacles",
+              "perception obstacle topic name");
+DEFINE_string(v2x_fusion_obstacles_topic, "/apollo/msf/obstacles",
+              "perception obstacle topic name");
+DEFINE_bool(use_v2x, false, "use v2x");
+// fusion
+DEFINE_string(fusion_conf_file, "fusion_params.pt", "the roi conf path");
+// app
+DEFINE_string(app_conf_file, "app_config.pt", "the inputs conf path");
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/configs/fusion_tracker_gflags.h

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * File: fusion_gflags.h
+ * Brief: Global flags definition.
+ */
+
+#pragma once
+
+#include "gflags/gflags.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+// config manager
+DECLARE_string(config_manager_path);
+DECLARE_string(work_root);
+DECLARE_string(config_path);
+DECLARE_string(v2x_module_name);
+DECLARE_string(localization_topic);
+DECLARE_string(v2x_obstacles_topic);
+DECLARE_string(v2x_fusion_obstacles_topic);
+DECLARE_bool(use_v2x);
+// fusion
+DECLARE_string(fusion_conf_file);
+// app
+DECLARE_string(app_conf_file);
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/libs/common/base/BUILD

+load("@rules_cc//cc:defs.bzl", "cc_library", "cc_test")
+load("//tools:cpplint.bzl", "cpplint")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "base",
+    hdrs = [
+        "v2x_object.h",
+    ],
+    deps = [
+        "//modules/perception/base:base_type",
+        "//modules/perception/base:box",
+        "//modules/perception/base:object",
+        "@com_google_absl//absl/strings",
+    ],
+)
+
+cc_test(
+    name = "v2x_object_test",
+    size = "small",
+    srcs = ["v2x_object_test.cc"],
+    deps = [
+        ":base",
+        "@com_google_googletest//:gtest_main",
+    ],
+)
+
+cpplint()

modules/v2x/fusion/libs/common/base/v2x_object.h

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "absl/strings/str_cat.h"
+
+#include "modules/perception/base/object_supplement.h"
+#include "modules/perception/base/object_types.h"
+#include "modules/perception/base/sensor_meta.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+namespace base {
+
+using apollo::perception::base::CameraObjectSupplement;
+using apollo::perception::base::FusionObjectSupplement;
+using apollo::perception::base::ObjectSubType;
+using apollo::perception::base::ObjectType;
+using apollo::perception::base::SensorType;
+
+enum class V2xType {
+  UNKNOWN = 0,
+  ZOMBIES_CAR = 1,
+  BLIND_ZONE = 2,
+  HOST_VEHICLE = 13,
+};
+
+enum class MessageType {
+  UNKNOWN_MESSAGE_TYPE = -1,
+  ROADSIDE = 0,
+  VEHICLE = 1,
+};
+
+// Value and Variance
+template <typename Val, typename Var>
+class Info {
+ public:
+  Info() = default;
+  Info(Val val, Var var) {
+    value_ = val;
+    variance_ = var;
+  }
+  ~Info() = default;
+  // Info(const Info &) = delete;
+  Info(const Info &rhs) {
+    value_ = rhs.value_;
+    variance_ = rhs.variance_;
+  }
+  // Info &operator=(const Info &) = delete;
+  Info &operator=(const Info &rhs) {
+    value_ = rhs.value_;
+    variance_ = rhs.variance_;
+    return *this;
+  }
+
+  void Set(Val value, Var variance) {
+    value_ = value;
+    variance_ = variance;
+  }
+  Val Value() const { return value_; }
+
+  Var Variance() const { return variance_; }
+
+ protected:
+  Val value_;
+  Var variance_;
+};
+
+class Info3f : public Info<Eigen::Vector3f, Eigen::Matrix3f> {
+ public:
+  Info3f() {
+    value_.setZero();
+    variance_.setZero();
+  }
+  ~Info3f() = default;
+
+  float x() const { return value_(0); }
+
+  float y() const { return value_(1); }
+
+  float z() const { return value_(2); }
+
+  float length() const { return value_(0); }
+
+  float width() const { return value_(1); }
+
+  float height() const { return value_(2); }
+};
+
+class Info3d : public Info<Eigen::Vector3d, Eigen::Matrix3d> {
+ public:
+  double x() const { return value_(0); }
+
+  double y() const { return value_(1); }
+
+  double z() const { return value_(2); }
+
+  double length() const { return value_(0); }
+
+  double width() const { return value_(1); }
+
+  double height() const { return value_(2); }
+};
+
+struct alignas(16) Object {
+  typedef Info<bool, float> Infob;
+  typedef Info<float, float> Infof;
+  typedef Info<double, double> Infod;
+  typedef Info<Eigen::Vector2f, Eigen::Matrix2f> Info2f;
+  typedef Info<Eigen::Vector2d, Eigen::Matrix2d> Info2d;
+  typedef Eigen::Vector2f Point2f;
+  typedef Eigen::Vector3f Point3f;
+  Object() : is_temporary_lost(false, 0.0) {}
+  ~Object() = default;
+  Object(const Object &) = default;
+  Object &operator=(const Object &) = default;
+  bool operator<(const Object &rhs) const { return timestamp < rhs.timestamp; }
+  bool operator>(const Object &rhs) const { return timestamp > rhs.timestamp; }
+  bool operator==(const Object &rhs) const {
+    return timestamp == rhs.timestamp;
+  }
+  std::string ToString() const;
+  void Reset();
+  // camera, lidar, radar and others
+  SensorType sensor_type = SensorType::UNKNOWN_SENSOR_TYPE;
+  // ROADSIDE
+  MessageType message_type = MessageType::UNKNOWN_MESSAGE_TYPE;
+  // @brief sensor-specific object supplements, optional
+  CameraObjectSupplement camera_supplement;
+  FusionObjectSupplement fusion_supplement;
+  // message timestamp
+  double timestamp = 0.0;
+  // original sensor timestamp
+  double sensor_timestamp = 0.0;
+  // @brief age of the tracked object, required
+  double tracking_time = 0.0;
+  // @brief timestamp of latest measurement, required
+  double latest_tracked_time = 0.0;
+  std::string frame_id = "";
+  // @brief track id, required
+  int track_id = -1;
+  // @breif object id per frame, required
+  int global_id = -1;
+  // @brief center position of the boundingbox (x, y, z), required
+  Info3d position;
+  // @brief anchor point, required
+  Info3d anchor_point;
+  // 0-2Pi from east
+  Infod theta;
+  // @brief theta variance, required
+  Infod theta_variance;
+  // @brief main direction of the object, required
+  Info3d direction;
+  /* @brief size = [length, width, height] of boundingbox
+     length is the size of the main direction, required
+  */
+  Info3d size;
+  // @brief convex hull of the object, required
+  // Point3f polygon;
+  // @brief object type, required
+  ObjectType type = ObjectType::UNKNOWN;
+
+  V2xType v2x_type = V2xType::UNKNOWN;
+  // @brief type variance, required
+  double type_variance = 1.0;
+  // @brief probability for each type, required
+  std::vector<float> type_probs;
+  // @brief object sub-type, optional
+  ObjectSubType sub_type = ObjectSubType::UNKNOWN;
+  // @brief probability for each sub-type, optional
+  std::vector<float> sub_type_probs;
+
+  std::vector<std::vector<Info3d>> tentacles;
+
+  std::vector<Info3d> polygon;
+
+  // tracking information
+  // @brief the variance of tracked
+  Infod track_variance;
+  // @brief velocity of the object, required
+  Info3d velocity;
+  // @brief acceleration of the object, required
+  Info3d acceleration;
+  // @brief motion state of the tracked object, required
+  Infob is_stationary;
+
+  Infob is_temporary_lost;
+  std::string DebugString() const {
+    return absl::StrCat("id: ", track_id, ", ",              //
+                        "time: ", timestamp, ", ",           //
+                        "sensor type: ", sensor_type, ", ",  //
+                        "x: ", position.x(), ", ",           //
+                        "y: ", position.y(), ", ",           //
+                        "vx: ", velocity.x(), ", ",          //
+                        "vy: ", velocity.y(), ", ",          //
+                        "yaw: ", theta.Value());
+  }
+};
+
+typedef std::shared_ptr<Object> ObjectPtr;
+typedef std::shared_ptr<const Object> ObjectConstPtr;
+
+struct alignas(16) ObjectList {
+  ObjectList() = default;
+  ~ObjectList() = default;
+  ObjectList(const ObjectList &) = default;
+  ObjectList &operator=(const ObjectList &) = default;
+  bool operator<(const ObjectList &rhs) const {
+    return timestamp < rhs.timestamp;
+  }
+  bool operator>(const ObjectList &rhs) const {
+    return timestamp > rhs.timestamp;
+  }
+  bool operator==(const ObjectList &rhs) const {
+    return timestamp == rhs.timestamp;
+  }
+
+  SensorType sensor_type;
+  // @brief sensor-specific object supplements, optional
+  CameraObjectSupplement camera_supplement;
+  // LidarObjectSupplement lidar_supplement;
+  // RadarObjectSupplement radar_supplement;
+  FusionObjectSupplement fusion_supplement;
+
+  // message timestamp
+  double timestamp = 0.0;
+  // original sensor timestamp
+  double sensor_timestamp = 0.0;
+  // @brief age of the tracked object, require
+  std::string frame_id = "";
+
+  std::vector<ObjectPtr> objects;
+};
+
+typedef std::shared_ptr<ObjectList> ObjectListPtr;
+typedef std::shared_ptr<const ObjectList> ObjectListConstPtr;
+
+}  // namespace base
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/libs/common/base/v2x_object_test.cc

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#include "modules/v2x/fusion/libs/common/base/v2x_object.h"
+
+#include "gtest/gtest.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+namespace base {
+
+TEST(Object, operators) {
+  std::vector<Object> objects;
+  Object object;
+  object.timestamp = 4.1;
+  object.track_id = 0;
+  objects.push_back(object);
+  object.timestamp = 3.2;
+  object.track_id = 1;
+  objects.push_back(object);
+  object.timestamp = 2.3;
+  object.track_id = 2;
+  objects.push_back(object);
+  object.timestamp = 1.4;
+  object.track_id = 3;
+  objects.push_back(object);
+  sort(objects.begin(), objects.end(), std::less<Object>());
+  EXPECT_EQ(objects.at(0).track_id, 3);
+}
+
+}  // namespace base
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/libs/fusion/BUILD

+load("@rules_cc//cc:defs.bzl", "cc_library", "cc_test")
+load("//tools:cpplint.bzl", "cpplint")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "v2x_fusion_lib",
+    srcs = [
+        "fusion.cc",
+    ],
+    hdrs = [
+        "fusion.h",
+        "km.h",
+        "test_tools.h",
+    ],
+    copts = ['-DMODULE_NAME=\\"v2x_fusion\\"'],
+    deps = [
+        "//cyber",
+        "//modules/common/math",
+        "//modules/v2x/fusion/configs:v2x_configs_lib",
+        "//modules/v2x/fusion/libs/common/base",
+    ],
+)
+
+cc_test(
+    name = "fusion_test",
+    size = "small",
+    srcs = ["fusion_test.cc"],
+    deps = [
+        ":v2x_fusion_lib",
+        "@com_google_googletest//:gtest_main",
+    ],
+)
+
+cc_test(
+    name = "km_test",
+    size = "small",
+    srcs = ["km_test.cc"],
+    deps = [
+        ":v2x_fusion_lib",
+        "@com_google_googletest//:gtest_main",
+    ],
+)
+
+cpplint()

modules/v2x/fusion/libs/fusion/fusion.cc

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "modules/v2x/fusion/libs/fusion/fusion.h"
+
+#include <cmath>
+#include <utility>
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+Fusion::Fusion() {
+  ft_config_manager_ptr_ = FTConfigManager::Instance();
+
+  score_params_ = ft_config_manager_ptr_->fusion_params_.params.score_params();
+  switch (score_params_.confidence_level()) {
+    case fusion::ConfidenceLevel::C90P:
+      m_matched_dis_limit_ = std::sqrt(4.605);
+      break;
+    case fusion::ConfidenceLevel::C95P:
+      m_matched_dis_limit_ = std::sqrt(5.991);
+      break;
+    case fusion::ConfidenceLevel::C975P:
+      m_matched_dis_limit_ = std::sqrt(7.378);
+      break;
+    case fusion::ConfidenceLevel::C99P:
+      m_matched_dis_limit_ = std::sqrt(9.210);
+      break;
+    default:
+      break;
+  }
+}
+
+bool Fusion::Init() {
+  last_timestamp_ = -1.0;
+  return true;
+}
+
+bool Fusion::Proc(
+    const std::vector<std::vector<base::Object>> &input_objectlists,
+    double timestamp) {
+  fusion_result_.clear();
+  updated_objects_.clear();
+  for (unsigned int i = 0; i < input_objectlists.size(); ++i) {
+    CombineNewResource(input_objectlists[i]);
+  }
+  return true;
+}
+
+bool Fusion::CombineNewResource(const std::vector<base::Object> &new_objects) {
+  return CombineNewResource(new_objects, &updated_objects_, &fusion_result_);
+}
+
+bool Fusion::CombineNewResource(
+    const std::vector<base::Object> &new_objects,
+    std::vector<base::Object> *fused_objects,
+    std::vector<std::vector<base::Object>> *fusion_result) {
+  if (new_objects.empty()) {
+    return false;
+  }
+  if (fused_objects->size() < 1) {
+    fused_objects->assign(new_objects.begin(), new_objects.end());
+    for (unsigned int j = 0; j < new_objects.size(); ++j) {
+      std::vector<base::Object> matched_objects;
+      matched_objects.push_back(new_objects[j]);
+      fusion_result->push_back(matched_objects);
+    }
+    return true;
+  }
+  int u_num = fused_objects->size();
+  int v_num = new_objects.size();
+  Eigen::MatrixXf association_mat(u_num, v_num);
+  ComputeAssociateMatrix(*fused_objects, new_objects, &association_mat);
+  std::vector<std::pair<int, int>> match_cps;
+  if (u_num > v_num) {
+    km_matcher_.GetKMResult(association_mat.transpose(), &match_cps, true);
+  } else {
+    km_matcher_.GetKMResult(association_mat, &match_cps, false);
+  }
+  for (auto it = match_cps.begin(); it != match_cps.end(); it++) {
+    if (it->second != -1) {
+      if (it->first == -1) {
+        fused_objects->push_back(new_objects[it->second]);
+        std::vector<base::Object> matched_objects;
+        matched_objects.push_back(fused_objects->back());
+        fusion_result->push_back(matched_objects);
+      } else {
+        (*fusion_result)[it->first].push_back(new_objects[it->second]);
+      }
+    }
+  }
+  return true;
+}
+
+double Fusion::CheckOdistance(const base::Object &in1_ptr,
+                              const base::Object &in2_ptr) {
+  double xi = in1_ptr.position.x();
+  double yi = in1_ptr.position.y();
+  double xj = in2_ptr.position.x();
+  double yj = in2_ptr.position.y();
+  double distance = std::hypot(xi - xj, yi - yj);
+  return distance;
+}
+
+bool Fusion::CheckDisScore(const base::Object &in1_ptr,
+                           const base::Object &in2_ptr, double *score) {
+  double dis = CheckOdistance(in1_ptr, in2_ptr);
+  *score = 2.5 * std::max(0.0, score_params_.max_match_distance() - dis);
+  return true;
+}
+
+bool Fusion::CheckTypeScore(const base::Object &in1_ptr,
+                            const base::Object &in2_ptr, double *score) {
+  double same_prob = 0;
+  if (in1_ptr.sub_type == in2_ptr.sub_type) {
+    same_prob =
+        1 - (1 - in1_ptr.sub_type_probs[0]) * (1 - in2_ptr.sub_type_probs[0]);
+  } else if (in1_ptr.type == in2_ptr.type ||
+             in1_ptr.type == v2x::ft::base::ObjectType::UNKNOWN ||
+             in2_ptr.type == v2x::ft::base::ObjectType::UNKNOWN) {
+    same_prob =
+        (1 - in1_ptr.sub_type_probs.at(0)) * in2_ptr.sub_type_probs.at(0) +
+        (1 - in2_ptr.sub_type_probs.at(0)) * in1_ptr.sub_type_probs.at(0);
+    same_prob *= score_params_.prob_scale();
+  }
+  *score *= same_prob;
+  return true;
+}
+
+bool Fusion::ComputeAssociateMatrix(
+    const std::vector<base::Object> &in1_objects,  // fused
+    const std::vector<base::Object> &in2_objects,  // new
+    Eigen::MatrixXf *association_mat) {
+  for (unsigned int i = 0; i < in1_objects.size(); ++i) {
+    for (unsigned int j = 0; j < in2_objects.size(); ++j) {
+      const base::Object &obj1_ptr = in1_objects[i];
+      const base::Object &obj2_ptr = in2_objects[j];
+      double score = 0;
+      if (!CheckDisScore(obj1_ptr, obj2_ptr, &score)) {
+        AERROR << "V2X Fusion: check dis score failed";
+      }
+      if (score_params_.check_type() &&
+          !CheckTypeScore(obj1_ptr, obj2_ptr, &score)) {
+        AERROR << "V2X Fusion: check type failed";
+      }
+      (*association_mat)(i, j) =
+          (score >= score_params_.min_score()) ? score : 0;
+    }
+  }
+  return true;
+}
+
+int Fusion::DeleteRedundant(std::vector<base::Object> *objects) {
+  std::vector<unsigned int> to_be_deleted;
+  for (unsigned int i = 0; i < objects->size(); ++i) {
+    for (unsigned int j = i + 1; j < objects->size(); ++j) {
+      double distance = CheckOdistance(objects->at(i), objects->at(j));
+      if (distance < 1) {
+        to_be_deleted.push_back(j);
+      }
+    }
+  }
+  for (auto iter = to_be_deleted.rbegin(); iter != to_be_deleted.rend();
+       ++iter) {
+    objects->erase(objects->begin() + *iter);
+  }
+  return to_be_deleted.size();
+}
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/libs/fusion/fusion.h

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#pragma once
+
+#include <unistd.h>
+
+#include <algorithm>
+#include <cmath>
+#include <cstdlib>
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "modules/v2x/fusion/configs/ft_config_manager.h"
+#include "modules/v2x/fusion/libs/common/base/v2x_object.h"
+#include "modules/v2x/fusion/libs/fusion/km.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+class Fusion {
+ public:
+  Fusion();
+  ~Fusion() {}
+  bool Proc(const std::vector<std::vector<base::Object>> &input_objectlists,
+            double timestamp);
+  bool Init();
+  std::vector<base::Object> &get_fused_objects() { return updated_objects_; }
+  std::vector<std::vector<base::Object>> &get_fusion_result() {
+    return fusion_result_;
+  }
+  bool CombineNewResource(
+      const std::vector<base::Object> &new_objects,
+      std::vector<base::Object> *fused_objects,
+      std::vector<std::vector<base::Object>> *fusion_result);
+  int DeleteRedundant(std::vector<base::Object> *objects);
+
+ private:
+  bool CombineNewResource(const std::vector<base::Object> &new_objects);
+  bool ComputeAssociateMatrix(const std::vector<base::Object> &in1_objects,
+                              const std::vector<base::Object> &in2_objects,
+                              Eigen::MatrixXf *association_mat);
+  double CheckOdistance(const base::Object &in1_ptr,
+                        const base::Object &in2_ptr);
+  bool CheckDisScore(const base::Object &in1_ptr, const base::Object &in2_ptr,
+                     double *score);
+  bool CheckTypeScore(const base::Object &in1_ptr, const base::Object &in2_ptr,
+                      double *score);
+  double last_timestamp_;
+  const double MAX_SCORE = 250000;
+  float m_matched_dis_limit_;
+  FTConfigManager *ft_config_manager_ptr_;
+  KMkernal km_matcher_;
+  fusion::ScoreParams score_params_;
+  std::vector<std::vector<base::Object>> fusion_result_;
+  std::vector<base::Object> updated_objects_;
+};
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/libs/fusion/fusion_test.cc

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#include "modules/v2x/fusion/libs/fusion/fusion.h"
+
+#include <fstream>
+#include <iostream>
+
+#include "gtest/gtest.h"
+
+#include "modules/v2x/fusion/libs/fusion/test_tools.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+TEST(Fusion, get_km_result) {
+  Fusion fusion;
+  std::vector<base::Object> fused_objects;
+  std::vector<std::vector<base::Object>> fusion_result;
+  std::vector<base::Object> objects1;
+  std::vector<base::Object> objects2;
+  std::vector<base::Object> objects3;
+  std::vector<base::Object> objects4;
+  EXPECT_FALSE(
+      fusion.CombineNewResource(objects1, &fused_objects, &fusion_result));
+  LoadData("/apollo/modules/v2x/fusion/test_data/fusion_object1", &objects1,
+           "camera1");
+  LoadData("/apollo/modules/v2x/fusion/test_data/fusion_object2", &objects2,
+           "camera2");
+  LoadData("/apollo/modules/v2x/fusion/test_data/fusion_object3", &objects3,
+           "camera3");
+  LoadData("/apollo/modules/v2x/fusion/test_data/fusion_object4", &objects4,
+           "camera4");
+  EXPECT_TRUE(
+      fusion.CombineNewResource(objects1, &fused_objects, &fusion_result));
+  EXPECT_TRUE(
+      fusion.CombineNewResource(objects2, &fused_objects, &fusion_result));
+  EXPECT_TRUE(
+      fusion.CombineNewResource(objects3, &fused_objects, &fusion_result));
+  EXPECT_TRUE(
+      fusion.CombineNewResource(objects4, &fused_objects, &fusion_result));
+  EXPECT_GE(fused_objects.size(), 9);
+  EXPECT_LE(fused_objects.size(), 12);
+}
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/libs/fusion/km.h

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#pragma once
+
+#include <unistd.h>
+
+#include <algorithm>
+#include <cmath>
+#include <cstdlib>
+#include <iostream>
+#include <set>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+
+#define USR_INF 999999999999
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+class KMkernal {
+ public:
+  KMkernal() = default;
+  ~KMkernal() = default;
+  template <typename T>
+  bool GetKMResult(const T& association_mat,
+                   std::vector<std::pair<int, int>>* match_cps,
+                   bool need_reverse = false);
+
+ private:
+  int u_size_;
+  int v_size_;
+  double* ex_u_;
+  double* ex_v_;
+  int* v_matched_;
+  double* v_slack_;
+  std::set<int> used_u_;
+  std::set<int> used_v_;
+  template <typename T>
+  bool FindCP(const T& mat, int i);
+};
+template <typename T>
+bool KMkernal::GetKMResult(const T& association_mat,
+                           std::vector<std::pair<int, int>>* match_cps,
+                           bool need_reverse) {
+  match_cps->clear();
+  u_size_ = association_mat.rows();
+  v_size_ = association_mat.cols();
+  if (u_size_ > v_size_) return false;
+  ex_u_ = new double[u_size_];
+  ex_v_ = new double[v_size_];
+  v_matched_ = new int[v_size_];
+  std::fill(v_matched_, v_matched_ + v_size_, -1);
+  memset(ex_v_, 0, v_size_ * sizeof(double));
+  for (int i = 0; i < u_size_; ++i) {
+    ex_u_[i] = association_mat(i, 0);
+    for (int j = 1; j < v_size_; ++j) {
+      ex_u_[i] = std::max(static_cast<float>(ex_u_[i]), association_mat(i, j));
+    }
+  }
+  for (int i = 0; i < u_size_; ++i) {
+    if (ex_u_[i] <= 0) {
+      if (need_reverse)
+        match_cps->push_back(std::make_pair(-1, i));
+      else
+        match_cps->push_back(std::make_pair(i, -1));
+      continue;
+    }
+    v_slack_ = new double[v_size_];
+    std::fill(v_slack_, v_slack_ + v_size_, USR_INF);
+    while (1) {
+      used_u_.clear();
+      used_v_.clear();
+      if (FindCP(association_mat, i)) break;
+      double d = USR_INF;
+      for (int j = 0; j < v_size_; ++j)
+        if (used_v_.find(j) == used_v_.end()) d = std::min(d, v_slack_[j]);
+      for (auto it = used_u_.begin(); it != used_u_.end(); it++) {
+        ex_u_[*it] -= d;
+      }
+      for (int j = 0; j < v_size_; ++j) {
+        if (used_v_.find(j) != used_v_.end())
+          ex_v_[j] += d;
+        else
+          v_slack_[j] -= d;
+      }
+    }
+    delete[] v_slack_;
+  }
+  if (need_reverse) {
+    for (int j = 0; j < v_size_; ++j) {
+      if (v_matched_[j] == -1) {
+        match_cps->push_back(std::make_pair(j, -1));
+      } else if (association_mat(v_matched_[j], j) > 0) {
+        match_cps->push_back(std::make_pair(j, v_matched_[j]));
+      } else {
+        match_cps->push_back(std::make_pair(-1, v_matched_[j]));
+        match_cps->push_back(std::make_pair(j, -1));
+      }
+    }
+  } else {
+    for (int j = 0; j < v_size_; ++j) {
+      if (v_matched_[j] == -1) {
+        match_cps->push_back(std::make_pair(-1, j));
+      } else if (association_mat(v_matched_[j], j) > 0) {
+        match_cps->push_back(std::make_pair(v_matched_[j], j));
+      } else {
+        match_cps->push_back(std::make_pair(v_matched_[j], -1));
+        match_cps->push_back(std::make_pair(-1, j));
+      }
+    }
+  }
+  delete[] ex_u_;
+  delete[] ex_v_;
+  delete[] v_matched_;
+  return true;
+}
+
+template <typename T>
+bool KMkernal::FindCP(const T& mat, int i) {
+  used_u_.insert(i);
+  for (int j = 0; j < v_size_; ++j) {
+    if (used_v_.find(j) != used_v_.end()) {
+      continue;
+    }
+    double gap = ex_u_[i] + ex_v_[j] - mat(i, j);
+    if (gap <= 0) {
+      // res = 0;
+      used_v_.insert(j);
+      bool match_success = v_matched_[j] == -1 || FindCP(mat, v_matched_[j]);
+      if (match_success) {
+        v_matched_[j] = i;
+        return true;
+      }
+    } else {
+      v_slack_[j] = std::min(v_slack_[j], gap);
+    }
+  }
+  return false;
+}
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/libs/fusion/km_test.cc

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#include "modules/v2x/fusion/libs/fusion/km.h"
+
+#include "gtest/gtest.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+TEST(KMkernal, get_km_result) {
+  KMkernal km_matcher;
+  Eigen::MatrixXf association_mat(5, 4);
+  association_mat << 50, 34, 0, 0, 10, 0, 17, 0, 0, 31, 18, 10, 0, 0, 8, 17, 0,
+      0, 0, 2;
+  std::vector<std::pair<int, int>> match_cps;
+  EXPECT_FALSE(km_matcher.GetKMResult(association_mat, &match_cps));
+  EXPECT_TRUE(
+      km_matcher.GetKMResult(association_mat.transpose(), &match_cps, true));
+}
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/libs/fusion/test_tools.h

+/******************************************************************************
+ * Copyright 2020 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include <fstream>
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include "modules/v2x/fusion/libs/common/base/v2x_object.h"
+
+namespace apollo {
+namespace v2x {
+namespace ft {
+
+bool LoadData(const std::string& file, std::vector<base::Object>* objects,
+              const std::string& frame_id) {
+  std::fstream fin;
+  fin.open(file.c_str(), std::ios::in);
+  if (!fin) {
+    return false;
+  }
+  std::string line;
+  while (getline(fin, line)) {
+    std::istringstream iss(line);
+    float sub_type_probs, x, y, z, xx, xy, xz, yx, yy, yz, zx, zy, zz;
+    Eigen::Vector3d pos;
+    Eigen::Matrix3d var;
+    int type, sub_type;
+    iss >> type >> sub_type >> sub_type_probs >> x >> y >> z >> xx >> xy >>
+        xz >> yx >> yy >> yz >> zx >> zy >> zz;
+    pos << x, y, z;
+    var << xx, xy, xz, yx, yy, yz, zx, zy, zz;
+    base::Object obj;
+    obj.type = base::ObjectType::VEHICLE;
+    Eigen::Vector3d car_size, bus_size, van_size;
+    Eigen::Matrix3d id_var;
+    car_size << 4.2, 2.0, 1.8;
+    bus_size << 12, 2.2, 3;
+    van_size << 4.5, 2.1, 2;
+    id_var << 1, 0, 0, 0, 1, 0, 0, 0, 1;
+    switch (sub_type) {
+      case 3:
+        obj.sub_type = base::ObjectSubType::CAR;
+        obj.size.Set(car_size, id_var);
+        break;
+      case 4:
+        obj.sub_type = base::ObjectSubType::VAN;
+        obj.size.Set(van_size, id_var);
+        break;
+      case 5:
+        obj.sub_type = base::ObjectSubType::BUS;
+        obj.size.Set(bus_size, id_var);
+        break;
+      default:
+        break;
+    }
+    obj.sensor_type = base::SensorType::MONOCULAR_CAMERA;
+    obj.frame_id = frame_id;
+    obj.position.Set(pos, var);
+    obj.theta.Set(0, 1);
+    obj.type_probs.push_back(0.9f);
+    obj.sub_type_probs.push_back(sub_type_probs);
+    objects->push_back(obj);
+  }
+  return true;
+}
+
+}  // namespace ft
+}  // namespace v2x
+}  // namespace apollo

modules/v2x/fusion/test_data/app_config.pt

+input {
+  input_type: OLD
+  seq: 0
+  channel_name: "/perception/obstacles_old"
+}
+input {
+  input_type: CAMERA
+  seq: 1
+  channel_name: "/perception/obstacles_camera1"
+}
+input {
+  input_type: CAMERA
+  seq: 2
+  channel_name: "/perception/obstacles_camera2"
+}
+input {
+  input_type: CAMERA
+  seq: 3
+  channel_name: "/perception/obstacles_camera3"
+}
+input {
+  input_type: CAMERA
+  seq: 4
+  channel_name: "/perception/obstacles_camera4"
+}
+input {
+  input_type: CAMERA
+  seq: 5
+  channel_name: "/perception/obstacles_camera5"
+}
+input {
+  input_type: CAMERA
+  seq: 6
+  channel_name: "/perception/obstacles_camera6"
+}
+input {
+  input_type: CAMERA
+  seq: 7
+  channel_name: "/perception/obstacles_camera7"
+}
+input {
+  input_type: CAMERA
+  seq: 8
+  channel_name: "/perception/obstacles_camera8"
+}
+output_name: "/perception/obstacles"
+

modules/v2x/fusion/test_data/benchmark

+5 3 10 20 0
+5 3 20 20 0
+5 5 40 20 0
+5 3 40 28 0
+5 5 40 40 0
+5 3 50 30 0
+5 3 50 50 0
+5 3 52 58 0
+5 3 52 80 0
+5 3 62 60 0

modules/v2x/fusion/test_data/fusion_object1

+5 3 0.9 9.31946521 18.30961269 0 1.87197597 -0.43041365 0 -0.43041365 2.70802403 0 0 0 1
+5 3 0.92 19.89205768 19.74458419 0 1.87197597 -0.43041365 0 -0.43041365 2.70802403 0 0 0 1
+5 3 0.9 40.86857973 29.44662098 0 1.87197597 -0.43041365 0 -0.43041365 2.70802403 0 0 0 1
+5 4 0.2 48.96013459 30.90766418 0 1.87197597 -0.43041365 0 -0.43041365 2.70802403 0 0 0 1
+5 3 0.9 51.71806427 48.35927642 0 1.87197597 -0.43041365 0 -0.43041365 2.70802403 0 0 0 1
+5 3 0.9 53.62918569 57.52490467 0 1.87197597 -0.43041365 0 -0.43041365 2.70802403 0 0 0 1
+5 3 0.9 52.26166017 81.807556 0 1.87197597 -0.43041365 0 -0.43041365 2.70802403 0 0 0 1
+5 5 0.1 60.41045885 59.87098393 0 1.87197597 -0.43041365 0 -0.43041365 2.70802403 0 0 0 1
\ No newline at end of file

modules/v2x/fusion/test_data/fusion_object2

+5 3 0.9 11.61865708 18.75230918 0 1.51751643 -0.49272486 0 -0.49272486 1.68748357 0 0 0 1
+5 3 0.9 21.48883758 17.21299772 0 1.51751643 -0.49272486 0 -0.49272486 1.68748357 0 0 0 1
+5 3 0.1 41.3889516 19.32466078 0 1.51751643 -0.49272486 0 -0.49272486 1.68748357 0 0 0 1
+5 3 0.9 42.67292524 27.95346616 0 1.51751643 -0.49272486 0 -0.49272486 1.68748357 0 0 0 1
+5 3 0.9 50.4440022 29.66052762 0 1.51751643 -0.49272486 0 -0.49272486 1.68748357 0 0 0 1
+5 3 0.9 49.87589212 50.78223584 0 1.51751643 -0.49272486 0 -0.49272486 1.68748357 0 0 0 1
+5 3 0.9 61.7858522 61.33488594 0 1.51751643 -0.49272486 0 -0.49272486 1.68748357 0 0 0 1
\ No newline at end of file

modules/v2x/fusion/test_data/fusion_object3

+5 3 0.9 7.51697628 20.11302683 0 4.80338629 -0.36491583 0 -0.36491583 1.20301371 0 0 0 1
+5 3 0.9 18.76581442 20.3855129 0 4.80338629 -0.36491583 0 -0.36491583 1.20301371 0 0 0 1
+5 3 0.1 40.26919145 39.59741714 0 4.80338629 -0.36491583 0 -0.36491583 1.20301371 0 0 0 1
+5 3 0.9 54.46195886 30.55647023 0 4.80338629 -0.36491583 0 -0.36491583 1.20301371 0 0 0 1
+5 3 0.9 51.48225631 58.84663973 0 4.80338629 -0.36491583 0 -0.36491583 1.20301371 0 0 0 1
+5 3 0.9 51.05921321 77.69595062 0 4.80338629 -0.36491583 0 -0.36491583 1.20301371 0 0 0 1
\ No newline at end of file

modules/v2x/fusion/test_data/fusion_object4

+5 3 0.9 19.45528984 20.64674959 0 1.89160066 1.5809713 0 1.5809713 3.12089934 0 0 0 1
+5 3 0.1 40.27277858 27.25332916 0 1.89160066 1.5809713 0 1.5809713 3.12089934 0 0 0 1
+5 5 0.1 48.94089065 29.8692135 0 1.89160066 1.5809713 0 1.5809713 3.12089934 0 0 0 1
+5 3 0.9 51.67702261 79.53939934 0 1.89160066 1.5809713 0 1.5809713 3.12089934 0 0 0 1
+5 3 0.9 60.15997698 59.60247437 0 1.89160066 1.5809713 0 1.5809713 3.12089934 0 0 0 1
\ No newline at end of file

modules/v2x/fusion/test_data/fusion_params.pt

+score_params {
+  prob_scale: 0.125
+  max_match_distance: 10
+  min_score: 0
+  use_mahalanobis_distance: true
+  check_type: true
+  confidence_level: C975P
+}
+

modules/v2x/fusion/test_data/fusion_params_wrong.pt

+score_params {
+  prob_scales: 0.125
+  max_match_distance: 10
+  min_score: 0
+  use_mahalanobis_distance: true
+  check_type: true
+}
+

modules/v2x/proto/BUILD

@@ -248,3 +248,32 @@ py_proto_library(
         "//modules/common/proto:header_py_pb2",
     ],
 )
+
+cc_proto_library(
+    name = "fusion_params_cc_proto",
+    deps = [
+        ":fusion_params_lib",
+    ],
+)
+
+proto_library(
+    name = "fusion_params_lib",
+    srcs = ["fusion_params.proto"],
+)
+
+cc_proto_library(
+    name = "v2x_obstacles_cc_proto",
+    deps = [
+        ":v2x_obstacles_proto_lib",
+    ],
+)
+
+proto_library(
+    name = "v2x_obstacles_proto_lib",
+    srcs = ["v2x_obstacles.proto"],
+    deps = [
+        "//modules/common/proto:error_code_proto",
+        "//modules/common/proto:header_proto",
+        "//modules/perception/proto:perception_obstacle_proto",
+    ],
+)

modules/v2x/proto/fusion_params.proto

+syntax = "proto2";
+
+package apollo.v2x.ft.fusion;
+
+message KMParams {
+  optional double min_score = 1;
+}
+
+message Param {
+  optional double k = 1;
+  optional double b = 2;
+}
+
+message SingleCameraParam {
+  optional int32 camera_id = 1;
+  repeated Param param = 2;
+}
+enum ConfidenceLevel {
+  C90P = 0;
+  C95P = 1;
+  C975P = 2;
+  C99P = 3;
+}
+message ScoreParams {
+  optional double prob_scale = 1;
+  optional double max_match_distance = 2;
+  optional double min_score = 3;
+  optional bool use_mahalanobis_distance = 4 [default = false];
+  optional bool check_type = 5 [default = false];
+  optional ConfidenceLevel confidence_level = 6 [default = C975P];
+}
+
+message FusionParams {
+  repeated SingleCameraParam fusion_params= 1;
+  optional KMParams km_params = 2;
+  required ScoreParams score_params = 3;
+}

modules/v2x/proto/v2x_obstacles.proto

+syntax = "proto2";
+
+package apollo.v2x;
+
+import "modules/common/proto/header.proto";
+import "modules/common/proto/error_code.proto";
+import "modules/perception/proto/perception_obstacle.proto";
+
+message Point {
+  optional double x = 1;  // in meters.
+  optional double y = 2;  // in meters.
+  optional double z = 3;  // height in meters.
+}
+
+message MiniAreaMap {
+  optional bytes rscu_id = 1;  // RSCU id.
+  optional Point feature_position = 2; // the centre of intersection or sstraight area
+  optional Point start_position = 3; // the centre of intersection or sstraight area
+  optional Point end_position = 4; // the centre of intersection or sstraight area
+}
+
+message AbnormalInformation {
+  optional double average_speed = 1;
+  optional double vehicle_density = 2;
+}
+
+message V2XInformation {
+  enum V2XType {
+      NONE = 0;
+      ZOMBIES_CAR = 1;
+      BLIND_ZONE = 2;
+  };
+  repeated V2XType v2x_type = 1;  // use case type
+  optional Point traffic_event_start = 3;  // congestion  or others initial position.
+  optional Point traffic_event_start_error = 4; // congestion or others initial position error.
+  optional Point traffic_event_end = 5;  // congestion or others end position.
+  optional Point traffic_event_end_error = 6; // congestion or others end position error.
+  optional AbnormalInformation abnormal_info = 7; // abnormal info
+}
+
+message V2XObstacle {
+  optional apollo.perception.PerceptionObstacle perception_obstacle = 1;  // obstacle.
+  optional V2XInformation v2x_info = 2; // v2x use case info
+}
+
+message V2XObstacles {
+  repeated V2XObstacle v2x_obstacle = 1;  // An array of obstacles
+  optional MiniAreaMap area_map = 2; // Mini area map
+  optional double traffic_flow = 3; // Traffic flow
+  optional apollo.common.Header header = 4;  // Header
+  optional apollo.common.ErrorCode error_code = 5 [default = OK]; // perception error code
+}