From 7a9f9913e9a637f8f356b8e49149a3c4157e3803 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 19 Feb 2025 14:25:52 +0900 Subject: [PATCH 1/6] refactor(multi_object_tracker): streamline input channel configuration handling feat(multi_object_tracker): introduce InputChannel struct for input channel configuration refactor(multi_object_tracker): improve marker handling and initialization in TrackerObjectDebugger feat(multi_object_tracker): enhance InputChannel with trust flags for object properties refactor(multi_object_tracker): remove unused channel_size parameter from tracker constructors feat(multi_object_tracker): update InputChannel flags to trust object extension and classification fix(multi_object_tracker): replace channel.index with channel_index for consistency feat(multi_object_tracker): update TrackerObjectDebugger and TrackerProcessor to accept channels_config parameter refactor(multi_object_tracker): remove redundant existence probability initialization from tracker constructors feat(multi_object_tracker): integrate data association into TrackerProcessor and add associate method feat(multi_object_tracker): enhance updateWithMeasurement to include channel_info for improved classification handling refactor(multi_object_tracker): replace object_id with uuid in DynamicObject and related classes fix(multi_object_tracker): update UUID handling in Tracker to use uuid_msg for consistency refactor(multi_object_tracker): simplify pose and covariance handling in tracker classes refactor(multi_object_tracker): replace pose_with_covariance with separate pose and covariance attributes in DynamicObject refactor: remove z state from tracker. it will uses object state refactor(multi_object_tracker): streamline object handling in trackers and remove unnecessary shape processing refactor(multi_object_tracker): remove z position handling from trackers and update object kinematics structure refactor(multi_object_tracker): remove BoundingBox structure from trackers and implement object extension limits refactor(multi_object_tracker): remove unnecessary blank lines in tracker getTrackedObject methods refactor(multi_object_tracker): simplify input channel configuration by removing trust flags and consolidating parameters Signed-off-by: Taekjin LEE --- .../association/association.hpp | 23 ++- .../object_model/types.hpp | 37 +++- .../tracker/model/bicycle_tracker.hpp | 14 +- .../model/multiple_vehicle_tracker.hpp | 3 +- .../tracker/model/pass_through_tracker.hpp | 4 +- .../model/pedestrian_and_bicycle_tracker.hpp | 3 +- .../tracker/model/pedestrian_tracker.hpp | 19 +- .../tracker/model/tracker_base.hpp | 37 +--- .../tracker/model/unknown_tracker.hpp | 15 +- .../tracker/model/vehicle_tracker.hpp | 12 +- .../lib/association/association.cpp | 130 ++++++------- .../lib/object_model/shapes.cpp | 22 +-- .../lib/object_model/types.cpp | 23 ++- .../lib/odometry.cpp | 13 +- .../lib/tracker/model/bicycle_tracker.cpp | 133 +++++--------- .../model/multiple_vehicle_tracker.cpp | 22 +-- .../tracker/model/pass_through_tracker.cpp | 50 ++--- .../model/pedestrian_and_bicycle_tracker.cpp | 20 +- .../lib/tracker/model/pedestrian_tracker.cpp | 172 +++++------------- .../lib/tracker/model/tracker_base.cpp | 44 +++-- .../lib/tracker/model/unknown_tracker.cpp | 80 ++------ .../lib/tracker/model/vehicle_tracker.cpp | 150 +++++---------- .../motion_model/bicycle_motion_model.cpp | 2 +- .../motion_model/ctrv_motion_model.cpp | 2 +- .../tracker/motion_model/cv_motion_model.cpp | 2 +- .../lib/uncertainty/uncertainty_processor.cpp | 19 +- .../src/debugger/debug_object.cpp | 102 ++++++----- .../src/debugger/debug_object.hpp | 23 +-- .../src/debugger/debugger.cpp | 8 +- .../src/debugger/debugger.hpp | 17 +- .../src/multi_object_tracker_node.cpp | 167 ++++++++--------- .../src/multi_object_tracker_node.hpp | 6 +- .../src/processor/input_manager.cpp | 68 ++++--- .../src/processor/input_manager.hpp | 27 +-- .../src/processor/processor.cpp | 69 +++++-- .../src/processor/processor.hpp | 16 +- 36 files changed, 665 insertions(+), 889 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp index b92e17987eb5f..a4b61755164af 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp @@ -36,6 +36,16 @@ namespace autoware::multi_object_tracker { +struct AssociatorConfig +{ + std::vector can_assign_matrix; + std::vector max_dist_matrix; + std::vector max_area_matrix; + std::vector min_area_matrix; + std::vector max_rad_matrix; + std::vector min_iou_matrix; +}; + class DataAssociation { private: @@ -50,17 +60,20 @@ class DataAssociation public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - DataAssociation( - std::vector can_assign_vector, std::vector max_dist_vector, - std::vector max_area_vector, std::vector min_area_vector, - std::vector max_rad_vector, std::vector min_iou_vector); + explicit DataAssociation(const AssociatorConfig & config); + virtual ~DataAssociation() {} + void assign( const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, std::unordered_map & reverse_assignment); + + double calculateScore( + const types::DynamicObject & tracked_object, const std::uint8_t tracker_label, + const types::DynamicObject & measurement_object, const std::uint8_t measurement_label) const; + Eigen::MatrixXd calcScoreMatrix( const types::DynamicObjectList & measurements, const std::list> & trackers); - virtual ~DataAssociation() {} }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp index 7dab840aac481..016051c454f05 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp @@ -11,9 +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. -// -// -// Author: v1.0 Taekjin Lee #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_ @@ -34,12 +31,27 @@ #include +#include #include namespace autoware::multi_object_tracker { namespace types { +// constants +constexpr size_t max_channel_size = 16; + +// channel configuration +struct InputChannel +{ + uint index; // index of the channel + std::string input_topic; // topic name of the detection, e.g. "/detection/lidar" + 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 +}; + +// object model enum OrientationAvailability : uint8_t { UNAVAILABLE = 0, SIGN_UNKNOWN = 1, @@ -48,8 +60,6 @@ enum OrientationAvailability : uint8_t { 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; @@ -58,11 +68,25 @@ struct ObjectKinematics struct DynamicObject { - unique_identifier_msgs::msg::UUID object_id = unique_identifier_msgs::msg::UUID(); + // identification + unique_identifier_msgs::msg::UUID uuid = unique_identifier_msgs::msg::UUID(); + + // existence information uint channel_index; float existence_probability; + std::vector existence_probabilities; + + // object classification std::vector classification; + + // object kinematics (pose and twist) ObjectKinematics kinematics; + geometry_msgs::msg::Pose pose; + std::array pose_covariance; + geometry_msgs::msg::Twist twist; + std::array twist_covariance; + + // object extension (size and shape) autoware_perception_msgs::msg::Shape shape; }; @@ -82,7 +106,6 @@ DynamicObjectList toDynamicObjectList( 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_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index ad3667eb240c0..2e4449aee529d 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -32,27 +32,15 @@ namespace autoware::multi_object_tracker class BicycleTracker : public Tracker { private: - types::DynamicObject object_; rclcpp::Logger logger_; object_model::ObjectModel object_model_ = object_model::bicycle; - double z_; - - struct BoundingBox - { - double length; - double width; - double height; - }; - BoundingBox bounding_box_; - BicycleMotionModel motion_model_; using IDX = BicycleMotionModel::IDX; public: - BicycleTracker( - const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); + BicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index b9e026bf3c009..80dbacc1d17d2 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -36,8 +36,7 @@ class MultipleVehicleTracker : public Tracker VehicleTracker big_vehicle_tracker_; public: - MultipleVehicleTracker( - const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); + MultipleVehicleTracker(const rclcpp::Time & time, const types::DynamicObject & object); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index 6230ba6e3b0f4..938e704212b8d 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -30,14 +30,12 @@ namespace autoware::multi_object_tracker class PassThroughTracker : public Tracker { private: - types::DynamicObject object_; types::DynamicObject prev_observed_object_; rclcpp::Logger logger_; rclcpp::Time last_update_time_; public: - PassThroughTracker( - const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); + 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, diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index 8a4bfc59d37ac..abd59557507b2 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -36,8 +36,7 @@ class PedestrianAndBicycleTracker : public Tracker BicycleTracker bicycle_tracker_; public: - PedestrianAndBicycleTracker( - const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); + PedestrianAndBicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 5a2acc50a8249..c39809b4d0a17 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -32,33 +32,16 @@ namespace autoware::multi_object_tracker class PedestrianTracker : public Tracker { private: - types::DynamicObject object_; rclcpp::Logger logger_; object_model::ObjectModel object_model_ = object_model::pedestrian; - double z_; - - struct BoundingBox - { - double length; - double width; - double height; - }; - struct Cylinder - { - double width; - double height; - }; - BoundingBox bounding_box_; - Cylinder cylinder_; // cspell: ignore CTRV CTRVMotionModel motion_model_; using IDX = CTRVMotionModel::IDX; public: - PedestrianTracker( - const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); + PedestrianTracker(const rclcpp::Time & time, const types::DynamicObject & object); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index ac5527fca6400..cca1c2a861816 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -20,6 +20,7 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ #define EIGEN_MPL2_ONLY +#include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/object_model/types.hpp" #include @@ -39,24 +40,16 @@ namespace autoware::multi_object_tracker class Tracker { private: - unique_identifier_msgs::msg::UUID uuid_; - - // classification - std::vector classification_; - // existence states int no_measurement_count_; int total_no_measurement_count_; int total_measurement_count_; rclcpp::Time last_update_with_measurement_time_; - std::vector existence_probabilities_; + std::vector existence_probabilities_; // remove if possible float total_existence_probability_; public: - Tracker( - const rclcpp::Time & time, - const std::vector & classification, - const size_t & channel_size); + Tracker(const rclcpp::Time & time, const types::DynamicObject & object); virtual ~Tracker() = default; void initializeExistenceProbabilities( @@ -68,17 +61,12 @@ class Tracker } bool updateWithMeasurement( const types::DynamicObject & object, const rclcpp::Time & measurement_time, - const geometry_msgs::msg::Transform & self_transform); + const geometry_msgs::msg::Transform & self_transform, const types::InputChannel & channel_info); bool updateWithoutMeasurement(const rclcpp::Time & now); - // classification - std::vector getClassification() const - { - return classification_; - } std::uint8_t getHighestProbLabel() const { - return autoware::object_recognition_utils::getHighestProbLabel(classification_); + return autoware::object_recognition_utils::getHighestProbLabel(object_.classification); } // existence states @@ -91,21 +79,14 @@ class Tracker } protected: - unique_identifier_msgs::msg::UUID getUUID() const { return uuid_; } - void setClassification( - const std::vector & classification) - { - classification_ = classification; - } + types::DynamicObject object_; + void updateClassification( const std::vector & classification); - // virtual functions -public: - virtual geometry_msgs::msg::PoseWithCovariance getPoseWithCovariance( - const rclcpp::Time & time) const; + void limitObjectExtension(const object_model::ObjectModel object_model); -protected: + // virtual functions virtual bool measure( const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) = 0; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index 075db6b8a9d69..fdb24b8a3c1c8 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -19,6 +19,7 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_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/cv_motion_model.hpp" @@ -31,25 +32,15 @@ namespace autoware::multi_object_tracker class UnknownTracker : public Tracker { private: - types::DynamicObject object_; rclcpp::Logger logger_; - struct EkfParams - { - double r_cov_x; - double r_cov_y; - double r_cov_vx; - double r_cov_vy; - } ekf_params_; - - double z_; + object_model::ObjectModel object_model_ = object_model::unknown; CVMotionModel motion_model_; using IDX = CVMotionModel::IDX; public: - UnknownTracker( - const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); + UnknownTracker(const rclcpp::Time & time, const types::DynamicObject & object); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index f50d117acc081..1f7ba00a7c99d 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -37,16 +37,6 @@ class VehicleTracker : public Tracker double velocity_deviation_threshold_; - types::DynamicObject object_; - double z_; - - struct BoundingBox - { - double length; - double width; - double height; - }; - BoundingBox bounding_box_; Eigen::Vector2d tracking_offset_; BicycleMotionModel motion_model_; @@ -55,7 +45,7 @@ class VehicleTracker : public Tracker public: VehicleTracker( const object_model::ObjectModel & object_model, const rclcpp::Time & time, - const types::DynamicObject & object, const size_t channel_size); + const types::DynamicObject & object); bool predict(const rclcpp::Time & time) override; bool measure( diff --git a/perception/autoware_multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp index c66d11d839496..b832b693206f5 100644 --- a/perception/autoware_multi_object_tracker/lib/association/association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/association.cpp @@ -41,11 +41,10 @@ double getMahalanobisDistance( return std::sqrt(mahalanobis_squared(0)); } -Eigen::Matrix2d getXYCovariance(const geometry_msgs::msg::PoseWithCovariance & pose_covariance) +Eigen::Matrix2d getXYCovariance(const std::array & pose_covariance) { Eigen::Matrix2d covariance; - covariance << pose_covariance.covariance[0], pose_covariance.covariance[1], - pose_covariance.covariance[6], pose_covariance.covariance[7]; + covariance << pose_covariance[0], pose_covariance[1], pose_covariance[6], pose_covariance[7]; return covariance; } @@ -72,12 +71,15 @@ double getFormedYawAngle( namespace autoware::multi_object_tracker { -DataAssociation::DataAssociation( - std::vector can_assign_vector, std::vector max_dist_vector, - std::vector max_area_vector, std::vector min_area_vector, - std::vector max_rad_vector, std::vector min_iou_vector) -: score_threshold_(0.01) +DataAssociation::DataAssociation(const AssociatorConfig & config) : score_threshold_(0.01) { + std::vector can_assign_vector = config.can_assign_matrix; + std::vector max_dist_vector = config.max_dist_matrix; + std::vector max_area_vector = config.max_area_matrix; + std::vector min_area_vector = config.min_area_matrix; + std::vector max_rad_vector = config.max_rad_matrix; + std::vector min_iou_vector = config.min_iou_matrix; + { const int assign_label_num = static_cast(std::sqrt(can_assign_vector.size())); Eigen::Map can_assign_matrix_tmp( @@ -154,11 +156,19 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const types::DynamicObjectList & measurements, const std::list> & trackers) { + // Ensure that the detected_objects and list_tracker are not empty + if (measurements.objects.empty() || trackers.empty()) { + return Eigen::MatrixXd(); + } + // Initialize the score matrix Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(trackers.size(), measurements.objects.size()); + size_t tracker_idx = 0; for (auto tracker_itr = trackers.begin(); tracker_itr != trackers.end(); ++tracker_itr, ++tracker_idx) { + types::DynamicObject tracked_object; + (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); const std::uint8_t tracker_label = (*tracker_itr)->getHighestProbLabel(); for (size_t measurement_idx = 0; measurement_idx < measurements.objects.size(); @@ -167,60 +177,9 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( const std::uint8_t measurement_label = autoware::object_recognition_utils::getHighestProbLabel(measurement_object.classification); - double score = 0.0; - if (can_assign_matrix_(tracker_label, measurement_label)) { - types::DynamicObject tracked_object; - (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); - - const double max_dist = max_dist_matrix_(tracker_label, measurement_label); - const double dist = autoware_utils::calc_distance2d( - measurement_object.kinematics.pose_with_covariance.pose.position, - tracked_object.kinematics.pose_with_covariance.pose.position); - - bool passed_gate = true; - // dist gate - { // passed_gate is always true - if (max_dist < dist) passed_gate = false; - } - // area gate - if (passed_gate) { - const double max_area = max_area_matrix_(tracker_label, measurement_label); - const double min_area = min_area_matrix_(tracker_label, measurement_label); - const double area = autoware_utils::get_area(measurement_object.shape); - if (area < min_area || max_area < area) passed_gate = false; - } - // angle gate - if (passed_gate) { - const double max_rad = max_rad_matrix_(tracker_label, measurement_label); - const double angle = getFormedYawAngle( - measurement_object.kinematics.pose_with_covariance.pose.orientation, - tracked_object.kinematics.pose_with_covariance.pose.orientation, false); - if (std::fabs(max_rad) < M_PI && std::fabs(max_rad) < std::fabs(angle)) - passed_gate = false; - } - // mahalanobis dist gate - if (passed_gate) { - const double mahalanobis_dist = getMahalanobisDistance( - measurement_object.kinematics.pose_with_covariance.pose.position, - tracked_object.kinematics.pose_with_covariance.pose.position, - getXYCovariance(tracked_object.kinematics.pose_with_covariance)); - if (3.035 /*99%*/ <= mahalanobis_dist) passed_gate = false; - } - // 2d iou gate - if (passed_gate) { - const double min_iou = min_iou_matrix_(tracker_label, measurement_label); - const double min_union_iou_area = 1e-2; - const double iou = - shapes::get2dIoU(measurement_object, tracked_object, min_union_iou_area); - if (iou < min_iou) passed_gate = false; - } - - // all gate is passed - if (passed_gate) { - score = (max_dist - std::min(dist, max_dist)) / max_dist; - if (score < score_threshold_) score = 0.0; - } - } + double score = + calculateScore(tracked_object, tracker_label, measurement_object, measurement_label); + score_matrix(tracker_idx, measurement_idx) = score; } } @@ -228,4 +187,51 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( return score_matrix; } +double DataAssociation::calculateScore( + const types::DynamicObject & tracked_object, const std::uint8_t tracker_label, + const types::DynamicObject & measurement_object, const std::uint8_t measurement_label) const +{ + if (!can_assign_matrix_(tracker_label, measurement_label)) { + return 0.0; + } + + const double max_dist = max_dist_matrix_(tracker_label, measurement_label); + const double dist = + autoware_utils::calc_distance2d(measurement_object.pose.position, tracked_object.pose.position); + + // dist gate + if (max_dist < dist) return 0.0; + + // area gate + const double max_area = max_area_matrix_(tracker_label, measurement_label); + const double min_area = min_area_matrix_(tracker_label, measurement_label); + const double area = autoware_utils::get_area(measurement_object.shape); + if (area < min_area || max_area < area) return 0.0; + + // angle gate + const double max_rad = max_rad_matrix_(tracker_label, measurement_label); + const double angle = + getFormedYawAngle(measurement_object.pose.orientation, tracked_object.pose.orientation, false); + if (std::fabs(max_rad) < M_PI && std::fabs(max_rad) < std::fabs(angle)) { + return 0.0; + } + + // mahalanobis dist gate + const double mahalanobis_dist = getMahalanobisDistance( + measurement_object.pose.position, tracked_object.pose.position, + getXYCovariance(tracked_object.pose_covariance)); + if (3.035 /*99%*/ <= mahalanobis_dist) return 0.0; + + // 2d iou gate + const double min_iou = min_iou_matrix_(tracker_label, measurement_label); + const double min_union_iou_area = 1e-2; + const double iou = shapes::get2dIoU(measurement_object, tracked_object, min_union_iou_area); + if (iou < min_iou) return 0.0; + + // all gate is passed + double score = (max_dist - std::min(dist, max_dist)) / max_dist; + if (score < score_threshold_) score = 0.0; + return score; +} + } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index 1e1e6cf5bbeef..53ec4cf907cd7 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -67,11 +67,9 @@ double get2dIoU( { static const double MIN_AREA = 1e-6; - const auto source_polygon = autoware_utils::to_polygon2d( - source_object.kinematics.pose_with_covariance.pose, source_object.shape); + const auto source_polygon = autoware_utils::to_polygon2d(source_object.pose, source_object.shape); if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; - const auto target_polygon = autoware_utils::to_polygon2d( - target_object.kinematics.pose_with_covariance.pose, target_object.shape); + const auto target_polygon = autoware_utils::to_polygon2d(target_object.pose, target_object.shape); if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; const double intersection_area = getIntersectionArea(source_polygon, target_polygon); @@ -111,18 +109,16 @@ bool convertConvexHullToBoundingBox( } // calc new center - const Eigen::Vector2d center{ - input_object.kinematics.pose_with_covariance.pose.position.x, - input_object.kinematics.pose_with_covariance.pose.position.y}; - const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); + const Eigen::Vector2d center{input_object.pose.position.x, input_object.pose.position.y}; + const auto yaw = tf2::getYaw(input_object.pose.orientation); const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center; // set output parameters output_object = input_object; - output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); - output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); + output_object.pose.position.x = new_center.x(); + output_object.pose.position.y = new_center.y(); output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; output_object.shape.dimensions.x = max_x - min_x; @@ -135,7 +131,7 @@ bool convertConvexHullToBoundingBox( bool getMeasurementYaw( const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw) { - measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + measurement_yaw = tf2::getYaw(object.pose.orientation); // check orientation sign is known or not, and fix the limiting delta yaw double limiting_delta_yaw = M_PI_2; @@ -298,8 +294,8 @@ void calcAnchorPointOffset( // offset input object const Eigen::Matrix2d R = Eigen::Rotation2Dd(yaw).toRotationMatrix(); const Eigen::Vector2d rotated_offset = R * tracking_offset; - offset_object.kinematics.pose_with_covariance.pose.position.x += rotated_offset.x(); - offset_object.kinematics.pose_with_covariance.pose.position.y += rotated_offset.y(); + offset_object.pose.position.x += rotated_offset.x(); + offset_object.pose.position.y += rotated_offset.y(); } } // namespace shapes diff --git a/perception/autoware_multi_object_tracker/lib/object_model/types.cpp b/perception/autoware_multi_object_tracker/lib/object_model/types.cpp index 671d5313d0ff8..b5b19ee5388dd 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/types.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/types.cpp @@ -11,9 +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. -// -// -// Author: v1.0 Taekjin Lee #include "autoware/multi_object_tracker/object_model/types.hpp" @@ -29,12 +26,18 @@ DynamicObject toDynamicObject( const autoware_perception_msgs::msg::DetectedObject & det_object, const uint channel_index) { DynamicObject dynamic_object; + + // initialize existence_probabilities, using channel information dynamic_object.channel_index = channel_index; dynamic_object.existence_probability = det_object.existence_probability; + dynamic_object.classification = det_object.classification; - dynamic_object.kinematics.pose_with_covariance = det_object.kinematics.pose_with_covariance; - dynamic_object.kinematics.twist_with_covariance = det_object.kinematics.twist_with_covariance; + dynamic_object.pose = det_object.kinematics.pose_with_covariance.pose; + dynamic_object.pose_covariance = det_object.kinematics.pose_with_covariance.covariance; + dynamic_object.twist = det_object.kinematics.twist_with_covariance.twist; + dynamic_object.twist_covariance = det_object.kinematics.twist_with_covariance.covariance; + dynamic_object.kinematics.has_position_covariance = det_object.kinematics.has_position_covariance; if ( det_object.kinematics.orientation_availability == @@ -52,6 +55,7 @@ DynamicObject toDynamicObject( dynamic_object.kinematics.has_twist = det_object.kinematics.has_twist; dynamic_object.kinematics.has_twist_covariance = det_object.kinematics.has_twist_covariance; + // shape dynamic_object.shape = det_object.shape; return dynamic_object; @@ -73,12 +77,15 @@ DynamicObjectList toDynamicObjectList( autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object) { autoware_perception_msgs::msg::TrackedObject tracked_object; - tracked_object.object_id = dyn_object.object_id; + tracked_object.object_id = dyn_object.uuid; tracked_object.existence_probability = dyn_object.existence_probability; tracked_object.classification = dyn_object.classification; - tracked_object.kinematics.pose_with_covariance = dyn_object.kinematics.pose_with_covariance; - tracked_object.kinematics.twist_with_covariance = dyn_object.kinematics.twist_with_covariance; + tracked_object.kinematics.pose_with_covariance.pose = dyn_object.pose; + tracked_object.kinematics.pose_with_covariance.covariance = dyn_object.pose_covariance; + tracked_object.kinematics.twist_with_covariance.twist = dyn_object.twist; + tracked_object.kinematics.twist_with_covariance.covariance = dyn_object.twist_covariance; + if (dyn_object.kinematics.orientation_availability == OrientationAvailability::UNAVAILABLE) { tracked_object.kinematics.orientation_availability = autoware_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; diff --git a/perception/autoware_multi_object_tracker/lib/odometry.cpp b/perception/autoware_multi_object_tracker/lib/odometry.cpp index 2d31fe876c329..538d619d14870 100644 --- a/perception/autoware_multi_object_tracker/lib/odometry.cpp +++ b/perception/autoware_multi_object_tracker/lib/odometry.cpp @@ -157,20 +157,23 @@ std::optional Odometry::transformObjects( tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); } for (auto & object : output_objects.objects) { - auto & pose_with_cov = object.kinematics.pose_with_covariance; - tf2::fromMsg(pose_with_cov.pose, tf_objects_world2objects); + auto & pose = object.pose; + auto & pose_cov = object.pose_covariance; + tf2::fromMsg(pose, tf_objects_world2objects); tf_target2objects = tf_target2objects_world * tf_objects_world2objects; // transform pose, frame difference and object pose - tf2::toMsg(tf_target2objects, pose_with_cov.pose); + tf2::toMsg(tf_target2objects, pose); // transform covariance, only the frame difference - pose_with_cov.covariance = - tf2::transformCovariance(pose_with_cov.covariance, tf_target2objects_world); + pose_cov = tf2::transformCovariance(pose_cov, tf_target2objects_world); } } // Add the odometry uncertainty to the object uncertainty if (enable_odometry_uncertainty_) { // Create a modeled odometry message const auto odometry = getOdometryFromTf(input_objects.header.stamp); + if (!odometry) { + return std::nullopt; + } // Add the odometry uncertainty to the object uncertainty uncertainty::addOdometryUncertainty(odometry.value(), output_objects); } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 1fcfd56908db0..354335b4ec0c7 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -40,36 +40,18 @@ namespace autoware::multi_object_tracker { - -using Label = autoware_perception_msgs::msg::ObjectClassification; - -BicycleTracker::BicycleTracker( - const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) -: Tracker(time, object.classification, channel_size), - logger_(rclcpp::get_logger("BicycleTracker")), - z_(object.kinematics.pose_with_covariance.pose.position.z) +BicycleTracker::BicycleTracker(const rclcpp::Time & time, const types::DynamicObject & object) +: Tracker(time, object), logger_(rclcpp::get_logger("BicycleTracker")) { - object_ = object; - - // initialize existence probability - initializeExistenceProbabilities(object.channel_index, object.existence_probability); - - // OBJECT SHAPE MODEL - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - } else { - bounding_box_ = { - object_model_.init_size.length, object_model_.init_size.width, - object_model_.init_size.height}; // default value + if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + // set default initial size + auto & object_extension = object_.shape.dimensions; + object_extension.x = object_model_.init_size.length; + object_extension.y = object_model_.init_size.width; + object_extension.z = object_model_.init_size.height; } // set maximum and minimum size - bounding_box_.length = std::clamp( - bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); - bounding_box_.width = std::clamp( - bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); - bounding_box_.height = std::clamp( - bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); + limitObjectExtension(object_model_); // Set motion model parameters { @@ -100,11 +82,11 @@ BicycleTracker::BicycleTracker( // Set initial state { using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - const double x = object.kinematics.pose_with_covariance.pose.position.x; - const double y = object.kinematics.pose_with_covariance.pose.position.y; - const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double x = object.pose.position.x; + const double y = object.pose.position.y; + const double yaw = tf2::getYaw(object.pose.orientation); - auto pose_cov = object.kinematics.pose_with_covariance.covariance; + auto pose_cov = object.pose_covariance; if (!object.kinematics.has_position_covariance) { // initial state covariance const auto & p0_cov_x = object_model_.initial_covariance.pos_x; @@ -124,15 +106,15 @@ BicycleTracker::BicycleTracker( double vel = 0.0; double vel_cov = object_model_.initial_covariance.vel_long; if (object.kinematics.has_twist) { - vel = object.kinematics.twist_with_covariance.twist.linear.x; + vel = object.twist.linear.x; } if (object.kinematics.has_twist_covariance) { - vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; + vel_cov = object.twist_covariance[XYZRPY_COV_IDX::X_X]; } const double slip = 0.0; const double slip_cov = object_model_.bicycle_state.init_slip_angle_cov; - const double & length = bounding_box_.length; + const double & length = object_.shape.dimensions.x; // initialize motion model motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); @@ -148,17 +130,7 @@ types::DynamicObject BicycleTracker::getUpdatingObject( const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) const { - types::DynamicObject updating_object = object; - - // OBJECT SHAPE MODEL - // convert to bounding box if input is convex shape - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - if (!shapes::convertConvexHullToBoundingBox(object, updating_object)) { - updating_object = object; - } - } - - return updating_object; + return object; } bool BicycleTracker::measureWithPose(const types::DynamicObject & object) @@ -171,23 +143,21 @@ bool BicycleTracker::measureWithPose(const types::DynamicObject & object) // update bool is_updated = false; { - const double x = object.kinematics.pose_with_covariance.pose.position.x; - const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double x = object.pose.position.x; + const double y = object.pose.position.y; const double yaw = measurement_yaw; if (is_yaw_available) { - is_updated = motion_model_.updateStatePoseHead( - x, y, yaw, object.kinematics.pose_with_covariance.covariance); + is_updated = motion_model_.updateStatePoseHead(x, y, yaw, object.pose_covariance); } else { - is_updated = - motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); + is_updated = motion_model_.updateStatePose(x, y, object.pose_covariance); } motion_model_.limitStates(); } // position z constexpr double gain = 0.1; - z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; + object_.pose.position.z = (1.0 - gain) * object_.pose.position.z + gain * object.pose.position.z; return is_updated; } @@ -203,29 +173,28 @@ bool BicycleTracker::measureWithShape(const types::DynamicObject & object) constexpr double size_max = 30.0; // [m] constexpr double size_min = 0.1; // [m] if ( - object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max || - object.shape.dimensions.z > size_max || object.shape.dimensions.x < size_min || - object.shape.dimensions.y < size_min || object.shape.dimensions.z < size_min) { + object.shape.dimensions.x > size_max || object.shape.dimensions.x < size_min || + object.shape.dimensions.y > size_max || object.shape.dimensions.y < size_min || + object.shape.dimensions.z > size_max || object.shape.dimensions.z < size_min) { return false; } // update object size constexpr double gain = 0.1; constexpr double gain_inv = 1.0 - gain; - bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; - bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; - bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; + auto & object_extension = object_.shape.dimensions; + object_extension.x = gain_inv * object_extension.x + gain * object.shape.dimensions.x; + object_extension.y = gain_inv * object_extension.y + gain * object.shape.dimensions.y; + object_extension.z = gain_inv * object_extension.z + gain * object.shape.dimensions.z; + + // set shape type, which is bounding box + object_.shape.type = object.shape.type; // set maximum and minimum size - bounding_box_.length = std::clamp( - bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); - bounding_box_.width = std::clamp( - bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); - bounding_box_.height = std::clamp( - bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); + limitObjectExtension(object_model_); // update motion model - motion_model_.updateExtendedState(bounding_box_.length); + motion_model_.updateExtendedState(object_extension.x); return true; } @@ -234,17 +203,6 @@ bool BicycleTracker::measure( const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { - // keep the latest input object - object_ = object; - - // update classification - const auto & current_classification = getClassification(); - if ( - autoware::object_recognition_utils::getHighestProbLabel(object.classification) == - Label::UNKNOWN) { - setClassification(current_classification); - } - // check time gap const double dt = motion_model_.getDeltaTime(time); if (0.01 /*10msec*/ < dt) { @@ -267,29 +225,20 @@ bool BicycleTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; - object.object_id = getUUID(); - object.classification = getClassification(); - - auto & pose_with_cov = object.kinematics.pose_with_covariance; - auto & twist_with_cov = object.kinematics.twist_with_covariance; + auto & pose = object.pose; + auto & pose_cov = object.pose_covariance; + auto & twist = object.twist; + auto & twist_cov = object.twist_covariance; // predict from motion model - if (!motion_model_.getPredictedState( - time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, - twist_with_cov.covariance)) { + if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "BicycleTracker::getTrackedObject: Failed to get predicted state."); return false; } - // position - pose_with_cov.pose.position.z = z_; - // set shape - object.shape.dimensions.x = bounding_box_.length; - object.shape.dimensions.y = bounding_box_.width; - object.shape.dimensions.z = bounding_box_.height; - const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); - const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); + const auto origin_yaw = tf2::getYaw(object_.pose.orientation); + const auto ekf_pose_yaw = tf2::getYaw(pose.orientation); object.shape.footprint = autoware_utils::rotate_polygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index 9f249ab3bc7bc..b23ca7386e1f1 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -22,17 +22,12 @@ namespace autoware::multi_object_tracker { - -using Label = autoware_perception_msgs::msg::ObjectClassification; - MultipleVehicleTracker::MultipleVehicleTracker( - const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) -: Tracker(time, object.classification, channel_size), - normal_vehicle_tracker_(object_model::normal_vehicle, time, object, channel_size), - big_vehicle_tracker_(object_model::big_vehicle, time, object, channel_size) + const rclcpp::Time & time, const types::DynamicObject & object) +: Tracker(time, object), + normal_vehicle_tracker_(object_model::normal_vehicle, time, object), + big_vehicle_tracker_(object_model::big_vehicle, time, object) { - // initialize existence probability - initializeExistenceProbabilities(object.channel_index, object.existence_probability); } bool MultipleVehicleTracker::predict(const rclcpp::Time & time) @@ -48,10 +43,7 @@ bool MultipleVehicleTracker::measure( { big_vehicle_tracker_.measure(object, time, self_transform); normal_vehicle_tracker_.measure(object, time, self_transform); - if ( - autoware::object_recognition_utils::getHighestProbLabel(object.classification) != - Label::UNKNOWN) - updateClassification(object.classification); + return true; } @@ -66,8 +58,8 @@ bool MultipleVehicleTracker::getTrackedObject( } else if (label == Label::BUS || label == Label::TRUCK || label == Label::TRAILER) { big_vehicle_tracker_.getTrackedObject(time, object); } - object.object_id = getUUID(); - object.classification = getClassification(); + object.uuid = object_.uuid; + object.classification = object_.classification; return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index 0ec33d3fafcbe..252641a810223 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -34,18 +34,11 @@ namespace autoware::multi_object_tracker { - PassThroughTracker::PassThroughTracker( - const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) -: Tracker(time, object.classification, channel_size), - logger_(rclcpp::get_logger("PassThroughTracker")), - last_update_time_(time) + const rclcpp::Time & time, const types::DynamicObject & object) +: Tracker(time, object), logger_(rclcpp::get_logger("PassThroughTracker")), last_update_time_(time) { - object_ = object; prev_observed_object_ = object; - - // initialize existence probability - initializeExistenceProbabilities(object.channel_index, object.existence_probability); } bool PassThroughTracker::predict(const rclcpp::Time & time) @@ -69,11 +62,9 @@ bool PassThroughTracker::measure( // Update Velocity if the observed object does not have twist information const double dt = (time - last_update_time_).seconds(); if (!object_.kinematics.has_twist && dt > 1e-6) { - const double dx = object_.kinematics.pose_with_covariance.pose.position.x - - prev_observed_object_.kinematics.pose_with_covariance.pose.position.x; - const double dy = object_.kinematics.pose_with_covariance.pose.position.y - - prev_observed_object_.kinematics.pose_with_covariance.pose.position.y; - object_.kinematics.twist_with_covariance.twist.linear.x = std::hypot(dx, dy) / dt; + const double dx = object_.pose.position.x - prev_observed_object_.pose.position.x; + const double dy = object_.pose.position.y - prev_observed_object_.pose.position.y; + object_.twist.linear.x = std::hypot(dx, dy) / dt; } last_update_time_ = time; @@ -86,24 +77,23 @@ bool PassThroughTracker::getTrackedObject( { using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; object = object_; - object.object_id = getUUID(); - object.classification = getClassification(); - object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X] = 0.0; - object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_Y] = 0.0; - object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_X] = 0.0; - object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y] = 0.0; - object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::Z_Z] = 0.0; - object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = 0.0; - object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = 0.0; - object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::YAW_YAW] = 0.0; + + object.pose_covariance[XYZRPY_COV_IDX::X_X] = 0.0; + object.pose_covariance[XYZRPY_COV_IDX::X_Y] = 0.0; + object.pose_covariance[XYZRPY_COV_IDX::Y_X] = 0.0; + object.pose_covariance[XYZRPY_COV_IDX::Y_Y] = 0.0; + object.pose_covariance[XYZRPY_COV_IDX::Z_Z] = 0.0; + object.pose_covariance[XYZRPY_COV_IDX::ROLL_ROLL] = 0.0; + object.pose_covariance[XYZRPY_COV_IDX::PITCH_PITCH] = 0.0; + object.pose_covariance[XYZRPY_COV_IDX::YAW_YAW] = 0.0; // twist covariance - object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X] = 0.0; - object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Y_Y] = 0.0; - object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::Z_Z] = 0.0; - object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::ROLL_ROLL] = 0.0; - object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::PITCH_PITCH] = 0.0; - object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::YAW_YAW] = 0.0; + object.twist_covariance[XYZRPY_COV_IDX::X_X] = 0.0; + object.twist_covariance[XYZRPY_COV_IDX::Y_Y] = 0.0; + object.twist_covariance[XYZRPY_COV_IDX::Z_Z] = 0.0; + object.twist_covariance[XYZRPY_COV_IDX::ROLL_ROLL] = 0.0; + object.twist_covariance[XYZRPY_COV_IDX::PITCH_PITCH] = 0.0; + object.twist_covariance[XYZRPY_COV_IDX::YAW_YAW] = 0.0; const double dt = (time - last_update_time_).seconds(); if (0.5 /*500msec*/ < dt) { diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 21ce949231062..486cf5e90c1d8 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -20,17 +20,10 @@ namespace autoware::multi_object_tracker { - -using Label = autoware_perception_msgs::msg::ObjectClassification; - PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( - const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) -: Tracker(time, object.classification, channel_size), - pedestrian_tracker_(time, object, channel_size), - bicycle_tracker_(time, object, channel_size) + const rclcpp::Time & time, const types::DynamicObject & object) +: Tracker(time, object), pedestrian_tracker_(time, object), bicycle_tracker_(time, object) { - // initialize existence probability - initializeExistenceProbabilities(object.channel_index, object.existence_probability); } bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) @@ -46,10 +39,7 @@ bool PedestrianAndBicycleTracker::measure( { pedestrian_tracker_.measure(object, time, self_transform); bicycle_tracker_.measure(object, time, self_transform); - if ( - autoware::object_recognition_utils::getHighestProbLabel(object.classification) != - Label::UNKNOWN) - updateClassification(object.classification); + return true; } @@ -64,8 +54,8 @@ bool PedestrianAndBicycleTracker::getTrackedObject( } else if (label == Label::BICYCLE || label == Label::MOTORCYCLE) { bicycle_tracker_.getTrackedObject(time, object); } - object.object_id = getUUID(); - object.classification = getClassification(); + object.uuid = object_.uuid; + object.classification = object_.classification; return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index fecd02d4ea898..3058d410120c2 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -38,44 +38,18 @@ namespace autoware::multi_object_tracker { - -using Label = autoware_perception_msgs::msg::ObjectClassification; - -PedestrianTracker::PedestrianTracker( - const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) -: Tracker(time, object.classification, channel_size), - logger_(rclcpp::get_logger("PedestrianTracker")), - z_(object.kinematics.pose_with_covariance.pose.position.z) +PedestrianTracker::PedestrianTracker(const rclcpp::Time & time, const types::DynamicObject & object) +: Tracker(time, object), logger_(rclcpp::get_logger("PedestrianTracker")) { - object_ = object; - - // initialize existence probability - initializeExistenceProbabilities(object.channel_index, object.existence_probability); - - // OBJECT SHAPE MODEL - bounding_box_ = { - object_model_.init_size.length, object_model_.init_size.width, - object_model_.init_size.height}; // default value - cylinder_ = {object_model_.init_size.length, object_model_.init_size.height}; // default value - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { - cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; - } else if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { - // do not update polygon shape + if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { + // set default initial size + auto & object_extension = object_.shape.dimensions; + object_extension.x = object_model_.init_size.length; + object_extension.y = object_model_.init_size.width; + object_extension.z = object_model_.init_size.height; } // set maximum and minimum size - bounding_box_.length = std::clamp( - bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); - bounding_box_.width = std::clamp( - bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); - bounding_box_.height = std::clamp( - bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); - cylinder_.width = std::clamp( - cylinder_.width, object_model_.size_limit.length_min, object_model_.size_limit.length_max); - cylinder_.height = std::clamp( - cylinder_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); + limitObjectExtension(object_model_); // Set motion model parameters { @@ -97,11 +71,11 @@ PedestrianTracker::PedestrianTracker( // Set initial state { using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - const double x = object.kinematics.pose_with_covariance.pose.position.x; - const double y = object.kinematics.pose_with_covariance.pose.position.y; - const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double x = object.pose.position.x; + const double y = object.pose.position.y; + const double yaw = tf2::getYaw(object.pose.orientation); - auto pose_cov = object.kinematics.pose_with_covariance.covariance; + auto pose_cov = object.pose_covariance; if (!object.kinematics.has_position_covariance) { // initial state covariance const auto & p0_cov_x = object_model_.initial_covariance.pos_x; @@ -121,15 +95,15 @@ PedestrianTracker::PedestrianTracker( double vel = 0.0; double wz = 0.0; if (object.kinematics.has_twist) { - vel = object.kinematics.twist_with_covariance.twist.linear.x; - wz = object.kinematics.twist_with_covariance.twist.angular.z; + vel = object.twist.linear.x; + wz = object.twist.angular.z; } double vel_cov = object_model_.initial_covariance.vel_long; double wz_cov = object_model_.initial_covariance.yaw_rate; if (object.kinematics.has_twist_covariance) { - vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; - wz_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::YAW_YAW]; + vel_cov = object.twist_covariance[XYZRPY_COV_IDX::X_X]; + wz_cov = object.twist_covariance[XYZRPY_COV_IDX::YAW_YAW]; } // initialize motion model @@ -146,9 +120,7 @@ types::DynamicObject PedestrianTracker::getUpdatingObject( const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) const { - types::DynamicObject updating_object = object; - - return updating_object; + return object; } bool PedestrianTracker::measureWithPose(const types::DynamicObject & object) @@ -156,73 +128,48 @@ bool PedestrianTracker::measureWithPose(const types::DynamicObject & object) // update motion model bool is_updated = false; { - const double x = object.kinematics.pose_with_covariance.pose.position.x; - const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double x = object.pose.position.x; + const double y = object.pose.position.y; - is_updated = - motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); + is_updated = motion_model_.updateStatePose(x, y, object.pose_covariance); motion_model_.limitStates(); } // position z constexpr double gain = 0.1; - z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; + object_.pose.position.z = (1.0 - gain) * object_.pose.position.z + gain * object.pose.position.z; return is_updated; } bool PedestrianTracker::measureWithShape(const types::DynamicObject & object) { - constexpr double gain = 0.1; - constexpr double gain_inv = 1.0 - gain; - - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - // check bound box size abnormality + if (object.shape.type != autoware_perception_msgs::msg::Shape::POLYGON) { constexpr double size_max = 30.0; // [m] constexpr double size_min = 0.1; // [m] - bool is_size_valid = - (object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max && - object.shape.dimensions.z <= size_max && object.shape.dimensions.x >= size_min && - object.shape.dimensions.y >= size_min && object.shape.dimensions.z >= size_min); - if (!is_size_valid) { + if ( + object.shape.dimensions.x > size_max || object.shape.dimensions.x < size_min || + object.shape.dimensions.y > size_max || object.shape.dimensions.y < size_min || + object.shape.dimensions.z > size_max || object.shape.dimensions.z < size_min) { return false; } - // update bounding box size - bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; - bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; - bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; - - } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { - // check cylinder size abnormality - constexpr double size_max = 30.0; // [m] - constexpr double size_min = 0.1; // [m] - bool is_size_valid = - (object.shape.dimensions.x <= size_max && object.shape.dimensions.z <= size_max && - object.shape.dimensions.x >= size_min && object.shape.dimensions.z >= size_min); - if (!is_size_valid) { - return false; - } - // update cylinder size - cylinder_.width = gain_inv * cylinder_.width + gain * object.shape.dimensions.x; - cylinder_.height = gain_inv * cylinder_.height + gain * object.shape.dimensions.z; - + constexpr double gain = 0.5; + constexpr double gain_inv = 1.0 - gain; + auto & object_extension = object_.shape.dimensions; + object_extension.x = gain_inv * object_extension.x + gain * object.shape.dimensions.x; + object_extension.y = gain_inv * object_extension.y + gain * object.shape.dimensions.y; + object_extension.z = gain_inv * object_extension.z + gain * object.shape.dimensions.z; + + // update shape type, bounding box or cylinder + object_.shape.type = object.shape.type; + + // set maximum and minimum size + limitObjectExtension(object_model_); } else { // do not update polygon shape return false; } - // set maximum and minimum size - bounding_box_.length = std::clamp( - bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); - bounding_box_.width = std::clamp( - bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); - bounding_box_.height = std::clamp( - bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); - cylinder_.width = std::clamp( - cylinder_.width, object_model_.size_limit.length_min, object_model_.size_limit.length_max); - cylinder_.height = std::clamp( - cylinder_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); - return true; } @@ -233,13 +180,6 @@ bool PedestrianTracker::measure( // keep the latest input object object_ = object; - const auto & current_classification = getClassification(); - if ( - autoware::object_recognition_utils::getHighestProbLabel(object.classification) == - Label::UNKNOWN) { - setClassification(current_classification); - } - // check time gap const double dt = motion_model_.getDeltaTime(time); if (0.01 /*10msec*/ < dt) { @@ -263,38 +203,22 @@ bool PedestrianTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; - object.object_id = getUUID(); - object.classification = getClassification(); - - auto & pose_with_cov = object.kinematics.pose_with_covariance; - auto & twist_with_cov = object.kinematics.twist_with_covariance; + auto & pose = object.pose; + auto & pose_cov = object.pose_covariance; + auto & twist = object.twist; + auto & twist_cov = object.twist_covariance; // predict from motion model - if (!motion_model_.getPredictedState( - time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, - twist_with_cov.covariance)) { + if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "PedestrianTracker::getTrackedObject: Failed to get predicted state."); return false; } - // position - pose_with_cov.pose.position.z = z_; - // set shape - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - object.shape.dimensions.x = bounding_box_.length; - object.shape.dimensions.y = bounding_box_.width; - object.shape.dimensions.z = bounding_box_.height; - } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { - object.shape.dimensions.x = cylinder_.width; - object.shape.dimensions.y = cylinder_.width; - object.shape.dimensions.z = cylinder_.height; - } else if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { - const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); - const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); - object.shape.footprint = - autoware_utils::rotate_polygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); - } + const auto origin_yaw = tf2::getYaw(object_.pose.orientation); + const auto ekf_pose_yaw = tf2::getYaw(pose.orientation); + object.shape.footprint = + autoware_utils::rotate_polygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp index 31ad1bf94cadd..0d0dca86a4136 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -42,23 +42,22 @@ float decayProbability(const float & prior, const float & delta_time) namespace autoware::multi_object_tracker { -Tracker::Tracker( - const rclcpp::Time & time, - const std::vector & classification, - const size_t & channel_size) -: classification_(classification), - no_measurement_count_(0), +Tracker::Tracker(const rclcpp::Time & time, const types::DynamicObject & detected_object) +: no_measurement_count_(0), total_no_measurement_count_(0), total_measurement_count_(1), - last_update_with_measurement_time_(time) + last_update_with_measurement_time_(time), + object_(detected_object) { // Generate random number std::mt19937 gen(std::random_device{}()); std::independent_bits_engine bit_eng(gen); - std::generate(uuid_.uuid.begin(), uuid_.uuid.end(), bit_eng); + unique_identifier_msgs::msg::UUID uuid_msg; + std::generate(uuid_msg.uuid.begin(), uuid_msg.uuid.end(), bit_eng); + object_.uuid = uuid_msg; // Initialize existence probabilities - existence_probabilities_.resize(channel_size, 0.001); + existence_probabilities_.resize(types::max_channel_size, 0.001); total_existence_probability_ = 0.001; } @@ -80,7 +79,7 @@ void Tracker::initializeExistenceProbabilities( bool Tracker::updateWithMeasurement( const types::DynamicObject & object, const rclcpp::Time & measurement_time, - const geometry_msgs::msg::Transform & self_transform) + const geometry_msgs::msg::Transform & self_transform, const types::InputChannel & channel_info) { // Update existence probability { @@ -94,7 +93,7 @@ bool Tracker::updateWithMeasurement( constexpr float probability_false_detection = 0.2; // update measured channel probability without decay - const uint channel_index = object.channel_index; + const uint & channel_index = channel_info.index; existence_probabilities_[channel_index] = updateProbability( existence_probabilities_[channel_index], probability_true_detection, probability_false_detection); @@ -113,6 +112,13 @@ bool Tracker::updateWithMeasurement( last_update_with_measurement_time_ = measurement_time; + // Update classification + if ( + autoware::object_recognition_utils::getHighestProbLabel(object.classification) != + autoware_perception_msgs::msg::ObjectClassification::UNKNOWN) { + updateClassification(object.classification); + } + // Update object measure(object, measurement_time, self_transform); @@ -168,6 +174,8 @@ void Tracker::updateClassification( auto classification_input = classification; normalizeProbabilities(classification_input); + auto & classification_ = object_.classification; + // Update the matched classification probability with a gain for (const auto & new_class : classification_input) { bool found = false; @@ -197,12 +205,16 @@ void Tracker::updateClassification( normalizeProbabilities(classification_); } -geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( - const rclcpp::Time & time) const +void Tracker::limitObjectExtension(const object_model::ObjectModel object_model) { - types::DynamicObject object; - getTrackedObject(time, object); - return object.kinematics.pose_with_covariance; + auto & object_extension = object_.shape.dimensions; + // set maximum and minimum size + object_extension.x = std::clamp( + object_extension.x, object_model.size_limit.length_min, object_model.size_limit.length_max); + object_extension.y = std::clamp( + object_extension.y, object_model.size_limit.width_min, object_model.size_limit.width_max); + object_extension.z = std::clamp( + object_extension.z, object_model.size_limit.height_min, object_model.size_limit.height_max); } } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index 1da27db525b45..cd27796557194 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -35,24 +35,9 @@ namespace autoware::multi_object_tracker { -UnknownTracker::UnknownTracker( - const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) -: Tracker(time, object.classification, channel_size), - logger_(rclcpp::get_logger("UnknownTracker")), - z_(object.kinematics.pose_with_covariance.pose.position.z) +UnknownTracker::UnknownTracker(const rclcpp::Time & time, const types::DynamicObject & object) +: Tracker(time, object), logger_(rclcpp::get_logger("UnknownTracker")) { - object_ = object; - - // initialize existence probability - initializeExistenceProbabilities(object.channel_index, object.existence_probability); - - // initialize params - // measurement noise covariance - constexpr double r_stddev_x = 1.0; // [m] - constexpr double r_stddev_y = 1.0; // [m] - ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); - ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); - // Set motion model parameters { constexpr double q_stddev_x = 0.5; // [m/s] @@ -71,17 +56,17 @@ UnknownTracker::UnknownTracker( // Set initial state { using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - const double x = object.kinematics.pose_with_covariance.pose.position.x; - const double y = object.kinematics.pose_with_covariance.pose.position.y; - auto pose_cov = object.kinematics.pose_with_covariance.covariance; - auto twist_cov = object.kinematics.twist_with_covariance.covariance; - const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double x = object.pose.position.x; + const double y = object.pose.position.y; + auto pose_cov = object.pose_covariance; + auto twist_cov = object.twist_covariance; + const double yaw = tf2::getYaw(object.pose.orientation); double vx = 0.0; double vy = 0.0; if (object.kinematics.has_twist) { - const double & vel_x = object.kinematics.twist_with_covariance.twist.linear.x; - const double & vel_y = object.kinematics.twist_with_covariance.twist.linear.y; + const double & vel_x = object.twist.linear.x; + const double & vel_y = object.twist.linear.y; vx = std::cos(yaw) * vel_x - std::sin(yaw) * vel_y; vy = std::sin(yaw) * vel_x + std::cos(yaw) * vel_y; } @@ -139,27 +124,7 @@ bool UnknownTracker::predict(const rclcpp::Time & time) types::DynamicObject UnknownTracker::getUpdatingObject( const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) { - types::DynamicObject updating_object = object; - - // UNCERTAINTY MODEL - if (!object.kinematics.has_position_covariance) { - // fill covariance matrix - using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - const double & r_cov_x = ekf_params_.r_cov_x; - const double & r_cov_y = ekf_params_.r_cov_y; - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const double cos_yaw = std::cos(pose_yaw); - const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0f * pose_yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[XYZRPY_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x - } - return updating_object; + return object; } bool UnknownTracker::measureWithPose(const types::DynamicObject & object) @@ -167,17 +132,16 @@ bool UnknownTracker::measureWithPose(const types::DynamicObject & object) // update motion model bool is_updated = false; { - const double x = object.kinematics.pose_with_covariance.pose.position.x; - const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double x = object.pose.position.x; + const double y = object.pose.position.y; - is_updated = - motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); + is_updated = motion_model_.updateStatePose(x, y, object.pose_covariance); motion_model_.limitStates(); } // position z constexpr double gain = 0.1; - z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; + object_.pose.position.z = (1.0 - gain) * object_.pose.position.z + gain * object.pose.position.z; return is_updated; } @@ -210,23 +174,17 @@ bool UnknownTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; - object.object_id = getUUID(); - object.classification = getClassification(); - - auto & pose_with_cov = object.kinematics.pose_with_covariance; - auto & twist_with_cov = object.kinematics.twist_with_covariance; + auto & pose = object.pose; + auto & pose_cov = object.pose_covariance; + auto & twist = object.twist; + auto & twist_cov = object.twist_covariance; // predict from motion model - if (!motion_model_.getPredictedState( - time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, - twist_with_cov.covariance)) { + if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "UnknownTracker::getTrackedObject: Failed to get predicted state."); return false; } - // position - pose_with_cov.pose.position.z = z_; - return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index 201db246fb4b3..902fc62fac9cb 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -40,55 +40,28 @@ namespace autoware::multi_object_tracker { - -using Label = autoware_perception_msgs::msg::ObjectClassification; - VehicleTracker::VehicleTracker( const object_model::ObjectModel & object_model, const rclcpp::Time & time, - const types::DynamicObject & object, const size_t channel_size) -: Tracker(time, object.classification, channel_size), + const types::DynamicObject & object) +: Tracker(time, object), object_model_(object_model), logger_(rclcpp::get_logger("VehicleTracker")), - z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { - object_ = object; - - // initialize existence probability - initializeExistenceProbabilities(object.channel_index, object.existence_probability); - // velocity deviation threshold // if the predicted velocity is close to the observed velocity, // the observed velocity is used as the measurement. velocity_deviation_threshold_ = autoware_utils::kmph2mps(10); // [m/s] - // OBJECT SHAPE MODEL - if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - } else { - types::DynamicObject bbox_object; - if (!shapes::convertConvexHullToBoundingBox(object, bbox_object)) { - RCLCPP_WARN( - logger_, - "VehicleTracker::VehicleTracker: Failed to convert convex hull to bounding " - "box."); - bounding_box_ = { - object_model_.init_size.length, object_model_.init_size.width, - object_model_.init_size.height}; // default value - } else { - bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; - } + if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + // set default initial size + auto & object_extension = object_.shape.dimensions; + object_extension.x = object_model_.init_size.length; + object_extension.y = object_model_.init_size.width; + object_extension.z = object_model_.init_size.height; } // set maximum and minimum size - bounding_box_.length = std::clamp( - bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); - bounding_box_.width = std::clamp( - bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); - bounding_box_.height = std::clamp( - bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); + limitObjectExtension(object_model_); // Set motion model parameters { @@ -119,11 +92,11 @@ VehicleTracker::VehicleTracker( // Set initial state { using autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - const double x = object.kinematics.pose_with_covariance.pose.position.x; - const double y = object.kinematics.pose_with_covariance.pose.position.y; - const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double x = object.pose.position.x; + const double y = object.pose.position.y; + const double yaw = tf2::getYaw(object.pose.orientation); - auto pose_cov = object.kinematics.pose_with_covariance.covariance; + auto pose_cov = object.pose_covariance; if (!object.kinematics.has_position_covariance) { // initial state covariance const auto & p0_cov_x = object_model_.initial_covariance.pos_x; @@ -143,15 +116,15 @@ VehicleTracker::VehicleTracker( double vel = 0.0; double vel_cov = object_model_.initial_covariance.vel_long; if (object.kinematics.has_twist) { - vel = object.kinematics.twist_with_covariance.twist.linear.x; + vel = object.twist.linear.x; } if (object.kinematics.has_twist_covariance) { - vel_cov = object.kinematics.twist_with_covariance.covariance[XYZRPY_COV_IDX::X_X]; + vel_cov = object.twist_covariance[XYZRPY_COV_IDX::X_X]; } const double slip = 0.0; const double slip_cov = object_model_.bicycle_state.init_slip_angle_cov; - const double & length = bounding_box_.length; + const double & length = object_.shape.dimensions.x; // initialize motion model motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); @@ -168,18 +141,6 @@ types::DynamicObject VehicleTracker::getUpdatingObject( { types::DynamicObject updating_object = object; - // OBJECT SHAPE MODEL - // convert to bounding box if input is convex shape - types::DynamicObject bbox_object = object; - if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - if (!shapes::convertConvexHullToBoundingBox(object, bbox_object)) { - RCLCPP_WARN( - logger_, - "VehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); - bbox_object = object; - } - } - // current (predicted) state const double tracked_x = motion_model_.getStateElement(IDX::X); const double tracked_y = motion_model_.getStateElement(IDX::Y); @@ -187,10 +148,11 @@ types::DynamicObject VehicleTracker::getUpdatingObject( // get offset measurement const int nearest_corner_index = shapes::getNearestCornerOrSurface( - tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); + tracked_x, tracked_y, tracked_yaw, object_.shape.dimensions.y, object_.shape.dimensions.x, + self_transform); shapes::calcAnchorPointOffset( - bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, - updating_object, tracking_offset_); + object_.shape.dimensions.y, object_.shape.dimensions.x, nearest_corner_index, object, + tracked_yaw, updating_object, tracking_offset_); return updating_object; } @@ -204,7 +166,7 @@ bool VehicleTracker::measureWithPose(const types::DynamicObject & object) // and the predicted velocity is close to the observed velocity bool is_velocity_available = false; if (object.kinematics.has_twist) { - const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + const double & observed_vel = object.twist.linear.x; if (std::fabs(tracked_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small is_velocity_available = true; @@ -214,25 +176,23 @@ bool VehicleTracker::measureWithPose(const types::DynamicObject & object) // update bool is_updated = false; { - const double x = object.kinematics.pose_with_covariance.pose.position.x; - const double y = object.kinematics.pose_with_covariance.pose.position.y; - const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const double vel = object.kinematics.twist_with_covariance.twist.linear.x; + const double x = object.pose.position.x; + const double y = object.pose.position.y; + const double yaw = tf2::getYaw(object.pose.orientation); + const double vel = object.twist.linear.x; if (is_velocity_available) { is_updated = motion_model_.updateStatePoseHeadVel( - x, y, yaw, object.kinematics.pose_with_covariance.covariance, vel, - object.kinematics.twist_with_covariance.covariance); + x, y, yaw, object.pose_covariance, vel, object.twist_covariance); } else { - is_updated = motion_model_.updateStatePoseHead( - x, y, yaw, object.kinematics.pose_with_covariance.covariance); + is_updated = motion_model_.updateStatePoseHead(x, y, yaw, object.pose_covariance); } motion_model_.limitStates(); } // position z constexpr double gain = 0.1; - z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; + object_.pose.position.z = (1.0 - gain) * object_.pose.position.z + gain * object.pose.position.z; return is_updated; } @@ -247,30 +207,25 @@ bool VehicleTracker::measureWithShape(const types::DynamicObject & object) // check object size abnormality constexpr double size_max = 35.0; // [m] constexpr double size_min = 1.0; // [m] - bool is_size_valid = - (object.shape.dimensions.x <= size_max && object.shape.dimensions.y <= size_max && - object.shape.dimensions.x >= size_min && object.shape.dimensions.y >= size_min); - if (!is_size_valid) { + if ( + object.shape.dimensions.x > size_max || object.shape.dimensions.x < size_min || + object.shape.dimensions.y > size_max || object.shape.dimensions.y < size_min) { return false; } // update object size constexpr double gain = 0.5; constexpr double gain_inv = 1.0 - gain; - bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; - bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; - bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; + auto & object_extension = object_.shape.dimensions; + object_extension.x = gain_inv * object_extension.x + gain * object.shape.dimensions.x; + object_extension.y = gain_inv * object_extension.y + gain * object.shape.dimensions.y; + object_extension.z = gain_inv * object_extension.z + gain * object.shape.dimensions.z; // set maximum and minimum size - bounding_box_.length = std::clamp( - bounding_box_.length, object_model_.size_limit.length_min, object_model_.size_limit.length_max); - bounding_box_.width = std::clamp( - bounding_box_.width, object_model_.size_limit.width_min, object_model_.size_limit.width_max); - bounding_box_.height = std::clamp( - bounding_box_.height, object_model_.size_limit.height_min, object_model_.size_limit.height_max); + limitObjectExtension(object_model_); // update motion model - motion_model_.updateExtendedState(bounding_box_.length); + motion_model_.updateExtendedState(object_extension.x); // update offset into object position { @@ -296,14 +251,6 @@ bool VehicleTracker::measure( // keep the latest input object object_ = object; - // update classification - const auto & current_classification = getClassification(); - if ( - autoware::object_recognition_utils::getHighestProbLabel(object.classification) == - Label::UNKNOWN) { - setClassification(current_classification); - } - // check time gap const double dt = motion_model_.getDeltaTime(time); if (0.01 /*10msec*/ < dt) { @@ -326,29 +273,20 @@ bool VehicleTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; - object.object_id = getUUID(); - object.classification = getClassification(); - - auto & pose_with_cov = object.kinematics.pose_with_covariance; - auto & twist_with_cov = object.kinematics.twist_with_covariance; + auto & pose = object.pose; + auto & pose_cov = object.pose_covariance; + auto & twist = object.twist; + auto & twist_cov = object.twist_covariance; // predict from motion model - if (!motion_model_.getPredictedState( - time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, - twist_with_cov.covariance)) { + if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "VehicleTracker::getTrackedObject: Failed to get predicted state."); return false; } - // position - pose_with_cov.pose.position.z = z_; - // set shape - object.shape.dimensions.x = bounding_box_.length; - object.shape.dimensions.y = bounding_box_.width; - object.shape.dimensions.z = bounding_box_.height; - const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); - const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); + const auto origin_yaw = tf2::getYaw(object_.pose.orientation); + const auto ekf_pose_yaw = tf2::getYaw(pose.orientation); object.shape.footprint = autoware_utils::rotate_polygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 52dd3bc3cc66f..a0b5861acbd0b 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -412,7 +412,7 @@ bool BicycleMotionModel::getPredictedState( // set position pose.position.x = X(IDX::X); pose.position.y = X(IDX::Y); - pose.position.z = 0.0; + // do not change z // set orientation tf2::Quaternion quaternion; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index 14d7ce8eb67aa..6a9850c6fa601 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -329,7 +329,7 @@ bool CTRVMotionModel::getPredictedState( // set position pose.position.x = X(IDX::X); pose.position.y = X(IDX::Y); - pose.position.z = 0.0; + // do not change z // set orientation tf2::Quaternion quaternion; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp index e208f8b6ad1f9..5137cda71f3a7 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp @@ -241,7 +241,7 @@ bool CVMotionModel::getPredictedState( // set position pose.position.x = X(IDX::X); pose.position.y = X(IDX::Y); - pose.position.z = 0.0; + // do not change z // set twist twist.linear.x = X(IDX::VX) * std::cos(-yaw) - X(IDX::VY) * std::sin(-yaw); diff --git a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp index 5923000c6bfc7..9c27af02a0a23 100644 --- a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp +++ b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp @@ -50,6 +50,9 @@ object_model::StateCovariance covarianceFromObjectClass(const ObjectClassificati case ObjectClassification::PEDESTRIAN: obj_class_model = object_model::pedestrian; break; + case ObjectClassification::UNKNOWN: + obj_class_model = object_model::unknown; + break; default: obj_class_model = object_model::normal_vehicle; break; @@ -70,10 +73,10 @@ types::DynamicObject modelUncertaintyByClass( const auto & r_cov_y = measurement_covariance.pos_y; // yaw angle - const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double pose_yaw = tf2::getYaw(object.pose.orientation); // fill position covariance matrix - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + auto & pose_cov = updating_object.pose_covariance; const double cos_yaw = std::cos(pose_yaw); const double sin_yaw = std::sin(pose_yaw); const double sin_2yaw = std::sin(2.0 * pose_yaw); @@ -95,7 +98,7 @@ types::DynamicObject modelUncertaintyByClass( } // fill twist covariance matrix - auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + auto & twist_cov = updating_object.twist_covariance; twist_cov[XYZRPY_COV_IDX::X_X] = measurement_covariance.vel_long; twist_cov[XYZRPY_COV_IDX::X_Y] = 0.0; twist_cov[XYZRPY_COV_IDX::Y_X] = 0.0; @@ -129,14 +132,14 @@ void normalizeUncertainty(types::DynamicObjectList & detected_objects) for (auto & object : detected_objects.objects) { // normalize position covariance - auto & pose_cov = object.kinematics.pose_with_covariance.covariance; + auto & pose_cov = object.pose_covariance; pose_cov[XYZRPY_COV_IDX::X_X] = std::max(pose_cov[XYZRPY_COV_IDX::X_X], min_cov_dist); pose_cov[XYZRPY_COV_IDX::Y_Y] = std::max(pose_cov[XYZRPY_COV_IDX::Y_Y], min_cov_dist); pose_cov[XYZRPY_COV_IDX::Z_Z] = std::max(pose_cov[XYZRPY_COV_IDX::Z_Z], min_cov_dist); pose_cov[XYZRPY_COV_IDX::YAW_YAW] = std::max(pose_cov[XYZRPY_COV_IDX::YAW_YAW], min_cov_rad); // normalize twist covariance - auto & twist_cov = object.kinematics.twist_with_covariance.covariance; + auto & twist_cov = object.twist_covariance; twist_cov[XYZRPY_COV_IDX::X_X] = std::max(twist_cov[XYZRPY_COV_IDX::X_X], min_cov_vel); twist_cov[XYZRPY_COV_IDX::Y_Y] = std::max(twist_cov[XYZRPY_COV_IDX::Y_Y], min_cov_vel); } @@ -174,8 +177,8 @@ void addOdometryUncertainty(const Odometry & odometry, types::DynamicObjectList const double & cov_yaw_rate = odom_twist_cov[35]; for (auto & object : detected_objects.objects) { - auto & object_pose = object.kinematics.pose_with_covariance.pose; - auto & object_pose_cov = object.kinematics.pose_with_covariance.covariance; + auto & object_pose = object.pose; + auto & object_pose_cov = object.pose_covariance; const double dx = object_pose.position.x - odom_pose.position.x; const double dy = object_pose.position.y - odom_pose.position.y; const double r2 = dx * dx + dy * dy; @@ -211,7 +214,7 @@ void addOdometryUncertainty(const Odometry & odometry, types::DynamicObjectList object_pose_cov[XYZRPY_COV_IDX::Y_Y] = m_pose_cov(1, 1); // 2. add odometry velocity uncertainty to the object velocity covariance - auto & object_twist_cov = object.kinematics.twist_with_covariance.covariance; + auto & object_twist_cov = object.twist_covariance; Eigen::MatrixXd m_twist_cov = Eigen::MatrixXd(2, 2); m_twist_cov << object_twist_cov[XYZRPY_COV_IDX::X_X], object_twist_cov[XYZRPY_COV_IDX::X_Y], object_twist_cov[XYZRPY_COV_IDX::Y_X], object_twist_cov[XYZRPY_COV_IDX::Y_Y]; diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp index 6a1703e029920..9500b15ef885a 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp @@ -54,11 +54,10 @@ int32_t uuidToInt(const boost::uuids::uuid & uuid) namespace autoware::multi_object_tracker { -TrackerObjectDebugger::TrackerObjectDebugger(const std::string & frame_id) +TrackerObjectDebugger::TrackerObjectDebugger( + const std::string & frame_id, const std::vector & channels_config) +: frame_id_(frame_id), channels_config_(channels_config) { - // set frame id - frame_id_ = frame_id; - // initialize markers markers_.markers.clear(); current_ids_.clear(); @@ -96,32 +95,30 @@ void TrackerObjectDebugger::collect( types::DynamicObject tracked_object; (*(tracker_itr))->getTrackedObject(message_time, tracked_object); - object_data.uuid = uuidToBoostUuid(tracked_object.object_id); - object_data.uuid_int = uuidToInt(object_data.uuid); - object_data.uuid_str = uuidToString(tracked_object.object_id); - object_data.channel_id = tracked_object.channel_index; + object_data.uuid = uuidToBoostUuid(tracked_object.uuid); + object_data.uuid_str = uuidToString(tracked_object.uuid); // tracker bool is_associated = false; geometry_msgs::msg::Point tracker_point, detection_point; - tracker_point.x = tracked_object.kinematics.pose_with_covariance.pose.position.x; - tracker_point.y = tracked_object.kinematics.pose_with_covariance.pose.position.y; - tracker_point.z = tracked_object.kinematics.pose_with_covariance.pose.position.z; + tracker_point.x = tracked_object.pose.position.x; + tracker_point.y = tracked_object.pose.position.y; + tracker_point.z = tracked_object.pose.position.z; // detection if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { const auto & associated_object = detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); - detection_point.x = associated_object.kinematics.pose_with_covariance.pose.position.x; - detection_point.y = associated_object.kinematics.pose_with_covariance.pose.position.y; - detection_point.z = associated_object.kinematics.pose_with_covariance.pose.position.z; + detection_point.x = associated_object.pose.position.x; + detection_point.y = associated_object.pose.position.y; + detection_point.z = associated_object.pose.position.z; is_associated = true; } else { detection_point.x = tracker_point.x; detection_point.y = tracker_point.y; detection_point.z = tracker_point.z; } - + object_data.channel_id = detected_objects.channel_index; object_data.tracker_point = tracker_point; object_data.detection_point = detection_point; object_data.is_associated = is_associated; @@ -144,7 +141,7 @@ void TrackerObjectDebugger::process() // update uuid_int for (auto & object_data : object_data_list_) { - current_ids_.insert(object_data.uuid_int); + current_ids_.insert(uuidToInt(object_data.uuid)); } // sort by uuid, collect the same uuid object_data as a group, and loop for the groups @@ -156,7 +153,7 @@ void TrackerObjectDebugger::process() [](const ObjectData & a, const ObjectData & b) { return a.uuid < b.uuid; }); // collect the same uuid object_data as a group - std::vector object_data_group; + std::vector object_data_group{}; boost::uuids::uuid previous_uuid = object_data_list_.front().uuid; for (const auto & object_data : object_data_list_) { // if the uuid is different, push the group and clear the group @@ -210,7 +207,7 @@ void TrackerObjectDebugger::draw( visualization_msgs::msg::Marker marker; marker.header.frame_id = frame_id_; marker.header.stamp = object_data_front.time; - marker.id = object_data_front.uuid_int; + marker.id = uuidToInt(object_data_front.uuid); marker.pose.position.x = 0; marker.pose.position.y = 0; marker.pose.position.z = 0; @@ -236,10 +233,12 @@ void TrackerObjectDebugger::draw( // print existence probability with channel name // probability to text, two digits of percentage std::string existence_probability_text = ""; - for (size_t i = 0; i < object_data_front.existence_vector.size(); ++i) { + const size_t channel_size = channels_config_.size(); + for (size_t i = 0; i < channel_size; ++i) { + if (object_data_front.existence_vector[i] < 0.00101) continue; std::stringstream stream; stream << std::fixed << std::setprecision(0) << object_data_front.existence_vector[i] * 100; - existence_probability_text += channel_names_[i] + stream.str() + ":"; + existence_probability_text += channels_config_[i].short_name + stream.str() + ":"; } if (!existence_probability_text.empty()) { existence_probability_text.pop_back(); @@ -247,7 +246,6 @@ void TrackerObjectDebugger::draw( existence_probability_text += "\n" + object_data_front.uuid_str.substr(0, 6); text_marker.text = existence_probability_text; - marker_array.markers.push_back(text_marker); // loop for each object_data in the group // boxed to tracker positions @@ -263,22 +261,26 @@ void TrackerObjectDebugger::draw( marker_track_boxes.scale.x = 0.4; marker_track_boxes.scale.y = 0.4; marker_track_boxes.scale.z = 0.4; + marker_track_boxes.color.a = 0.9; + marker_track_boxes.color.r = 1.0; + marker_track_boxes.color.g = 1.0; + marker_track_boxes.color.b = 1.0; // make detected object markers per channel std::vector marker_detect_boxes_per_channel; std::vector marker_detect_lines_per_channel; - for (size_t idx = 0; idx < channel_names_.size(); idx++) { + for (size_t idx = 0; idx < channels_config_.size(); idx++) { // get color - by channel index std_msgs::msg::ColorRGBA color; - color.a = 1.0; + color.a = 0.9; color.r = color_array[idx % PALETTE_SIZE][0]; color.g = color_array[idx % PALETTE_SIZE][1]; color.b = color_array[idx % PALETTE_SIZE][2]; visualization_msgs::msg::Marker marker_detect_boxes; marker_detect_boxes = marker; - marker_detect_boxes.ns = "detect_boxes_" + channel_names_[idx]; + marker_detect_boxes.ns = "detect_boxes_" + channels_config_[idx].short_name; marker_detect_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST; marker_detect_boxes.action = visualization_msgs::msg::Marker::ADD; marker_detect_boxes.scale.x = 0.2; @@ -289,7 +291,7 @@ void TrackerObjectDebugger::draw( visualization_msgs::msg::Marker marker_lines; marker_lines = marker; - marker_lines.ns = "association_lines_" + channel_names_[idx]; + marker_lines.ns = "association_lines_" + channels_config_[idx].short_name; marker_lines.type = visualization_msgs::msg::Marker::LINE_LIST; marker_lines.action = visualization_msgs::msg::Marker::ADD; marker_lines.scale.x = 0.15; @@ -298,8 +300,7 @@ void TrackerObjectDebugger::draw( marker_detect_lines_per_channel.push_back(marker_lines); } - bool is_any_associated = false; - + bool is_associated = false; for (const auto & object_data : object_data_group) { int channel_id = object_data.channel_id; @@ -312,7 +313,7 @@ void TrackerObjectDebugger::draw( // set association marker, if exists if (!object_data.is_associated) continue; - is_any_associated = true; + is_associated = true; // associated object box visualization_msgs::msg::Marker & marker_detect_boxes = @@ -337,26 +338,32 @@ void TrackerObjectDebugger::draw( } // add markers - marker_array.markers.push_back(marker_track_boxes); - if (is_any_associated) { - for (size_t i = 0; i < channel_names_.size(); i++) { - if (marker_detect_boxes_per_channel.at(i).points.empty()) continue; - marker_array.markers.push_back(marker_detect_boxes_per_channel.at(i)); - } - for (size_t i = 0; i < channel_names_.size(); i++) { - if (marker_detect_lines_per_channel.at(i).points.empty()) continue; - marker_array.markers.push_back(marker_detect_lines_per_channel.at(i)); - } - } else { - for (size_t i = 0; i < channel_names_.size(); i++) { + for (size_t i = 0; i < channels_config_.size(); i++) { + if (marker_detect_boxes_per_channel.at(i).points.empty()) { marker_detect_boxes_per_channel.at(i).action = visualization_msgs::msg::Marker::DELETE; - marker_array.markers.push_back(marker_detect_boxes_per_channel.at(i)); } - for (size_t i = 0; i < channel_names_.size(); i++) { + marker_array.markers.push_back(marker_detect_boxes_per_channel.at(i)); + } + for (size_t i = 0; i < channels_config_.size(); i++) { + if (marker_detect_lines_per_channel.at(i).points.empty()) { marker_detect_lines_per_channel.at(i).action = visualization_msgs::msg::Marker::DELETE; - marker_array.markers.push_back(marker_detect_lines_per_channel.at(i)); } + marker_array.markers.push_back(marker_detect_lines_per_channel.at(i)); } + + // if not associated, gray out the track box and text + if (!is_associated) { + marker_track_boxes.color.r = 0.5; + marker_track_boxes.color.g = 0.5; + marker_track_boxes.color.b = 0.5; + marker_track_boxes.color.a = 0.5; + text_marker.color.r = 0.5; + text_marker.color.g = 0.5; + text_marker.color.b = 0.5; + text_marker.color.a = 0.5; + } + marker_array.markers.push_back(text_marker); + marker_array.markers.push_back(marker_track_boxes); } return; @@ -378,18 +385,19 @@ void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & ma visualization_msgs::msg::Marker delete_marker; delete_marker.header.frame_id = frame_id_; delete_marker.header.stamp = message_time_; - delete_marker.ns = "existence_probability"; delete_marker.id = previous_id; delete_marker.action = visualization_msgs::msg::Marker::DELETE; + + delete_marker.ns = "existence_probability"; marker_array.markers.push_back(delete_marker); delete_marker.ns = "track_boxes"; marker_array.markers.push_back(delete_marker); - for (size_t idx = 0; idx < channel_names_.size(); idx++) { - delete_marker.ns = "detect_boxes_" + channel_names_[idx]; + for (size_t idx = 0; idx < channels_config_.size(); idx++) { + delete_marker.ns = "detect_boxes_" + channels_config_[idx].short_name; marker_array.markers.push_back(delete_marker); - delete_marker.ns = "association_lines_" + channel_names_[idx]; + delete_marker.ns = "association_lines_" + channels_config_[idx].short_name; marker_array.markers.push_back(delete_marker); } } diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp index fb4e9d2ded898..ec8edbee0fc8f 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp @@ -15,13 +15,14 @@ #ifndef DEBUGGER__DEBUG_OBJECT_HPP_ #define DEBUGGER__DEBUG_OBJECT_HPP_ +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" -#include "autoware_utils/ros/uuid_helper.hpp" +#include #include -#include "autoware_perception_msgs/msg/detected_objects.hpp" -#include "autoware_perception_msgs/msg/tracked_objects.hpp" +#include +#include #include #include #include @@ -46,7 +47,6 @@ struct ObjectData // object uuid boost::uuids::uuid uuid; - int uuid_int; std::string uuid_str; // association link, pair of coordinates @@ -65,27 +65,24 @@ struct ObjectData class TrackerObjectDebugger { public: - explicit TrackerObjectDebugger(const std::string & frame_id); + TrackerObjectDebugger( + const std::string & frame_id, const std::vector & channels_config); private: bool is_initialized_{false}; std::string frame_id_; + const std::vector channels_config_; + visualization_msgs::msg::MarkerArray markers_; - std::unordered_set current_ids_; - std::unordered_set previous_ids_; + std::unordered_set current_ids_{}; + std::unordered_set previous_ids_{}; rclcpp::Time message_time_; std::vector object_data_list_; std::list unused_marker_ids_; std::vector> object_data_groups_; - std::vector channel_names_; - public: - void setChannelNames(const std::vector & channel_names) - { - channel_names_ = channel_names; - } void collect( const rclcpp::Time & message_time, const std::list> & list_tracker, const types::DynamicObjectList & detected_objects, diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp index 4c1b330c0fa5c..368970dc22781 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp @@ -18,11 +18,14 @@ #include #include #include +#include namespace autoware::multi_object_tracker { -TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_id) -: node_(node), diagnostic_updater_(&node), object_debugger_(frame_id) +TrackerDebugger::TrackerDebugger( + rclcpp::Node & node, const std::string & frame_id, + const std::vector & channels_config) +: node_(node), diagnostic_updater_(&node), object_debugger_(frame_id, channels_config) { // declare debug parameters to decide whether to publish debug topics loadParameters(); @@ -200,6 +203,7 @@ void TrackerDebugger::publishObjectsMarkers() if (!debug_settings_.publish_debug_markers) return; visualization_msgs::msg::MarkerArray marker_message; + marker_message.markers.clear(); // process data object_debugger_.process(); diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp index 4b85b6bcb9c87..58505a147490f 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp @@ -15,16 +15,17 @@ #ifndef DEBUGGER__DEBUGGER_HPP_ #define DEBUGGER__DEBUGGER_HPP_ -#include "autoware_utils/ros/debug_publisher.hpp" -#include "autoware_utils/ros/published_time_publisher.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "debug_object.hpp" +#include +#include #include #include #include -#include "autoware_perception_msgs/msg/detected_objects.hpp" -#include "autoware_perception_msgs/msg/tracked_objects.hpp" +#include +#include #include #include @@ -43,7 +44,9 @@ namespace autoware::multi_object_tracker class TrackerDebugger { public: - explicit TrackerDebugger(rclcpp::Node & node, const std::string & frame_id); + TrackerDebugger( + rclcpp::Node & node, const std::string & frame_id, + const std::vector & channels_config); private: struct DEBUG_SETTINGS @@ -95,10 +98,6 @@ class TrackerDebugger void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat); // Debug object - void setObjectChannels(const std::vector & channels) - { - object_debugger_.setChannelNames(channels); - } void collectObjectInfo( const rclcpp::Time & message_time, const std::list> & list_tracker, const types::DynamicObjectList & detected_objects, diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 2dab26e67b5a2..5ea6cd1ff790f 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -17,6 +17,7 @@ #include "multi_object_tracker_node.hpp" #include "autoware/multi_object_tracker/object_model/shapes.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" #include @@ -71,53 +72,45 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) odometry_ = std::make_shared(*this, world_frame_id_, enable_odometry_uncertainty); // ROS interface - Input channels - // Get input channels - std::vector input_topic_names; - std::vector input_names_long; - std::vector input_names_short; - std::vector input_is_spawn_enabled; - + // Get input channels configuration if (selected_input_channels.empty()) { RCLCPP_ERROR(this->get_logger(), "No input topics are specified."); return; } + uint channel_index = 0; for (const auto & selected_input_channel : selected_input_channels) { + types::InputChannel input_channel_config; + input_channel_config.index = channel_index; + channel_index++; + // required parameters, no default value const std::string input_topic_name = declare_parameter("input_channels." + selected_input_channel + ".topic"); - input_topic_names.push_back(input_topic_name); + input_channel_config.input_topic = input_topic_name; // required parameter, but can set a default value - const bool spawn_enabled = declare_parameter( + input_channel_config.is_spawn_enabled = declare_parameter( "input_channels." + selected_input_channel + ".can_spawn_new_tracker", true); - input_is_spawn_enabled.push_back(spawn_enabled); // optional parameters const std::string default_name = selected_input_channel; const std::string name_long = declare_parameter( "input_channels." + selected_input_channel + ".optional.name", default_name); - input_names_long.push_back(name_long); + input_channel_config.long_name = name_long; const std::string default_name_short = selected_input_channel.substr(0, 3); const std::string name_short = declare_parameter( "input_channels." + selected_input_channel + ".optional.short_name", default_name_short); - input_names_short.push_back(name_short); - } - - input_channel_size_ = input_topic_names.size(); - input_channels_.resize(input_channel_size_); + input_channel_config.short_name = name_short; - for (size_t i = 0; i < input_channel_size_; i++) { - input_channels_[i].input_topic = input_topic_names[i]; - input_channels_[i].long_name = input_names_long[i]; - input_channels_[i].short_name = input_names_short[i]; - input_channels_[i].is_spawn_enabled = input_is_spawn_enabled[i]; + input_channels_config_.push_back(input_channel_config); } + input_channel_size_ = input_channels_config_.size(); // Initialize input manager input_manager_ = std::make_unique(*this, odometry_); - input_manager_->init(input_channels_); // Initialize input manager, set subscriptions + input_manager_->init(input_channels_config_); // Initialize input manager, set subscriptions input_manager_->setTriggerFunction( std::bind(&MultiObjectTracker::onTrigger, this)); // Set trigger function @@ -135,64 +128,68 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // Initialize processor { TrackerProcessorConfig config; - config.tracker_map.insert( - std::make_pair(Label::CAR, this->declare_parameter("car_tracker"))); - config.tracker_map.insert( - std::make_pair(Label::TRUCK, this->declare_parameter("truck_tracker"))); - config.tracker_map.insert( - std::make_pair(Label::BUS, this->declare_parameter("bus_tracker"))); - config.tracker_map.insert( - std::make_pair(Label::TRAILER, this->declare_parameter("trailer_tracker"))); - config.tracker_map.insert(std::make_pair( - Label::PEDESTRIAN, this->declare_parameter("pedestrian_tracker"))); - config.tracker_map.insert( - std::make_pair(Label::BICYCLE, this->declare_parameter("bicycle_tracker"))); - config.tracker_map.insert(std::make_pair( - Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); - config.channel_size = input_channel_size_; - - // Declare parameters - config.tracker_lifetime = declare_parameter("tracker_lifetime"); - config.min_known_object_removal_iou = declare_parameter("min_known_object_removal_iou"); - config.min_unknown_object_removal_iou = - declare_parameter("min_unknown_object_removal_iou"); - config.distance_threshold = declare_parameter("distance_threshold"); - - // Map from class name to label - std::map class_name_to_label = { - {"UNKNOWN", Label::UNKNOWN}, {"CAR", Label::CAR}, - {"TRUCK", Label::TRUCK}, {"BUS", Label::BUS}, - {"TRAILER", Label::TRAILER}, {"MOTORBIKE", Label::MOTORCYCLE}, - {"BICYCLE", Label::BICYCLE}, {"PEDESTRIAN", Label::PEDESTRIAN}}; - - // Declare parameters and initialize confident_count_threshold_map - for (const auto & [class_name, class_label] : class_name_to_label) { - int64_t value = declare_parameter("confident_count_threshold." + class_name); - config.confident_count_threshold[class_label] = static_cast(value); + { + config.tracker_map.insert( + std::make_pair(Label::CAR, this->declare_parameter("car_tracker"))); + config.tracker_map.insert( + std::make_pair(Label::TRUCK, this->declare_parameter("truck_tracker"))); + config.tracker_map.insert( + std::make_pair(Label::BUS, this->declare_parameter("bus_tracker"))); + config.tracker_map.insert( + std::make_pair(Label::TRAILER, this->declare_parameter("trailer_tracker"))); + config.tracker_map.insert(std::make_pair( + Label::PEDESTRIAN, this->declare_parameter("pedestrian_tracker"))); + config.tracker_map.insert( + std::make_pair(Label::BICYCLE, this->declare_parameter("bicycle_tracker"))); + config.tracker_map.insert(std::make_pair( + Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); + + // Declare parameters + config.tracker_lifetime = declare_parameter("tracker_lifetime"); + config.min_known_object_removal_iou = + declare_parameter("min_known_object_removal_iou"); + config.min_unknown_object_removal_iou = + declare_parameter("min_unknown_object_removal_iou"); + config.distance_threshold = declare_parameter("distance_threshold"); + + // Map from class name to label + std::map class_name_to_label = { + {"UNKNOWN", Label::UNKNOWN}, {"CAR", Label::CAR}, + {"TRUCK", Label::TRUCK}, {"BUS", Label::BUS}, + {"TRAILER", Label::TRAILER}, {"MOTORBIKE", Label::MOTORCYCLE}, + {"BICYCLE", Label::BICYCLE}, {"PEDESTRIAN", Label::PEDESTRIAN}}; + + // Declare parameters and initialize confident_count_threshold_map + for (const auto & [class_name, class_label] : class_name_to_label) { + int64_t value = declare_parameter("confident_count_threshold." + class_name); + config.confident_count_threshold[class_label] = static_cast(value); + } } - // Initialize processor with parameters - processor_ = std::make_unique(config); - } + AssociatorConfig associator_config; + { + const auto tmp = this->declare_parameter>("can_assign_matrix"); + const std::vector can_assign_matrix(tmp.begin(), tmp.end()); + associator_config.can_assign_matrix = can_assign_matrix; + associator_config.max_dist_matrix = + this->declare_parameter>("max_dist_matrix"); + associator_config.max_area_matrix = + this->declare_parameter>("max_area_matrix"); + associator_config.min_area_matrix = + this->declare_parameter>("min_area_matrix"); + associator_config.max_rad_matrix = + this->declare_parameter>("max_rad_matrix"); + associator_config.min_iou_matrix = + this->declare_parameter>("min_iou_matrix"); + } - // Data association initialization - { - const auto tmp = this->declare_parameter>("can_assign_matrix"); - const std::vector can_assign_matrix(tmp.begin(), tmp.end()); - const auto max_dist_matrix = this->declare_parameter>("max_dist_matrix"); - const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); - const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); - const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); - const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); - - association_ = std::make_unique( - can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, - min_iou_matrix); + // Initialize processor with parameters + processor_ = + std::make_unique(config, associator_config, input_channels_config_); } // Debugger - debugger_ = std::make_unique(*this, world_frame_id_); - debugger_->setObjectChannels(input_names_short); + debugger_ = std::make_unique(*this, world_frame_id_, input_channels_config_); published_time_publisher_ = std::make_unique(this); } @@ -257,23 +254,17 @@ void MultiObjectTracker::runProcess(const types::DynamicObjectList & detected_ob return; } - /* prediction */ + /* predict trackers to the measurement time */ processor_->predict(measurement_time); /* object association */ std::unordered_map direct_assignment, reverse_assignment; - { - const auto & list_tracker = processor_->getListTracker(); - // global nearest neighbor - Eigen::MatrixXd score_matrix = association_->calcScoreMatrix( - detected_objects, list_tracker); // row : tracker, col : measurement - association_->assign(score_matrix, direct_assignment, reverse_assignment); - - // Collect debug information - tracker list, existence probabilities, association results - debugger_->collectObjectInfo( - measurement_time, processor_->getListTracker(), detected_objects, direct_assignment, - reverse_assignment); - } + processor_->associate(detected_objects, direct_assignment, reverse_assignment); + + // Collect debug information - tracker list, existence probabilities, association results + debugger_->collectObjectInfo( + measurement_time, processor_->getListTracker(), detected_objects, direct_assignment, + reverse_assignment); /* tracker update */ processor_->update(detected_objects, *self_transform, direct_assignment); @@ -282,9 +273,7 @@ void MultiObjectTracker::runProcess(const types::DynamicObjectList & detected_ob processor_->prune(measurement_time); /* spawn new tracker */ - if (input_manager_->isChannelSpawnEnabled(detected_objects.channel_index)) { - processor_->spawn(detected_objects, reverse_assignment); - } + processor_->spawn(detected_objects, reverse_assignment); } void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time) diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index c967f8403a918..2099fce3b8395 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -19,7 +19,6 @@ #ifndef MULTI_OBJECT_TRACKER_NODE_HPP_ #define MULTI_OBJECT_TRACKER_NODE_HPP_ -#include "autoware/multi_object_tracker/association/association.hpp" #include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/odometry.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" @@ -65,8 +64,6 @@ class MultiObjectTracker : public rclcpp::Node private: // ROS interface rclcpp::Publisher::SharedPtr tracked_objects_pub_; - rclcpp::Subscription::SharedPtr - detected_object_sub_; // debugger std::unique_ptr debugger_; @@ -82,15 +79,14 @@ class MultiObjectTracker : public rclcpp::Node // internal states std::string world_frame_id_; // tracking frame - std::unique_ptr association_; std::unique_ptr processor_; // input manager std::unique_ptr input_manager_; std::shared_ptr odometry_; - std::vector input_channels_{}; size_t input_channel_size_{}; + std::vector input_channels_config_; // callback functions void onTimer(); diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index 1e0a340b19d55..4b3147cabd5e5 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -14,9 +14,12 @@ #include "input_manager.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" #include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" +#include + #include #include #include @@ -27,19 +30,11 @@ namespace autoware::multi_object_tracker /////////////////////////// /////// InputStream /////// /////////////////////////// -InputStream::InputStream(rclcpp::Node & node, uint & index, std::shared_ptr odometry) -: node_(node), index_(index), odometry_(odometry) -{ -} - -void InputStream::init(const InputChannel & input_channel) +InputStream::InputStream( + rclcpp::Node & node, const types::InputChannel & input_channel, + std::shared_ptr odometry) +: node_(node), channel_(input_channel), odometry_(odometry) { - // Initialize parameters - input_topic_ = input_channel.input_topic; - long_name_ = input_channel.long_name; - short_name_ = input_channel.short_name; - is_spawn_enabled_ = input_channel.is_spawn_enabled; - // Initialize queue objects_que_.clear(); @@ -58,7 +53,38 @@ void InputStream::onMessage( { const autoware_perception_msgs::msg::DetectedObjects & objects = *msg; - types::DynamicObjectList dynamic_objects = types::toDynamicObjectList(objects, index_); + types::DynamicObjectList dynamic_objects = types::toDynamicObjectList(objects, channel_.index); + + // object shape processing + for (auto & object : dynamic_objects.objects) { + const auto label = + autoware::object_recognition_utils::getHighestProbLabel(object.classification); + if (label == autoware_perception_msgs::msg::ObjectClassification::UNKNOWN) { + continue; + } + + // check object shape type, bounding box, cylinder, polygon + const auto object_type = object.shape.type; + if (object_type == autoware_perception_msgs::msg::Shape::POLYGON) { + // convert convex hull to bounding box + if (!shapes::convertConvexHullToBoundingBox(object, object)) { + RCLCPP_WARN( + node_.get_logger(), + "InputManager::onMessage %s: Failed to convert convex hull to bounding box.", + channel_.long_name.c_str()); + continue; + } + } else if (object_type == autoware_perception_msgs::msg::Shape::CYLINDER) { + // convert cylinder dimension to bounding box dimension + object.shape.dimensions.y = object.shape.dimensions.x; + } + // else, it is bounding box and nothing to do + + // calculate nearest point? + // calcAnchorPointOffset(object); + + // if object extension is not reliable, enlarge covariance of position and extend shape + } // Model the object uncertainty only if it is not available types::DynamicObjectList objects_with_uncertainty = @@ -69,7 +95,7 @@ void InputStream::onMessage( if (!transformed_objects) { RCLCPP_WARN( node_.get_logger(), "InputManager::onMessage %s: Failed to transform objects.", - long_name_.c_str()); + channel_.long_name.c_str()); return; } dynamic_objects = transformed_objects.value(); @@ -90,7 +116,7 @@ void InputStream::onMessage( // trigger the function if it is set if (func_trigger_) { - func_trigger_(index_); + func_trigger_(channel_.index); } } @@ -124,7 +150,7 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim node_.get_logger(), "InputManager::updateTimingStatus %s: Negative interval detected, now: %f, " "latest_message_time_: %f", - long_name_.c_str(), now.seconds(), latest_message_time_.seconds()); + channel_.long_name.c_str(), now.seconds(), latest_message_time_.seconds()); } else if (initial_count_ < INITIALIZATION_COUNT) { // Initialization constexpr double initial_gain = 0.5; @@ -151,7 +177,7 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim RCLCPP_WARN( node_.get_logger(), "InputManager::updateTimingStatus %s: Resetting the latest measurement time to %f", - long_name_.c_str(), objects_time.seconds()); + channel_.long_name.c_str(), objects_time.seconds()); } else { // Update only if the object time is newer than the latest measurement time latest_measurement_time_ = @@ -173,7 +199,7 @@ void InputStream::getObjectsOlderThan( node_.get_logger(), "InputManager::getObjectsOlderThan %s: Invalid object time interval, object_latest_time: %f, " "object_earliest_time: %f", - long_name_.c_str(), object_latest_time.seconds(), object_earliest_time.seconds()); + channel_.long_name.c_str(), object_latest_time.seconds(), object_earliest_time.seconds()); return; } @@ -210,7 +236,7 @@ InputManager::InputManager(rclcpp::Node & node, std::shared_ptr odomet latest_exported_object_time_ = node_.now() - rclcpp::Duration::from_seconds(3.0); } -void InputManager::init(const std::vector & input_channels) +void InputManager::init(const std::vector & input_channels) { // Check input sizes input_size_ = input_channels.size(); @@ -223,9 +249,7 @@ void InputManager::init(const std::vector & input_channels) sub_objects_array_.resize(input_size_); bool is_any_spawn_enabled = false; for (size_t i = 0; i < input_size_; i++) { - uint index(i); - InputStream input_stream(node_, index, odometry_); - input_stream.init(input_channels[i]); + InputStream input_stream(node_, input_channels[i], odometry_); input_stream.setTriggerFunction( std::bind(&InputManager::onTrigger, this, std::placeholders::_1)); input_streams_.push_back(std::make_shared(input_stream)); diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp index da715c39bb710..db69c646f7c11 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp @@ -32,20 +32,12 @@ namespace autoware::multi_object_tracker { using ObjectsList = std::vector; -struct InputChannel -{ - std::string input_topic; // topic name of the detection, e.g. "/detection/lidar" - 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 -}; - class InputStream { public: - explicit InputStream(rclcpp::Node & node, uint & index, std::shared_ptr odometry); - - void init(const InputChannel & input_channel); + InputStream( + rclcpp::Node & node, const types::InputChannel & input_channel, + std::shared_ptr odometry); void setTriggerFunction(std::function func_trigger) { @@ -56,11 +48,11 @@ class InputStream void updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time); bool isTimeInitialized() const { return initial_count_ > 0; } - uint getIndex() const { return index_; } + uint getIndex() const { return channel_.index; } void getObjectsOlderThan( const rclcpp::Time & object_latest_time, const rclcpp::Time & object_earliest_time, ObjectsList & objects_list); - bool isSpawnEnabled() const { return is_spawn_enabled_; } + bool isSpawnEnabled() const { return channel_.is_spawn_enabled; } void getTimeStatistics( double & latency_mean, double & latency_var, double & interval_mean, @@ -75,14 +67,9 @@ class InputStream private: rclcpp::Node & node_; - uint index_; + const types::InputChannel channel_; std::shared_ptr odometry_; - std::string input_topic_; - std::string long_name_; - std::string short_name_; - bool is_spawn_enabled_{}; - size_t que_size_{30}; std::deque objects_que_; @@ -102,7 +89,7 @@ class InputManager { public: InputManager(rclcpp::Node & node, std::shared_ptr odometry); - void init(const std::vector & input_channels); + void init(const std::vector & input_channels); void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } void onTrigger(const uint & index) const; diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index 02ad0767dc815..71109d8ec269f 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -28,6 +28,7 @@ #include #include #include +#include namespace autoware::multi_object_tracker { @@ -35,8 +36,12 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; using LabelType = autoware_perception_msgs::msg::ObjectClassification::_label_type; -TrackerProcessor::TrackerProcessor(const TrackerProcessorConfig & config) : config_(config) +TrackerProcessor::TrackerProcessor( + const TrackerProcessorConfig & config, const AssociatorConfig & associator_config, + const std::vector & channels_config) +: config_(config), channels_config_(channels_config) { + association_ = std::make_unique(associator_config); } void TrackerProcessor::predict(const rclcpp::Time & time) @@ -46,6 +51,18 @@ void TrackerProcessor::predict(const rclcpp::Time & time) } } +void TrackerProcessor::associate( + const types::DynamicObjectList & detected_objects, + std::unordered_map & direct_assignment, + std::unordered_map & reverse_assignment) const +{ + const auto & tracker_list = list_tracker_; + // global nearest neighbor + Eigen::MatrixXd score_matrix = association_->calcScoreMatrix( + detected_objects, tracker_list); // row : tracker, col : measurement + association_->assign(score_matrix, direct_assignment, reverse_assignment); +} + void TrackerProcessor::update( const types::DynamicObjectList & detected_objects, const geometry_msgs::msg::Transform & self_transform, @@ -55,11 +72,16 @@ void TrackerProcessor::update( const auto & time = detected_objects.header.stamp; for (auto tracker_itr = list_tracker_.begin(); tracker_itr != list_tracker_.end(); ++tracker_itr, ++tracker_idx) { - if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found + if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { + // found const auto & associated_object = detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); - (*(tracker_itr))->updateWithMeasurement(associated_object, time, self_transform); - } else { // not found + const types::InputChannel channel_info = channels_config_[associated_object.channel_index]; + (*(tracker_itr)) + ->updateWithMeasurement(associated_object, time, self_transform, channel_info); + + } else { + // not found (*(tracker_itr))->updateWithoutMeasurement(time); } } @@ -69,6 +91,13 @@ void TrackerProcessor::spawn( const types::DynamicObjectList & detected_objects, const std::unordered_map & reverse_assignment) { + const auto channel_config = channels_config_[detected_objects.channel_index]; + // If spawn is disabled, return + if (!channel_config.is_spawn_enabled) { + return; + } + + // Spawn new trackers for the objects that are not associated const auto & time = detected_objects.header.stamp; for (size_t i = 0; i < detected_objects.objects.size(); ++i) { if (reverse_assignment.find(i) != reverse_assignment.end()) { // found @@ -76,6 +105,12 @@ void TrackerProcessor::spawn( } const auto & new_object = detected_objects.objects.at(i); std::shared_ptr tracker = createNewTracker(new_object, time); + + // Initialize existence probabilities + tracker->initializeExistenceProbabilities( + new_object.channel_index, new_object.existence_probability); + + // Update the tracker with the new object if (tracker) list_tracker_.push_back(tracker); } } @@ -87,24 +122,20 @@ std::shared_ptr TrackerProcessor::createNewTracker( autoware::object_recognition_utils::getHighestProbLabel(object.classification); if (config_.tracker_map.count(label) != 0) { const auto tracker = config_.tracker_map.at(label); - if (tracker == "bicycle_tracker") - return std::make_shared(time, object, config_.channel_size); + if (tracker == "bicycle_tracker") return std::make_shared(time, object); if (tracker == "big_vehicle_tracker") - return std::make_shared( - object_model::big_vehicle, time, object, config_.channel_size); + return std::make_shared(object_model::big_vehicle, time, object); if (tracker == "multi_vehicle_tracker") - return std::make_shared(time, object, config_.channel_size); + return std::make_shared(time, object); if (tracker == "normal_vehicle_tracker") - return std::make_shared( - object_model::normal_vehicle, time, object, config_.channel_size); + return std::make_shared(object_model::normal_vehicle, time, object); if (tracker == "pass_through_tracker") - return std::make_shared(time, object, config_.channel_size); + return std::make_shared(time, object); if (tracker == "pedestrian_and_bicycle_tracker") - return std::make_shared(time, object, config_.channel_size); - if (tracker == "pedestrian_tracker") - return std::make_shared(time, object, config_.channel_size); + return std::make_shared(time, object); + if (tracker == "pedestrian_tracker") return std::make_shared(time, object); } - return std::make_shared(time, object, config_.channel_size); + return std::make_shared(time, object); } void TrackerProcessor::prune(const rclcpp::Time & time) @@ -144,10 +175,8 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) // Calculate the distance between the two objects const double distance = std::hypot( - object1.kinematics.pose_with_covariance.pose.position.x - - object2.kinematics.pose_with_covariance.pose.position.x, - object1.kinematics.pose_with_covariance.pose.position.y - - object2.kinematics.pose_with_covariance.pose.position.y); + object1.pose.position.x - object2.pose.position.x, + object1.pose.position.y - object2.pose.position.y); // If the distance is too large, skip if (distance > config_.distance_threshold) { diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp index ad296b1c07d8d..4c15f9c60cb83 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -15,6 +15,7 @@ #ifndef PROCESSOR__PROCESSOR_HPP_ #define PROCESSOR__PROCESSOR_HPP_ +#include "autoware/multi_object_tracker/association/association.hpp" #include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" @@ -37,7 +38,6 @@ using LabelType = autoware_perception_msgs::msg::ObjectClassification::_label_ty struct TrackerProcessorConfig { std::map tracker_map; - size_t channel_size; float tracker_lifetime; // [s] float min_known_object_removal_iou; // ratio [0, 1] float min_unknown_object_removal_iou; // ratio [0, 1] @@ -48,11 +48,17 @@ struct TrackerProcessorConfig class TrackerProcessor { public: - explicit TrackerProcessor(const TrackerProcessorConfig & config); + TrackerProcessor( + const TrackerProcessorConfig & config, const AssociatorConfig & associator_config, + const std::vector & channels_config); const std::list> & getListTracker() const { return list_tracker_; } // tracker processes void predict(const rclcpp::Time & time); + void associate( + const types::DynamicObjectList & detected_objects, + std::unordered_map & direct_assignment, + std::unordered_map & reverse_assignment) const; void update( const types::DynamicObjectList & detected_objects, const geometry_msgs::msg::Transform & self_transform, @@ -74,7 +80,11 @@ class TrackerProcessor void getExistenceProbabilities(std::vector> & existence_vectors) const; private: - TrackerProcessorConfig config_; + const TrackerProcessorConfig config_; + const std::vector & channels_config_; + + std::unique_ptr association_; + std::list> list_tracker_; void removeOldTracker(const rclcpp::Time & time); void removeOverlappedTracker(const rclcpp::Time & time); From c489770e64eb0e1d57e2a318747ca9dc13ac1dd4 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 28 Feb 2025 16:00:36 +0900 Subject: [PATCH 2/6] refactor(multi_object_tracker): use const reference in loop and simplify tracker update logic Signed-off-by: Taekjin LEE --- .../autoware_multi_object_tracker/src/debugger/debug_object.cpp | 2 +- .../autoware_multi_object_tracker/src/processor/processor.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp index 9500b15ef885a..757c862110bb6 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp @@ -140,7 +140,7 @@ void TrackerObjectDebugger::process() if (object_data_list_.empty()) return; // update uuid_int - for (auto & object_data : object_data_list_) { + for (const auto & object_data : object_data_list_) { current_ids_.insert(uuidToInt(object_data.uuid)); } diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index 71109d8ec269f..e60b795480484 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -111,7 +111,7 @@ void TrackerProcessor::spawn( new_object.channel_index, new_object.existence_probability); // Update the tracker with the new object - if (tracker) list_tracker_.push_back(tracker); + list_tracker_.push_back(tracker); } } From 6374effbf193fca01ef7b8c2f4a19cd4b445a7d9 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 28 Feb 2025 18:42:26 +0900 Subject: [PATCH 3/6] refactor(multi_object_tracker): update shape handling and streamline object tracking logic Signed-off-by: Taekjin LEE --- .../object_model/shapes.hpp | 13 +- .../object_model/types.hpp | 2 + .../lib/object_model/shapes.cpp | 123 ++++++------------ .../lib/tracker/model/vehicle_tracker.cpp | 24 ++-- 4 files changed, 58 insertions(+), 104 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp index 84c0eb912610a..299fab2709597 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp @@ -11,9 +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. -// -// -// Author: v1.0 Taekjin Lee #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ @@ -40,13 +37,13 @@ bool convertConvexHullToBoundingBox( 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); +Eigen::Vector2d getNearestCornerOrSurface( + const types::DynamicObject & object, 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); + const types::DynamicObject & this_object, const types::DynamicObject & input_object, + const Eigen::Vector2d anchor_vector, Eigen::Vector2d & tracking_offset, + types::DynamicObject & offset_object); } // namespace shapes } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp index 016051c454f05..6b930908a79ac 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -88,6 +89,7 @@ struct DynamicObject // object extension (size and shape) autoware_perception_msgs::msg::Shape shape; + geometry_msgs::msg::Point anchor_point; }; struct DynamicObjectList diff --git a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index 53ec4cf907cd7..520ec663b435c 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -11,9 +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. -// -// -// Author: v1.0 Yukihiro Saito, Taekjin Lee #include "autoware/multi_object_tracker/object_model/shapes.hpp" @@ -174,10 +171,15 @@ enum BBOX_IDX { * @param self_transform: Ego vehicle position in map frame * @return int index */ -int getNearestCornerOrSurface( - const double x, const double y, const double yaw, const double width, const double length, - const geometry_msgs::msg::Transform & self_transform) +Eigen::Vector2d getNearestCornerOrSurface( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) { + const double x = object.pose.position.x; + const double y = object.pose.position.y; + const double yaw = tf2::getYaw(object.pose.orientation); + const double width = object.shape.dimensions.y; + const double length = object.shape.dimensions.x; + // get local vehicle pose const double x0 = self_transform.translation.x; const double y0 = self_transform.translation.y; @@ -187,112 +189,65 @@ int getNearestCornerOrSurface( const double xl = std::cos(yaw) * (x0 - x) + std::sin(yaw) * (y0 - y); const double yl = -std::sin(yaw) * (x0 - x) + std::cos(yaw) * (y0 - y); - // Determine Index + // Determine anchor point // x+ (front) // __ // y+ | | y- // (left) | | (right) // -- // x- (rear) - int xgrid = 0; - int ygrid = 0; - const int labels[3][3] = { - {BBOX_IDX::FRONT_L_CORNER, BBOX_IDX::FRONT_SURFACE, BBOX_IDX::FRONT_R_CORNER}, - {BBOX_IDX::LEFT_SURFACE, BBOX_IDX::INSIDE, BBOX_IDX::RIGHT_SURFACE}, - {BBOX_IDX::REAR_L_CORNER, BBOX_IDX::REAR_SURFACE, BBOX_IDX::REAR_R_CORNER}}; + double anchor_x = 0; + double anchor_y = 0; if (xl > length / 2.0) { - xgrid = 0; // front + anchor_x = length / 2.0; } else if (xl > -length / 2.0) { - xgrid = 1; // middle + anchor_x = 0; } else { - xgrid = 2; // rear + anchor_x = -length / 2.0; } if (yl > width / 2.0) { - ygrid = 0; // left + anchor_y = width / 2.0; } else if (yl > -width / 2.0) { - ygrid = 1; // middle + anchor_y = 0; } else { - ygrid = 2; // right + anchor_y = -width / 2.0; } - return labels[xgrid][ygrid]; // 0 to 7 + 1(null) value -} - -/** - * @brief Calc bounding box center offset caused by shape change - * @param dw: width update [m] = w_new - w_old - * @param dl: length update [m] = l_new - l_old - * @param indx: nearest corner index - * @return 2d offset vector caused by shape change - */ -inline Eigen::Vector2d calcOffsetVectorFromShapeChange( - const double dw, const double dl, const int indx) -{ - Eigen::Vector2d offset; - // if surface - if (indx == BBOX_IDX::FRONT_SURFACE) { - offset(0, 0) = dl / 2.0; // move forward - offset(1, 0) = 0; - } else if (indx == BBOX_IDX::RIGHT_SURFACE) { - offset(0, 0) = 0; - offset(1, 0) = -dw / 2.0; // move right - } else if (indx == BBOX_IDX::REAR_SURFACE) { - offset(0, 0) = -dl / 2.0; // move backward - offset(1, 0) = 0; - } else if (indx == BBOX_IDX::LEFT_SURFACE) { - offset(0, 0) = 0; - offset(1, 0) = dw / 2.0; // move left - } - // if corner - if (indx == BBOX_IDX::FRONT_R_CORNER) { - offset(0, 0) = dl / 2.0; // move forward - offset(1, 0) = -dw / 2.0; // move right - } else if (indx == BBOX_IDX::REAR_R_CORNER) { - offset(0, 0) = -dl / 2.0; // move backward - offset(1, 0) = -dw / 2.0; // move right - } else if (indx == BBOX_IDX::REAR_L_CORNER) { - offset(0, 0) = -dl / 2.0; // move backward - offset(1, 0) = dw / 2.0; // move left - } else if (indx == BBOX_IDX::FRONT_L_CORNER) { - offset(0, 0) = dl / 2.0; // move forward - offset(1, 0) = dw / 2.0; // move left - } - return offset; // do nothing if indx == INVALID or INSIDE + return Eigen::Vector2d(anchor_x, anchor_y); } -/** - * @brief Convert input object center to tracking point based on nearest corner information - * 1. update anchor offset vector, 2. offset input bbox based on tracking_offset vector and - * prediction yaw angle - * @param w: last input bounding box width - * @param l: last input bounding box length - * @param indx: last input bounding box closest corner index - * @param input_object: input object bounding box - * @param yaw: current yaw estimation - * @param offset_object: output tracking measurement to feed ekf - * @return nearest corner index(int) - */ 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) + const types::DynamicObject & this_object, const types::DynamicObject & input_object, + const Eigen::Vector2d anchor_vector, Eigen::Vector2d & tracking_offset, + types::DynamicObject & offset_object) { // copy value offset_object = input_object; - // invalid index - if (indx == BBOX_IDX::INSIDE) { - return; // do nothing + // invalid anchor + if (anchor_vector.norm() < 1e-6) { + return; } + double input_yaw = tf2::getYaw(input_object.pose.orientation); // current object width and height - const double w_n = input_object.shape.dimensions.y; - const double l_n = input_object.shape.dimensions.x; + const double length = this_object.shape.dimensions.x; + const double width = this_object.shape.dimensions.y; // update offset - const Eigen::Vector2d offset = calcOffsetVectorFromShapeChange(w_n - w, l_n - l, indx); - tracking_offset = offset; + tracking_offset = anchor_vector; + if (tracking_offset.x() > 0) { + tracking_offset.x() -= length / 2.0; + } else if (anchor_vector.x() < 0) { + tracking_offset.x() += length / 2.0; + } + if (tracking_offset.y() > 0) { + tracking_offset.y() -= width / 2.0; + } else if (tracking_offset.y() < 0) { + tracking_offset.y() += width / 2.0; + } // offset input object - const Eigen::Matrix2d R = Eigen::Rotation2Dd(yaw).toRotationMatrix(); + const Eigen::Matrix2d R = Eigen::Rotation2Dd(input_yaw).toRotationMatrix(); const Eigen::Vector2d rotated_offset = R * tracking_offset; offset_object.pose.position.x += rotated_offset.x(); offset_object.pose.position.y += rotated_offset.y(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index 902fc62fac9cb..94a5274f1f925 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -133,7 +133,16 @@ VehicleTracker::VehicleTracker( bool VehicleTracker::predict(const rclcpp::Time & time) { - return motion_model_.predictState(time); + bool success = motion_model_.predictState(time); + if (!success) { + return false; + } + // // update object + // object_.pose.position.x = motion_model_.getStateElement(IDX::X); + // object_.pose.position.y = motion_.getStateElement(IDX::Y); + // object_.pose.orientation = tf2::toMsg( + // Eigen::AngleAxisd(motion_model_.getStateElement(IDX::YAW), Eigen::Vector3d::UnitZ())); + return true; } types::DynamicObject VehicleTracker::getUpdatingObject( @@ -141,18 +150,9 @@ types::DynamicObject VehicleTracker::getUpdatingObject( { types::DynamicObject updating_object = object; - // current (predicted) state - const double tracked_x = motion_model_.getStateElement(IDX::X); - const double tracked_y = motion_model_.getStateElement(IDX::Y); - const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); - // get offset measurement - const int nearest_corner_index = shapes::getNearestCornerOrSurface( - tracked_x, tracked_y, tracked_yaw, object_.shape.dimensions.y, object_.shape.dimensions.x, - self_transform); - shapes::calcAnchorPointOffset( - object_.shape.dimensions.y, object_.shape.dimensions.x, nearest_corner_index, object, - tracked_yaw, updating_object, tracking_offset_); + const Eigen::Vector2d anchor_vector = shapes::getNearestCornerOrSurface(object, self_transform); + shapes::calcAnchorPointOffset(object_, object, anchor_vector, tracking_offset_, updating_object); return updating_object; } From d3ce1dd10d9e64b706d821ed502f19271ae8f139 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 3 Mar 2025 20:42:27 +0900 Subject: [PATCH 4/6] refactor(multi_object_tracker): update shape handling to use geometry_msgs::msg::Point for anchor vectors Signed-off-by: Taekjin LEE --- .../multi_object_tracker/object_model/shapes.hpp | 4 ++-- .../tracker/model/tracker_base.hpp | 2 +- .../lib/object_model/shapes.cpp | 16 +++++++++------- .../lib/tracker/model/vehicle_tracker.cpp | 7 +------ 4 files changed, 13 insertions(+), 16 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp index 299fab2709597..82fc9abf23ce9 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp @@ -37,12 +37,12 @@ bool convertConvexHullToBoundingBox( bool getMeasurementYaw( const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw); -Eigen::Vector2d getNearestCornerOrSurface( +geometry_msgs::msg::Point getNearestCornerOrSurface( const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); void calcAnchorPointOffset( const types::DynamicObject & this_object, const types::DynamicObject & input_object, - const Eigen::Vector2d anchor_vector, Eigen::Vector2d & tracking_offset, + const geometry_msgs::msg::Point anchor_vector, Eigen::Vector2d & tracking_offset, types::DynamicObject & offset_object); } // namespace shapes } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index cca1c2a861816..1bd8086615d40 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -45,7 +45,7 @@ class Tracker int total_no_measurement_count_; int total_measurement_count_; rclcpp::Time last_update_with_measurement_time_; - std::vector existence_probabilities_; // remove if possible + std::vector existence_probabilities_; float total_existence_probability_; public: diff --git a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index 520ec663b435c..83a6ef2ce9442 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -171,7 +171,7 @@ enum BBOX_IDX { * @param self_transform: Ego vehicle position in map frame * @return int index */ -Eigen::Vector2d getNearestCornerOrSurface( +geometry_msgs::msg::Point getNearestCornerOrSurface( const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) { const double x = object.pose.position.x; @@ -212,19 +212,21 @@ Eigen::Vector2d getNearestCornerOrSurface( } else { anchor_y = -width / 2.0; } - - return Eigen::Vector2d(anchor_x, anchor_y); + geometry_msgs::msg::Point anchor_point; + anchor_point.x = anchor_x; + anchor_point.y = anchor_y; + return anchor_point; } void calcAnchorPointOffset( const types::DynamicObject & this_object, const types::DynamicObject & input_object, - const Eigen::Vector2d anchor_vector, Eigen::Vector2d & tracking_offset, + const geometry_msgs::msg::Point anchor_vector, Eigen::Vector2d & tracking_offset, types::DynamicObject & offset_object) { // copy value offset_object = input_object; // invalid anchor - if (anchor_vector.norm() < 1e-6) { + if (anchor_vector.x <= 1e-6 && anchor_vector.y <= 1e-6) { return; } double input_yaw = tf2::getYaw(input_object.pose.orientation); @@ -234,10 +236,10 @@ void calcAnchorPointOffset( const double width = this_object.shape.dimensions.y; // update offset - tracking_offset = anchor_vector; + tracking_offset = Eigen::Vector2d(anchor_vector.x, anchor_vector.y); if (tracking_offset.x() > 0) { tracking_offset.x() -= length / 2.0; - } else if (anchor_vector.x() < 0) { + } else if (tracking_offset.x() < 0) { tracking_offset.x() += length / 2.0; } if (tracking_offset.y() > 0) { diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index 94a5274f1f925..e7c36dea1f22c 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -137,11 +137,6 @@ bool VehicleTracker::predict(const rclcpp::Time & time) if (!success) { return false; } - // // update object - // object_.pose.position.x = motion_model_.getStateElement(IDX::X); - // object_.pose.position.y = motion_.getStateElement(IDX::Y); - // object_.pose.orientation = tf2::toMsg( - // Eigen::AngleAxisd(motion_model_.getStateElement(IDX::YAW), Eigen::Vector3d::UnitZ())); return true; } @@ -151,7 +146,7 @@ types::DynamicObject VehicleTracker::getUpdatingObject( types::DynamicObject updating_object = object; // get offset measurement - const Eigen::Vector2d anchor_vector = shapes::getNearestCornerOrSurface(object, self_transform); + const geometry_msgs::msg::Point anchor_vector = shapes::getNearestCornerOrSurface(object, self_transform); shapes::calcAnchorPointOffset(object_, object, anchor_vector, tracking_offset_, updating_object); return updating_object; From e0614ae6d3e419c548f150946a329bf98d2d13e8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 3 Mar 2025 11:46:00 +0000 Subject: [PATCH 5/6] style(pre-commit): autofix Signed-off-by: Taekjin LEE --- .../lib/tracker/model/vehicle_tracker.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index e7c36dea1f22c..8902725222882 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -146,7 +146,8 @@ types::DynamicObject VehicleTracker::getUpdatingObject( types::DynamicObject updating_object = object; // get offset measurement - const geometry_msgs::msg::Point anchor_vector = shapes::getNearestCornerOrSurface(object, self_transform); + const geometry_msgs::msg::Point anchor_vector = + shapes::getNearestCornerOrSurface(object, self_transform); shapes::calcAnchorPointOffset(object_, object, anchor_vector, tracking_offset_, updating_object); return updating_object; From 8ba56125812d701431ad7bf5720d93472e87fdb3 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 10 Mar 2025 12:00:07 +0900 Subject: [PATCH 6/6] refactor(multi_object_tracker): modify getNearestCornerOrSurface function signature and update related logic Signed-off-by: Taekjin LEE refactor(multi_object_tracker): remove self_transform parameter from measure and update methods refactor(multi_object_tracker): update calcAnchorPointOffset function signature and streamline object handling refactor(multi_object_tracker): set shape type to BOUNDING_BOX for object trackers --- .../object_model/shapes.hpp | 7 ++--- .../tracker/model/bicycle_tracker.hpp | 9 +----- .../model/multiple_vehicle_tracker.hpp | 4 +-- .../tracker/model/pass_through_tracker.hpp | 4 +-- .../model/pedestrian_and_bicycle_tracker.hpp | 4 +-- .../tracker/model/pedestrian_tracker.hpp | 9 +----- .../tracker/model/tracker_base.hpp | 6 ++-- .../tracker/model/unknown_tracker.hpp | 6 +--- .../tracker/model/vehicle_tracker.hpp | 8 +---- .../lib/object_model/shapes.cpp | 24 +++++++-------- .../lib/tracker/model/bicycle_tracker.cpp | 22 +++++--------- .../model/multiple_vehicle_tracker.cpp | 8 ++--- .../tracker/model/pass_through_tracker.cpp | 5 +--- .../model/pedestrian_and_bicycle_tracker.cpp | 7 ++--- .../lib/tracker/model/pedestrian_tracker.cpp | 27 +++++------------ .../lib/tracker/model/tracker_base.cpp | 4 +-- .../lib/tracker/model/unknown_tracker.cpp | 21 ++++---------- .../lib/tracker/model/vehicle_tracker.cpp | 29 +++++-------------- .../src/multi_object_tracker_node.cpp | 8 +---- .../src/processor/input_manager.cpp | 9 ++++-- .../src/processor/processor.cpp | 4 +-- .../src/processor/processor.hpp | 1 - 22 files changed, 69 insertions(+), 157 deletions(-) diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp index 82fc9abf23ce9..5e1de408862e1 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp @@ -37,12 +37,11 @@ bool convertConvexHullToBoundingBox( bool getMeasurementYaw( const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw); -geometry_msgs::msg::Point getNearestCornerOrSurface( - const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); +void getNearestCornerOrSurface( + const geometry_msgs::msg::Transform & self_transform, types::DynamicObject & object); void calcAnchorPointOffset( - const types::DynamicObject & this_object, const types::DynamicObject & input_object, - const geometry_msgs::msg::Point anchor_vector, Eigen::Vector2d & tracking_offset, + const types::DynamicObject & this_object, Eigen::Vector2d & tracking_offset, types::DynamicObject & offset_object); } // namespace shapes } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 2e4449aee529d..5bf5ed23da9e9 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -43,17 +43,10 @@ class BicycleTracker : public Tracker BicycleTracker(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, - const geometry_msgs::msg::Transform & self_transform) override; + bool measure(const types::DynamicObject & object, const rclcpp::Time & time) 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: - types::DynamicObject getUpdatingObject( - const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform) const; }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 80dbacc1d17d2..82122f34101cd 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -39,9 +39,7 @@ 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, - const geometry_msgs::msg::Transform & self_transform) override; + bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override; bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; virtual ~MultipleVehicleTracker() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index 938e704212b8d..d0dceabfaac25 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -37,9 +37,7 @@ 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, - const geometry_msgs::msg::Transform & self_transform) override; + bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override; bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index abd59557507b2..f0e741ddc1993 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -39,9 +39,7 @@ class PedestrianAndBicycleTracker : public Tracker 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, - const geometry_msgs::msg::Transform & self_transform) override; + bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override; bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; virtual ~PedestrianAndBicycleTracker() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index c39809b4d0a17..65495f8b5e108 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -44,17 +44,10 @@ 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, - const geometry_msgs::msg::Transform & self_transform) override; + bool measure(const types::DynamicObject & object, const rclcpp::Time & time) 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: - types::DynamicObject getUpdatingObject( - const types::DynamicObject & object, - const geometry_msgs::msg::Transform & self_transform) const; }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index 1bd8086615d40..637969c139cf1 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -61,7 +61,7 @@ class Tracker } bool updateWithMeasurement( const types::DynamicObject & object, const rclcpp::Time & measurement_time, - const geometry_msgs::msg::Transform & self_transform, const types::InputChannel & channel_info); + const types::InputChannel & channel_info); bool updateWithoutMeasurement(const rclcpp::Time & now); std::uint8_t getHighestProbLabel() const @@ -87,9 +87,7 @@ class Tracker void limitObjectExtension(const object_model::ObjectModel object_model); // virtual functions - virtual bool measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) = 0; + virtual bool measure(const types::DynamicObject & object, const rclcpp::Time & time) = 0; public: virtual bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const = 0; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index fdb24b8a3c1c8..5fb0a142ecf34 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -43,11 +43,7 @@ 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, - const geometry_msgs::msg::Transform & self_transform) override; - types::DynamicObject getUpdatingObject( - const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); + bool measure(const types::DynamicObject & object, const rclcpp::Time & time) override; bool measureWithPose(const types::DynamicObject & object); bool measureWithShape(const types::DynamicObject & object); bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index 1f7ba00a7c99d..4c682158a4aec 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -48,16 +48,10 @@ 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, - const geometry_msgs::msg::Transform & self_transform) override; + bool measure(const types::DynamicObject & object, const rclcpp::Time & time) 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: - types::DynamicObject getUpdatingObject( - const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index 83a6ef2ce9442..ee52e2a354d11 100644 --- a/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -171,8 +171,8 @@ enum BBOX_IDX { * @param self_transform: Ego vehicle position in map frame * @return int index */ -geometry_msgs::msg::Point getNearestCornerOrSurface( - const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) +void getNearestCornerOrSurface( + const geometry_msgs::msg::Transform & self_transform, types::DynamicObject & object) { const double x = object.pose.position.x; const double y = object.pose.position.y; @@ -212,24 +212,22 @@ geometry_msgs::msg::Point getNearestCornerOrSurface( } else { anchor_y = -width / 2.0; } - geometry_msgs::msg::Point anchor_point; - anchor_point.x = anchor_x; - anchor_point.y = anchor_y; - return anchor_point; + + object.anchor_point.x = anchor_x; + object.anchor_point.y = anchor_y; } void calcAnchorPointOffset( - const types::DynamicObject & this_object, const types::DynamicObject & input_object, - const geometry_msgs::msg::Point anchor_vector, Eigen::Vector2d & tracking_offset, - types::DynamicObject & offset_object) + const types::DynamicObject & this_object, Eigen::Vector2d & tracking_offset, + types::DynamicObject & updating_object) { // copy value - offset_object = input_object; + const geometry_msgs::msg::Point anchor_vector = updating_object.anchor_point; // invalid anchor if (anchor_vector.x <= 1e-6 && anchor_vector.y <= 1e-6) { return; } - double input_yaw = tf2::getYaw(input_object.pose.orientation); + double input_yaw = tf2::getYaw(updating_object.pose.orientation); // current object width and height const double length = this_object.shape.dimensions.x; @@ -251,8 +249,8 @@ void calcAnchorPointOffset( // offset input object const Eigen::Matrix2d R = Eigen::Rotation2Dd(input_yaw).toRotationMatrix(); const Eigen::Vector2d rotated_offset = R * tracking_offset; - offset_object.pose.position.x += rotated_offset.x(); - offset_object.pose.position.y += rotated_offset.y(); + updating_object.pose.position.x += rotated_offset.x(); + updating_object.pose.position.y += rotated_offset.y(); } } // namespace shapes diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 354335b4ec0c7..ad7112360267c 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -50,6 +50,8 @@ BicycleTracker::BicycleTracker(const rclcpp::Time & time, const types::DynamicOb object_extension.y = object_model_.init_size.width; object_extension.z = object_model_.init_size.height; } + object_.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + // set maximum and minimum size limitObjectExtension(object_model_); @@ -126,13 +128,6 @@ bool BicycleTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -types::DynamicObject BicycleTracker::getUpdatingObject( - const types::DynamicObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) const -{ - return object; -} - bool BicycleTracker::measureWithPose(const types::DynamicObject & object) { // get measurement yaw angle to update @@ -199,9 +194,7 @@ bool BicycleTracker::measureWithShape(const types::DynamicObject & object) return true; } -bool BicycleTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) +bool BicycleTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) { // check time gap const double dt = motion_model_.getDeltaTime(time); @@ -214,9 +207,8 @@ bool BicycleTracker::measure( } // update object - const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); - measureWithPose(updating_object); - measureWithShape(updating_object); + measureWithPose(object); + measureWithShape(object); return true; } @@ -225,12 +217,12 @@ bool BicycleTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; + + // predict from motion model auto & pose = object.pose; auto & pose_cov = object.pose_covariance; auto & twist = object.twist; auto & twist_cov = object.twist_covariance; - - // predict from motion model if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "BicycleTracker::getTrackedObject: Failed to get predicted state."); return false; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index b23ca7386e1f1..2803f65a84d29 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -37,12 +37,10 @@ bool MultipleVehicleTracker::predict(const rclcpp::Time & time) return true; } -bool MultipleVehicleTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) +bool MultipleVehicleTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) { - big_vehicle_tracker_.measure(object, time, self_transform); - normal_vehicle_tracker_.measure(object, time, self_transform); + big_vehicle_tracker_.measure(object, time); + normal_vehicle_tracker_.measure(object, time); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index 252641a810223..27500ee942d7f 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -52,9 +52,7 @@ bool PassThroughTracker::predict(const rclcpp::Time & time) return true; } -bool PassThroughTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) +bool PassThroughTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) { prev_observed_object_ = object_; object_ = object; @@ -68,7 +66,6 @@ bool PassThroughTracker::measure( } last_update_time_ = time; - (void)self_transform; // currently do not use self vehicle position return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 486cf5e90c1d8..0c28205cc2958 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -34,11 +34,10 @@ bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) } bool PedestrianAndBicycleTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) + const types::DynamicObject & object, const rclcpp::Time & time) { - pedestrian_tracker_.measure(object, time, self_transform); - bicycle_tracker_.measure(object, time, self_transform); + pedestrian_tracker_.measure(object, time); + bicycle_tracker_.measure(object, time); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index 3058d410120c2..6543591bda22d 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -116,13 +116,6 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -types::DynamicObject PedestrianTracker::getUpdatingObject( - const types::DynamicObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) const -{ - return object; -} - bool PedestrianTracker::measureWithPose(const types::DynamicObject & object) { // update motion model @@ -170,16 +163,14 @@ bool PedestrianTracker::measureWithShape(const types::DynamicObject & object) return false; } + // update shape type + object_.shape.type = object.shape.type; + return true; } -bool PedestrianTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) +bool PedestrianTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) { - // keep the latest input object - object_ = object; - // check time gap const double dt = motion_model_.getDeltaTime(time); if (0.01 /*10msec*/ < dt) { @@ -191,11 +182,9 @@ bool PedestrianTracker::measure( } // update object - const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); - measureWithPose(updating_object); - measureWithShape(updating_object); + measureWithPose(object); + measureWithShape(object); - (void)self_transform; // currently do not use self vehicle position return true; } @@ -203,12 +192,12 @@ bool PedestrianTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; + + // predict from motion model auto & pose = object.pose; auto & pose_cov = object.pose_covariance; auto & twist = object.twist; auto & twist_cov = object.twist_covariance; - - // predict from motion model if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "PedestrianTracker::getTrackedObject: Failed to get predicted state."); return false; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp index 0d0dca86a4136..a8a52e59cf079 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -79,7 +79,7 @@ void Tracker::initializeExistenceProbabilities( bool Tracker::updateWithMeasurement( const types::DynamicObject & object, const rclcpp::Time & measurement_time, - const geometry_msgs::msg::Transform & self_transform, const types::InputChannel & channel_info) + const types::InputChannel & channel_info) { // Update existence probability { @@ -120,7 +120,7 @@ bool Tracker::updateWithMeasurement( } // Update object - measure(object, measurement_time, self_transform); + measure(object, measurement_time); return true; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index cd27796557194..fec45f42bce3f 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -121,12 +121,6 @@ bool UnknownTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -types::DynamicObject UnknownTracker::getUpdatingObject( - const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) -{ - return object; -} - bool UnknownTracker::measureWithPose(const types::DynamicObject & object) { // update motion model @@ -146,12 +140,10 @@ bool UnknownTracker::measureWithPose(const types::DynamicObject & object) return is_updated; } -bool UnknownTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) +bool UnknownTracker::measure(const types::DynamicObject & object, const rclcpp::Time & time) { - // keep the latest input object - object_ = object; + // update object shape + object_.shape = object.shape; // check time gap const double dt = motion_model_.getDeltaTime(time); @@ -164,8 +156,7 @@ bool UnknownTracker::measure( } // update object - const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); - measureWithPose(updating_object); + measureWithPose(object); return true; } @@ -174,12 +165,12 @@ bool UnknownTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; + + // predict from motion model auto & pose = object.pose; auto & pose_cov = object.pose_covariance; auto & twist = object.twist; auto & twist_cov = object.twist_covariance; - - // predict from motion model if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "UnknownTracker::getTrackedObject: Failed to get predicted state."); return false; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index 8902725222882..9183ce3168ca5 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -60,6 +60,8 @@ VehicleTracker::VehicleTracker( object_extension.y = object_model_.init_size.width; object_extension.z = object_model_.init_size.height; } + object_.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + // set maximum and minimum size limitObjectExtension(object_model_); @@ -140,19 +142,6 @@ bool VehicleTracker::predict(const rclcpp::Time & time) return true; } -types::DynamicObject VehicleTracker::getUpdatingObject( - const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) -{ - types::DynamicObject updating_object = object; - - // get offset measurement - const geometry_msgs::msg::Point anchor_vector = - shapes::getNearestCornerOrSurface(object, self_transform); - shapes::calcAnchorPointOffset(object_, object, anchor_vector, tracking_offset_, updating_object); - - return updating_object; -} - bool VehicleTracker::measureWithPose(const types::DynamicObject & object) { // current (predicted) state @@ -240,13 +229,8 @@ bool VehicleTracker::measureWithShape(const types::DynamicObject & object) return true; } -bool VehicleTracker::measure( - const types::DynamicObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform) +bool VehicleTracker::measure(const types::DynamicObject & in_object, const rclcpp::Time & time) { - // keep the latest input object - object_ = object; - // check time gap const double dt = motion_model_.getDeltaTime(time); if (0.01 /*10msec*/ < dt) { @@ -258,7 +242,8 @@ bool VehicleTracker::measure( } // update object - const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); + types::DynamicObject updating_object = in_object; + shapes::calcAnchorPointOffset(object_, tracking_offset_, updating_object); measureWithPose(updating_object); measureWithShape(updating_object); @@ -269,12 +254,12 @@ bool VehicleTracker::getTrackedObject( const rclcpp::Time & time, types::DynamicObject & object) const { object = object_; + + // predict from motion model auto & pose = object.pose; auto & pose_cov = object.pose_covariance; auto & twist = object.twist; auto & twist_cov = object.twist_covariance; - - // predict from motion model if (!motion_model_.getPredictedState(time, pose, pose_cov, twist, twist_cov)) { RCLCPP_WARN(logger_, "VehicleTracker::getTrackedObject: Failed to get predicted state."); return false; diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 5ea6cd1ff790f..bf2c2de9b57e3 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -248,12 +248,6 @@ void MultiObjectTracker::runProcess(const types::DynamicObjectList & detected_ob const rclcpp::Time measurement_time = rclcpp::Time(detected_objects.header.stamp, this->now().get_clock_type()); - // Get the self transform - const auto self_transform = odometry_->getTransform(measurement_time); - if (!self_transform) { - return; - } - /* predict trackers to the measurement time */ processor_->predict(measurement_time); @@ -267,7 +261,7 @@ void MultiObjectTracker::runProcess(const types::DynamicObjectList & detected_ob reverse_assignment); /* tracker update */ - processor_->update(detected_objects, *self_transform, direct_assignment); + processor_->update(detected_objects, direct_assignment); /* tracker pruning */ processor_->prune(measurement_time); diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index 4b3147cabd5e5..045ff427dc823 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -52,6 +52,7 @@ void InputStream::onMessage( const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) { const autoware_perception_msgs::msg::DetectedObjects & objects = *msg; + const rclcpp::Time timestamp = objects.header.stamp; types::DynamicObjectList dynamic_objects = types::toDynamicObjectList(objects, channel_.index); @@ -80,8 +81,12 @@ void InputStream::onMessage( } // else, it is bounding box and nothing to do - // calculate nearest point? - // calcAnchorPointOffset(object); + // calculate nearest point + const auto self_transform = odometry_->getTransform(timestamp); + if (!self_transform) { + return; + } + shapes::getNearestCornerOrSurface(*self_transform, object); // if object extension is not reliable, enlarge covariance of position and extend shape } diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index e60b795480484..bbe09baff150c 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -65,7 +65,6 @@ void TrackerProcessor::associate( void TrackerProcessor::update( const types::DynamicObjectList & detected_objects, - const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & direct_assignment) { int tracker_idx = 0; @@ -77,8 +76,7 @@ void TrackerProcessor::update( const auto & associated_object = detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); const types::InputChannel channel_info = channels_config_[associated_object.channel_index]; - (*(tracker_itr)) - ->updateWithMeasurement(associated_object, time, self_transform, channel_info); + (*(tracker_itr))->updateWithMeasurement(associated_object, time, channel_info); } else { // not found diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp index 4c15f9c60cb83..9a86240487e9d 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -61,7 +61,6 @@ class TrackerProcessor std::unordered_map & reverse_assignment) const; void update( const types::DynamicObjectList & detected_objects, - const geometry_msgs::msg::Transform & self_transform, const std::unordered_map & direct_assignment); void spawn( const types::DynamicObjectList & detected_objects,