fix: total existence logic change

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp

@@ -57,6 +57,8 @@ class TrackerProcessor
     const rclcpp::Time & time,
     autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const;
 
+  void getExistenceProbabilities(std::vector<std::vector<float>> & existence_vectors) const;
+
 private:
   std::map<std::uint8_t, std::string> tracker_map_;
   std::list<std::shared_ptr<Tracker>> list_tracker_;

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

@@ -58,6 +58,11 @@ class Tracker
 
   void initializeExistenceProbabilities(
     const uint & channel_index, const float & existence_probability);
+  bool getExistenceProbabilityVector(std::vector<float> & existence_vector) const
+  {
+    existence_vector = existence_probabilities_;
+    return existence_vector.size() > 0;
+  }
   bool updateWithMeasurement(
     const autoware_auto_perception_msgs::msg::DetectedObject & object,
     const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform,

perception/multi_object_tracker/src/debugger/debugger.cpp

@@ -11,8 +11,6 @@
 // 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 "multi_object_tracker/debugger/debugger.hpp"
 
@@ -74,7 +72,7 @@ void TrackerDebugger::setupDiagnostics()
   diagnostic_updater_.setPeriod(0.1);
 }
 
-// object publishing functions
+// Object publishing functions
 
 void TrackerDebugger::publishTentativeObjects(
   const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const
@@ -84,7 +82,7 @@ void TrackerDebugger::publishTentativeObjects(
   }
 }
 
-// time measurement functions
+// Time measurement functions
 
 void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat)
 {

perception/multi_object_tracker/src/tracker/model/tracker_base.cpp

@@ -58,8 +58,9 @@ bool Tracker::updateWithMeasurement(
     no_measurement_count_ = 0;
     ++total_measurement_count_;
 
-    double const delta_time = (measurement_time - last_update_with_measurement_time_).seconds();
-    double const decay_rate = 5.0 / 10.0;
+    // existence probability on each channel
+    const double delta_time = (measurement_time - last_update_with_measurement_time_).seconds();
+    const double decay_rate = 5.0 / 10.0;
     existence_probabilities_[channel_index] = existence_probability_from_object;
     for (size_t i = 0; i < existence_probabilities_.size(); ++i) {
       if (i == channel_index) {
@@ -68,11 +69,9 @@ bool Tracker::updateWithMeasurement(
       existence_probabilities_[i] *= std::exp(-decay_rate * delta_time);
     }
 
-    // regularization
-    total_existence_probability_ =
-      std::accumulate(existence_probabilities_.begin(), existence_probabilities_.end(), 0.0f);
-    total_existence_probability_ = std::max(total_existence_probability_, 0.0f);
-    total_existence_probability_ = std::min(total_existence_probability_, 1.0f);
+    // total existence probability - object is detected
+    total_existence_probability_ +=
+      (1 - total_existence_probability_) * existence_probability_from_object;
   }
 
   last_update_with_measurement_time_ = measurement_time;
@@ -86,22 +85,16 @@ bool Tracker::updateWithMeasurement(
 bool Tracker::updateWithoutMeasurement(const rclcpp::Time & now)
 {
   // Update existence probability
+  ++no_measurement_count_;
+  ++total_no_measurement_count_;
   {
-    ++no_measurement_count_;
-    ++total_no_measurement_count_;
-
-    // decay
+    // decay existence probability
     double const delta_time = (now - last_update_with_measurement_time_).seconds();
     double const decay_rate = 5.0 / 10.0;
     for (size_t i = 0; i < existence_probabilities_.size(); ++i) {
       existence_probabilities_[i] *= std::exp(-decay_rate * delta_time);
     }
-
-    // regularization
-    total_existence_probability_ =
-      std::accumulate(existence_probabilities_.begin(), existence_probabilities_.end(), 0.0f);
-    total_existence_probability_ = std::max(total_existence_probability_, 0.0f);
-    total_existence_probability_ = std::min(total_existence_probability_, 1.0f);
+    total_existence_probability_ *= std::exp(-decay_rate * delta_time);
   }
 
   return true;