refactor(autoware_multi_object_tracker): define a new internal object class

perception/autoware_multi_object_tracker/CMakeLists.txt

@@ -30,6 +30,8 @@ set(${PROJECT_NAME}_src
 set(${PROJECT_NAME}_lib
   lib/association/association.cpp
   lib/association/mu_successive_shortest_path/mu_ssp.cpp
+  lib/object_model/types.cpp
+  lib/object_model/shapes.cpp
   lib/tracker/motion_model/motion_model_base.cpp
   lib/tracker/motion_model/bicycle_motion_model.cpp
   # cspell: ignore ctrv

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

@@ -27,7 +27,7 @@
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
-#include "autoware_perception_msgs/msg/detected_objects.hpp"
+#include <autoware_perception_msgs/msg/detected_objects.hpp>
 
 #include <list>
 #include <memory>
@@ -58,7 +58,7 @@ class DataAssociation
     const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
     std::unordered_map<int, int> & reverse_assignment);
   Eigen::MatrixXd calcScoreMatrix(
-    const autoware_perception_msgs::msg::DetectedObjects & measurements,
+    const types::DynamicObjectList & measurements,
     const std::list<std::shared_ptr<Tracker>> & trackers);
   virtual ~DataAssociation() {}
 };

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

@@ -0,0 +1,57 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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.
+//
+//
+// Author: v1.0 Taekjin Lee
+
+#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_
+#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_
+
+#include "autoware/multi_object_tracker/object_model/types.hpp"
+
+#include <Eigen/Core>
+
+#include <tf2_ros/buffer.h>
+
+#include <string>
+
+namespace autoware::multi_object_tracker
+{
+namespace shapes
+{
+bool transformObjects(
+  const types::DynamicObjectList & input_msg, const std::string & target_frame_id,
+  const tf2_ros::Buffer & tf_buffer, types::DynamicObjectList & output_msg);
+
+double get2dIoU(
+  const types::DynamicObject & source_object, const types::DynamicObject & target_object,
+  const double min_union_area = 0.01);
+
+bool convertConvexHullToBoundingBox(
+  const types::DynamicObject & input_object, types::DynamicObject & output_object);
+
+bool getMeasurementYaw(
+  const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw);
+
+int getNearestCornerOrSurface(
+  const double x, const double y, const double yaw, const double width, const double length,
+  const geometry_msgs::msg::Transform & self_transform);
+
+void calcAnchorPointOffset(
+  const double w, const double l, const int indx, const types::DynamicObject & input_object,
+  const double & yaw, types::DynamicObject & offset_object, Eigen::Vector2d & tracking_offset);
+}  // namespace shapes
+}  // namespace autoware::multi_object_tracker
+
+#endif  // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_

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

@@ -0,0 +1,88 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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.
+//
+//
+// Author: v1.0 Taekjin Lee
+
+#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_
+#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_
+
+#include <autoware_perception_msgs/msg/detected_object.hpp>
+#include <autoware_perception_msgs/msg/detected_object_kinematics.hpp>
+#include <autoware_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_perception_msgs/msg/object_classification.hpp>
+#include <autoware_perception_msgs/msg/shape.hpp>
+#include <autoware_perception_msgs/msg/tracked_object.hpp>
+#include <autoware_perception_msgs/msg/tracked_object_kinematics.hpp>
+#include <geometry_msgs/msg/polygon.hpp>
+#include <geometry_msgs/msg/pose_with_covariance.hpp>
+#include <geometry_msgs/msg/twist_with_covariance.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <std_msgs/msg/header.hpp>
+#include <unique_identifier_msgs/msg/uuid.hpp>
+
+#include <boost/optional.hpp>
+
+#include <vector>
+
+namespace autoware::multi_object_tracker
+{
+namespace types
+{
+enum OrientationAvailability : uint8_t {
+  UNAVAILABLE = 0,
+  SIGN_UNKNOWN = 1,
+  AVAILABLE = 2,
+};
+
+struct ObjectKinematics
+{
+  geometry_msgs::msg::PoseWithCovariance pose_with_covariance;
+  geometry_msgs::msg::TwistWithCovariance twist_with_covariance;
+  bool has_position_covariance = false;
+  OrientationAvailability orientation_availability;
+  bool has_twist = false;
+  bool has_twist_covariance = false;
+};
+
+struct DynamicObject
+{
+  unique_identifier_msgs::msg::UUID object_id = unique_identifier_msgs::msg::UUID();
+  uint channel_index;
+  float existence_probability;
+  std::vector<autoware_perception_msgs::msg::ObjectClassification> classification;
+  ObjectKinematics kinematics;
+  autoware_perception_msgs::msg::Shape shape;
+};
+
+struct DynamicObjectList
+{
+  std_msgs::msg::Header header;
+  uint channel_index;
+  std::vector<DynamicObject> objects;
+};
+
+DynamicObject toDynamicObject(
+  const autoware_perception_msgs::msg::DetectedObject & det_object, const uint channel_index = 0);
+
+DynamicObjectList toDynamicObjectList(
+  const autoware_perception_msgs::msg::DetectedObjects & det_objects, const uint channel_index = 0);
+
+autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object);
+
+}  // namespace types
+
+}  // namespace autoware::multi_object_tracker
+
+#endif  // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_

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

@@ -19,18 +19,20 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_
 
-#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/object_model/object_model.hpp"
+#include "autoware/multi_object_tracker/object_model/types.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
 #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp"
 
+#include <autoware/kalman_filter/kalman_filter.hpp>
+
 namespace autoware::multi_object_tracker
 {
 
 class BicycleTracker : public Tracker
 {
 private:
-  autoware_perception_msgs::msg::DetectedObject object_;
+  types::DynamicObject object_;
   rclcpp::Logger logger_;
 
   object_model::ObjectModel object_model_ = object_model::bicycle;
@@ -50,23 +52,19 @@ class BicycleTracker : public Tracker
 
 public:
   BicycleTracker(
-    const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
-    const uint & channel_index);
+    const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(
-    const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
+    const types::DynamicObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) override;
-  bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object);
-  bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object);
-  bool getTrackedObject(
-    const rclcpp::Time & time,
-    autoware_perception_msgs::msg::TrackedObject & object) const override;
+  bool measureWithPose(const types::DynamicObject & object);
+  bool measureWithShape(const types::DynamicObject & object);
+  bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
 
 private:
-  autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
-    const autoware_perception_msgs::msg::DetectedObject & object,
+  types::DynamicObject getUpdatingObject(
+    const types::DynamicObject & object,
     const geometry_msgs::msg::Transform & self_transform) const;
 };
 

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

@@ -19,10 +19,11 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_
 
-#include "autoware/kalman_filter/kalman_filter.hpp"
+#include "autoware/multi_object_tracker/object_model/types.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>
 #include <rclcpp/time.hpp>
 
 namespace autoware::multi_object_tracker
@@ -36,17 +37,13 @@ class MultipleVehicleTracker : public Tracker
 
 public:
   MultipleVehicleTracker(
-    const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
-    const uint & channel_index);
+    const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(
-    const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
+    const types::DynamicObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) override;
-  bool getTrackedObject(
-    const rclcpp::Time & time,
-    autoware_perception_msgs::msg::TrackedObject & object) const 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

@@ -19,32 +19,30 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_
 
-#include "autoware/kalman_filter/kalman_filter.hpp"
+#include "autoware/multi_object_tracker/object_model/types.hpp"
 #include "tracker_base.hpp"
 
+#include <autoware/kalman_filter/kalman_filter.hpp>
+
 namespace autoware::multi_object_tracker
 {
 
 class PassThroughTracker : public Tracker
 {
 private:
-  autoware_perception_msgs::msg::DetectedObject object_;
-  autoware_perception_msgs::msg::DetectedObject prev_observed_object_;
+  types::DynamicObject object_;
+  types::DynamicObject prev_observed_object_;
   rclcpp::Logger logger_;
   rclcpp::Time last_update_time_;
 
 public:
   PassThroughTracker(
-    const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
-    const uint & channel_index);
+    const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size);
   bool predict(const rclcpp::Time & time) override;
   bool measure(
-    const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
+    const types::DynamicObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) override;
-  bool getTrackedObject(
-    const rclcpp::Time & time,
-    autoware_perception_msgs::msg::TrackedObject & object) const override;
+  bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
 };
 
 }  // namespace autoware::multi_object_tracker

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

@@ -19,11 +19,13 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_
 
-#include "autoware/kalman_filter/kalman_filter.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/kalman_filter/kalman_filter.hpp>
+
 namespace autoware::multi_object_tracker
 {
 
@@ -35,17 +37,13 @@ class PedestrianAndBicycleTracker : public Tracker
 
 public:
   PedestrianAndBicycleTracker(
-    const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
-    const uint & channel_index);
+    const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(
-    const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
+    const types::DynamicObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) override;
-  bool getTrackedObject(
-    const rclcpp::Time & time,
-    autoware_perception_msgs::msg::TrackedObject & object) const 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

@@ -19,18 +19,20 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_
 
-#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/object_model/object_model.hpp"
+#include "autoware/multi_object_tracker/object_model/types.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
 #include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp"
 
+#include <autoware/kalman_filter/kalman_filter.hpp>
+
 namespace autoware::multi_object_tracker
 {
 
 class PedestrianTracker : public Tracker
 {
 private:
-  autoware_perception_msgs::msg::DetectedObject object_;
+  types::DynamicObject object_;
   rclcpp::Logger logger_;
 
   object_model::ObjectModel object_model_ = object_model::pedestrian;
@@ -56,23 +58,19 @@ class PedestrianTracker : public Tracker
 
 public:
   PedestrianTracker(
-    const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
-    const uint & channel_index);
+    const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(
-    const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
+    const types::DynamicObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) override;
-  bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object);
-  bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object);
-  bool getTrackedObject(
-    const rclcpp::Time & time,
-    autoware_perception_msgs::msg::TrackedObject & object) const override;
+  bool measureWithPose(const types::DynamicObject & object);
+  bool measureWithShape(const types::DynamicObject & object);
+  bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override;
 
 private:
-  autoware_perception_msgs::msg::DetectedObject getUpdatingObject(
-    const autoware_perception_msgs::msg::DetectedObject & object,
+  types::DynamicObject getUpdatingObject(
+    const types::DynamicObject & object,
     const geometry_msgs::msg::Transform & self_transform) const;
 };