add existence probability control

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp

@@ -101,26 +101,37 @@ class TrackerState
   void updateWithLIDAR(const rclcpp::Time & current_time, const TrackedObject & tracked_object);
   void updateWithRADAR(const rclcpp::Time & current_time, const TrackedObject & tracked_object);
   void updateWithCAMERA(const rclcpp::Time & current_time, const TrackedObject & tracked_object);
+  void updateWithoutSensor(const rclcpp::Time & current_time);
   bool update(const MEASUREMENT_STATE input, const TrackedObject & tracked_object);
   bool updateWithFunction(
     const MEASUREMENT_STATE input, const rclcpp::Time & current_time,
     const TrackedObject & tracked_object,
     std::function<void(TrackedObject &, const TrackedObject &)> update_func);
-  bool hasUUID(const MEASUREMENT_STATE input, const unique_identifier_msgs::msg::UUID & uuid);
+  // const functions
+  bool hasUUID(const MEASUREMENT_STATE input, const unique_identifier_msgs::msg::UUID & uuid) const;
+  bool isValid() const;
+  bool canPublish() const;
   TrackedObject getObject() const;
 
 public:
   // handle uuid
-  const unique_identifier_msgs::msg::UUID const_uuid_;
+  unique_identifier_msgs::msg::UUID const_uuid_;
   // each sensor input to uuid map
   std::unordered_map<MEASUREMENT_STATE, std::optional<unique_identifier_msgs::msg::UUID>>
     input_uuid_map_;
   MEASUREMENT_STATE measurement_state_;
 
-  std::unordered_map<MEASUREMENT_STATE, double> existence_probability_map_;
+  std::unordered_map<MEASUREMENT_STATE, double> default_existence_probability_map_ = {
+    {MEASUREMENT_STATE::LIDAR, 0.8},
+    {MEASUREMENT_STATE::RADAR, 0.7},
+    {MEASUREMENT_STATE::CAMERA, 0.6},
+  };
+  double existence_probability_ = 0.0;
+  double publish_probability_threshold_ = 0.5;
+  double remove_probability_threshold_ = 0.3;
 };
 
 TrackedObjects getTrackedObjectsFromTrackerStates(
-  const std::vector<TrackerState> & tracker_states, const rclcpp::Time & time);
+  std::vector<TrackerState> & tracker_states, const rclcpp::Time & time);
 
 #endif  // TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_

perception/tracking_object_merger/src/decorative_tracker_merger.cpp

@@ -134,13 +134,14 @@ void DecorativeTrackerMergerNode::mainObjectsCallback(
       }
     } else {
       // show fo sub objects
-      for (const auto & sub_obj : closest_time_sub_objects->objects) {
-        std::cout << "sub_objects(x,y): " << sub_obj.kinematics.pose_with_covariance.pose.position.x
-                  << ", " << sub_obj.kinematics.pose_with_covariance.pose.position.y << std::endl;
-      }
-      // update with old sub objects
-      this->decorativeMerger(MEASUREMENT_STATE::RADAR, closest_time_sub_objects);
+      // for (const auto & sub_obj : closest_time_sub_objects->objects) {
+      //   std::cout << "sub_objects(x,y): " <<
+      //   sub_obj.kinematics.pose_with_covariance.pose.position.x
+      //             << ", " << sub_obj.kinematics.pose_with_covariance.pose.position.y <<
+      //             std::endl;
     }
+    // update with old sub objects
+    this->decorativeMerger(MEASUREMENT_STATE::RADAR, closest_time_sub_objects);
   }
 
   // try to merge main object
@@ -215,8 +216,8 @@ bool DecorativeTrackerMergerNode::decorativeMerger(
       // merge object1 into object0_state
       object0_state.updateState(input_index, current_time, object1);
     } else {  // not found
-      // do nothing
-      // or decrease existence probability
+      // decrease existence probability
+      object0_state.updateWithoutSensor(current_time);
     }
   }
   // look for new object

perception/tracking_object_merger/src/utils/tracker_state.cpp

@@ -36,6 +36,7 @@ TrackerState::TrackerState(
 {
   input_uuid_map_[input_source] = tracked_object_.object_id;
   last_updated_time_map_[input_source] = last_update_time;
+  existence_probability_ = default_existence_probability_map_[input_source];
 }
 
 TrackerState::TrackerState(
@@ -49,6 +50,7 @@ TrackerState::TrackerState(
 {
   input_uuid_map_[input_source] = tracked_object_.object_id;
   last_updated_time_map_[input_source] = last_update_time;
+  existence_probability_ = default_existence_probability_map_[input_source];
 }
 
 /**
@@ -201,42 +203,95 @@ bool TrackerState::updateWithFunction(
   last_updated_time_map_[input] = current_time;
   input_uuid_map_[input] = input_tracked_object.object_id;
   measurement_state_ = measurement_state_ | input;
+  existence_probability_ =
+    std::max(existence_probability_, default_existence_probability_map_[input]);
 
   // update tracked object
   update_func(tracked_object_, input_tracked_object);
   tracked_object_.object_id = const_uuid_;  // overwrite uuid to stay same
   return true;
 }
 
+void TrackerState::updateWithoutSensor(const rclcpp::Time & current_time)
+{
+  // calc dt
+  double dt = (current_time - last_update_time_).seconds();
+  if (dt < 0) {
+    std::cerr << "[tracking_object_merger] dt is negative: " << dt << std::endl;
+    return;
+  }
+
+  // predict
+  if (dt > 0.0) {
+    this->predict(dt, utils::predictPastOrFutureTrackedObject);
+  }
+
+  // reduce probability
+  existence_probability_ -= 0.1;
+  if (existence_probability_ < 0.0) {
+    existence_probability_ = 0.0;
+  }
+}
+
 TrackedObject TrackerState::getObject() const
 {
   TrackedObject tracked_object = tracked_object_;
   tracked_object.object_id = const_uuid_;
+  tracked_object.existence_probability = existence_probability_;
   return tracked_object;
 }
 
 bool TrackerState::hasUUID(
-  const MEASUREMENT_STATE input, const unique_identifier_msgs::msg::UUID & uuid)
+  const MEASUREMENT_STATE input, const unique_identifier_msgs::msg::UUID & uuid) const
 {
   if (input_uuid_map_.find(input) == input_uuid_map_.end()) {
     return false;
   }
   return input_uuid_map_.at(input) == uuid;
 }
 
+bool TrackerState::isValid() const
+{
+  // check if tracker state is valid
+  if (existence_probability_ < remove_probability_threshold_) {
+    return false;
+  }
+  return true;
+}
+
+bool TrackerState::canPublish() const
+{
+  // check if tracker state is valid
+  if (existence_probability_ < publish_probability_threshold_) {
+    return false;
+  }
+  return true;
+}
+
 TrackerState::~TrackerState()
 {
   // destructor
 }
 
 TrackedObjects getTrackedObjectsFromTrackerStates(
-  const std::vector<TrackerState> & tracker_states, const rclcpp::Time & current_time)
+  std::vector<TrackerState> & tracker_states, const rclcpp::Time & current_time)
 {
   TrackedObjects tracked_objects;
-  for (const auto & tracker_state : tracker_states) {
-    tracked_objects.objects.push_back(tracker_state.getObject());
+
+  // sanitize and get tracked objects
+  for (auto it = tracker_states.begin(); it != tracker_states.end();) {
+    // check if tracker state is valid
+    if (it->isValid()) {
+      if (it->canPublish()) {
+        // if valid, get tracked object
+        tracked_objects.objects.push_back(it->getObject());
+      }
+      ++it;
+    } else {
+      // if not valid, delete tracker state
+      it = tracker_states.erase(it);
+    }
   }
-  // TODO(yoshiri): do prediction if object last input state is older than current time
 
   // update header
   tracked_objects.header.stamp = current_time;