feat(autoware_multi_object_tracker): selective update per channel

perception/autoware_multi_object_tracker/CMakeLists.txt

@@ -41,7 +41,6 @@ set(${PROJECT_NAME}_lib
   lib/tracker/model/tracker_base.cpp
   lib/tracker/model/vehicle_tracker.cpp
   lib/tracker/model/multiple_vehicle_tracker.cpp
-  lib/tracker/model/bicycle_tracker.cpp
   lib/tracker/model/pedestrian_tracker.cpp
   lib/tracker/model/pedestrian_and_bicycle_tracker.cpp
   lib/tracker/model/unknown_tracker.cpp

perception/autoware_multi_object_tracker/config/input_channels.param.yaml

@@ -3,80 +3,134 @@
     input_channels:
       detected_objects:
         topic: "/perception/object_recognition/detection/objects"
-        can_spawn_new_tracker: true
         optional:
           name: "detected_objects"
           short_name: "all"
       # LIDAR - rule-based
       lidar_clustering:
         topic: "/perception/object_recognition/detection/clustering/objects"
-        can_spawn_new_tracker: true
+        flags:
+          can_spawn_new_tracker: true
+          can_trust_existence_probability: false
+          can_trust_extension: false
+          can_trust_classification: false
+          can_trust_orientation: false
         optional:
           name: "clustering"
           short_name: "Lcl"
       # LIDAR - DNN
       lidar_centerpoint:
         topic: "/perception/object_recognition/detection/centerpoint/objects"
-        can_spawn_new_tracker: true
+        flags:
+          can_spawn_new_tracker: true
+          can_trust_existence_probability: true
+          can_trust_extension: true
+          can_trust_classification: true
+          can_trust_orientation: true
         optional:
           name: "centerpoint"
           short_name: "Lcp"
       lidar_centerpoint_validated:
         topic: "/perception/object_recognition/detection/centerpoint/validation/objects"
-        can_spawn_new_tracker: true
+        flags:
+          can_spawn_new_tracker: true
+          can_trust_existence_probability: true
+          can_trust_extension: true
+          can_trust_classification: true
+          can_trust_orientation: true
         optional:
           name: "centerpoint"
           short_name: "Lcp"
       lidar_apollo:
         topic: "/perception/object_recognition/detection/apollo/objects"
-        can_spawn_new_tracker: true
+        flags:
+          can_spawn_new_tracker: true
+          can_trust_existence_probability: true
+          can_trust_extension: true
+          can_trust_classification: true
+          can_trust_orientation: true
         optional:
           name: "apollo"
           short_name: "Lap"
       lidar_apollo_validated:
         topic: "/perception/object_recognition/detection/apollo/validation/objects"
-        can_spawn_new_tracker: true
+        flags:
+          can_spawn_new_tracker: true
+          can_trust_existence_probability: true
+          can_trust_extension: true
+          can_trust_classification: true
+          can_trust_orientation: true
         optional:
           name: "apollo"
           short_name: "Lap"
       # LIDAR-CAMERA - DNN
       # cspell:ignore lidar_pointpainting pointpainting
       lidar_pointpainting:
         topic: "/perception/object_recognition/detection/pointpainting/objects"
-        can_spawn_new_tracker: true
+        flags:
+          can_spawn_new_tracker: true
+          can_trust_existence_probability: true
+          can_trust_extension: true
+          can_trust_classification: true
+          can_trust_orientation: true
         optional:
           name: "pointpainting"
           short_name: "Lpp"
       lidar_pointpainting_validated:
         topic: "/perception/object_recognition/detection/pointpainting/validation/objects"
-        can_spawn_new_tracker: true
+        flags:
+          can_spawn_new_tracker: true
+          can_trust_existence_probability: true
+          can_trust_extension: true
+          can_trust_classification: true
+          can_trust_orientation: true
         optional:
           name: "pointpainting"
           short_name: "Lpp"
       # CAMERA-LIDAR
       camera_lidar_fusion:
         topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects"
-        can_spawn_new_tracker: true
+        flags:
+          can_spawn_new_tracker: true
+          can_trust_existence_probability: false
+          can_trust_extension: false
+          can_trust_classification: true
+          can_trust_orientation: false
         optional:
           name: "camera_lidar_fusion"
           short_name: "CLf"
       # CAMERA-LIDAR+TRACKER
       detection_by_tracker:
         topic: "/perception/object_recognition/detection/detection_by_tracker/objects"
-        can_spawn_new_tracker: false
+        flags:
+          can_spawn_new_tracker: false
+          can_trust_existence_probability: false
+          can_trust_extension: false
+          can_trust_classification: false
+          can_trust_orientation: false
         optional:
           name: "detection_by_tracker"
           short_name: "dbT"
       # RADAR
       radar:
         topic: "/sensing/radar/detected_objects"
-        can_spawn_new_tracker: true
+        flags:
+          can_spawn_new_tracker: true
+          can_trust_existence_probability: true
+          can_trust_extension: false
+          can_trust_classification: true
+          can_trust_orientation: false
         optional:
           name: "radar"
           short_name: "R"
       radar_far:
         topic: "/perception/object_recognition/detection/radar/far_objects"
-        can_spawn_new_tracker: true
+        flags:
+          can_spawn_new_tracker: true
+          can_trust_existence_probability: true
+          can_trust_extension: false
+          can_trust_classification: true
+          can_trust_orientation: false
         optional:
           name: "radar_far"
           short_name: "Rf"

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp

@@ -50,6 +50,10 @@ struct InputChannel
   std::string long_name = "Detected Object";  // full name of the detection
   std::string short_name = "DET";             // abbreviation of the name
   bool is_spawn_enabled = true;               // enable spawn of the object
+  bool trust_existence_probability = true;    // trust object existence probability
+  bool trust_extension = true;                // trust object extension
+  bool trust_classification = true;           // trust object classification
+  bool trust_orientation = true;              // trust object orientation(yaw)
 };
 
 // object model

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp

@@ -39,7 +39,9 @@ class MultipleVehicleTracker : public Tracker
   MultipleVehicleTracker(const rclcpp::Time & time, const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
-  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
+  bool measure(
+    const types::DynamicObject & object, const rclcpp::Time & time,
+    const types::InputChannel & channel_info) override;
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
   virtual ~MultipleVehicleTracker() {}
 };

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp

@@ -37,7 +37,9 @@ class PassThroughTracker : public Tracker
 public:
   PassThroughTracker(const rclcpp::Time & time, const types::DynamicObject & object);
   bool predict(const rclcpp::Time & time) override;
-  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
+  bool measure(
+    const types::DynamicObject & object, const rclcpp::Time & time,
+    const types::InputChannel & channel_info) override;
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
 };
 

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp

@@ -20,9 +20,9 @@
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_
 
 #include "autoware/multi_object_tracker/object_model/types.hpp"
-#include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp"
 #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
+#include "autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp"
 
 #include <autoware/kalman_filter/kalman_filter.hpp>
 
@@ -33,13 +33,15 @@ class PedestrianAndBicycleTracker : public Tracker
 {
 private:
   PedestrianTracker pedestrian_tracker_;
-  BicycleTracker bicycle_tracker_;
+  VehicleTracker bicycle_tracker_;
 
 public:
   PedestrianAndBicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
-  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
+  bool measure(
+    const types::DynamicObject & object, const rclcpp::Time & time,
+    const types::InputChannel & channel_info) override;
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
   virtual ~PedestrianAndBicycleTracker() {}
 };

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp

@@ -44,7 +44,9 @@ class PedestrianTracker : public Tracker
   PedestrianTracker(const rclcpp::Time & time, const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
-  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
+  bool measure(
+    const types::DynamicObject & object, const rclcpp::Time & time,
+    const types::InputChannel & channel_info) override;
   bool measureWithPose(const types::DynamicObject & object);
   bool measureWithShape(const types::DynamicObject & object);
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp

@@ -87,7 +87,9 @@ class Tracker
   void limitObjectExtension(const object_model::ObjectModel object_model);
 
   // virtual functions
-  virtual bool measure(const types::DynamicObject & object, const rclcpp::Time & time) = 0;
+  virtual bool measure(
+    const types::DynamicObject & object, const rclcpp::Time & time,
+    const types::InputChannel & channel_info) = 0;
 
 public:
   virtual bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const = 0;

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp

@@ -43,7 +43,9 @@ class UnknownTracker : public Tracker
   UnknownTracker(const rclcpp::Time & time, const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
-  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
+  bool measure(
+    const types::DynamicObject & object, const rclcpp::Time & time,
+    const types::InputChannel & channel_info) override;
   bool measureWithPose(const types::DynamicObject & object);
   bool measureWithShape(const types::DynamicObject & object);
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp

@@ -48,8 +48,11 @@ class VehicleTracker : public Tracker
     const types::DynamicObject & object);
 
   bool predict(const rclcpp::Time & time) override;
-  bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override;
-  bool measureWithPose(const types::DynamicObject & object);
+  bool measure(
+    const types::DynamicObject & object, const rclcpp::Time & time,
+    const types::InputChannel & channel_info) override;
+  bool measureWithPose(
+    const types::DynamicObject & object, const types::InputChannel & channel_info);
   bool measureWithShape(const types::DynamicObject & object);
   bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
 };

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp

@@ -94,6 +94,10 @@ class BicycleMotionModel : public MotionModel
     const double & x, const double & y, const double & yaw,
     const std::array<double, 36> & pose_cov);
 
+  bool updateStatePoseVel(
+    const double & x, const double & y, const std::array<double, 36> & pose_cov, const double & vel,
+    const std::array<double, 36> & twist_cov);
+
   bool updateStatePoseHeadVel(
     const double & x, const double & y, const double & yaw, const std::array<double, 36> & pose_cov,
     const double & vel, const std::array<double, 36> & twist_cov);

perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/tracker.hpp

@@ -19,7 +19,6 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__TRACKER_HPP_
 
-#include "model/bicycle_tracker.hpp"
 #include "model/multiple_vehicle_tracker.hpp"
 #include "model/pass_through_tracker.hpp"
 #include "model/pedestrian_and_bicycle_tracker.hpp"