diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index f284196e1c816..8eec03c5dc847 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -49,9 +49,7 @@ class BigVehicleTracker : public Tracker double height; }; BoundingBox bounding_box_; - BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; - int last_nearest_corner_index_; private: BicycleMotionModel motion_model_; @@ -77,16 +75,6 @@ class BigVehicleTracker : public Tracker const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const override; virtual ~BigVehicleTracker() {} - -private: - void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform) - { - Eigen::MatrixXd X_t(DIM, 1); - motion_model_.getStateVector(X_t); - last_nearest_corner_index_ = utils::getNearestCornerOrSurface( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, - self_transform); - } }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 4c9a5a113a340..107a3ef194afb 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -49,9 +49,7 @@ class NormalVehicleTracker : public Tracker double height; }; BoundingBox bounding_box_; - BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; - int last_nearest_corner_index_; private: BicycleMotionModel motion_model_; @@ -77,16 +75,6 @@ class NormalVehicleTracker : public Tracker const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const override; virtual ~NormalVehicleTracker() {} - -private: - void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform) - { - Eigen::MatrixXd X_t(DIM, 1); - motion_model_.getStateVector(X_t); - last_nearest_corner_index_ = utils::getNearestCornerOrSurface( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, - self_transform); - } }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index 2294462e05fea..259a58342c369 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -280,7 +280,7 @@ inline void calcAnchorPointOffset( // update offset const Eigen::Vector2d offset = calcOffsetVectorFromShapeChange(w_n - w, l_n - l, indx); - tracking_offset += offset; + tracking_offset = offset; // offset input object const Eigen::Matrix2d R = Eigen::Rotation2Dd(yaw).toRotationMatrix(); diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 5b7a52016967d..ebf5a977ed44a 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -44,7 +44,7 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; BigVehicleTracker::BigVehicleTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("BigVehicleTracker")), @@ -76,7 +76,6 @@ BigVehicleTracker::BigVehicleTracker( 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}; - last_input_bounding_box_ = bounding_box_; } else { autoware_perception_msgs::msg::DetectedObject bbox_object; if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { @@ -89,7 +88,6 @@ BigVehicleTracker::BigVehicleTracker( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; } - last_input_bounding_box_ = bounding_box_; } // set maximum and minimum size constexpr double max_size = 30.0; @@ -178,9 +176,6 @@ BigVehicleTracker::BigVehicleTracker( // initialize motion model motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } - - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step } bool BigVehicleTracker::predict(const rclcpp::Time & time) @@ -214,11 +209,11 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje } // get offset measurement - int nearest_corner_index = utils::getNearestCornerOrSurface( + const int nearest_corner_index = utils::getNearestCornerOrSurface( tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); utils::calcAnchorPointOffset( - last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, - bbox_object, tracked_yaw, updating_object, tracking_offset_); + bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, + updating_object, tracking_offset_); // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { @@ -332,15 +327,13 @@ bool BigVehicleTracker::measureWithShape( return false; } - constexpr double gain = 0.1; + constexpr double gain = 0.5; constexpr double gain_inv = 1.0 - gain; // update object 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; - last_input_bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; // set maximum and minimum size constexpr double max_size = 30.0; @@ -352,11 +345,19 @@ bool BigVehicleTracker::measureWithShape( // update motion model motion_model_.updateExtendedState(bounding_box_.length); - // // update offset into object position - // motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); - // // update offset - // tracking_offset_.x() = gain_inv * tracking_offset_.x(); - // tracking_offset_.y() = gain_inv * tracking_offset_.y(); + // update offset into object position + { + // rotate back the offset vector from object coordinate to global coordinate + const double yaw = motion_model_.getStateElement(IDX::YAW); + const double offset_x_global = + tracking_offset_.x() * std::cos(yaw) - tracking_offset_.y() * std::sin(yaw); + const double offset_y_global = + tracking_offset_.x() * std::sin(yaw) + tracking_offset_.y() * std::cos(yaw); + motion_model_.adjustPosition(-gain * offset_x_global, -gain * offset_y_global); + // update offset (object coordinate) + tracking_offset_.x() = gain_inv * tracking_offset_.x(); + tracking_offset_.y() = gain_inv * tracking_offset_.y(); + } return true; } @@ -390,9 +391,6 @@ bool BigVehicleTracker::measure( measureWithPose(updating_object); measureWithShape(updating_object); - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step - return true; } @@ -414,15 +412,6 @@ bool BigVehicleTracker::getTrackedObject( return false; } - // recover bounding box from tracking point - const double dl = bounding_box_.length - last_input_bounding_box_.length; - const double dw = bounding_box_.width - last_input_bounding_box_.width; - const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( - pose_with_cov.pose.position.x, pose_with_cov.pose.position.y, - motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); - pose_with_cov.pose.position.x = recovered_pose.x(); - pose_with_cov.pose.position.y = recovered_pose.y(); - // position pose_with_cov.pose.position.z = z_; diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index a4c080cb40eea..3b6f644614d9b 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -44,7 +44,7 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; NormalVehicleTracker::NormalVehicleTracker( const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, + const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, const uint & channel_index) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("NormalVehicleTracker")), @@ -76,7 +76,6 @@ NormalVehicleTracker::NormalVehicleTracker( 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}; - last_input_bounding_box_ = bounding_box_; } else { autoware_perception_msgs::msg::DetectedObject bbox_object; if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { @@ -90,7 +89,6 @@ NormalVehicleTracker::NormalVehicleTracker( bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; } - last_input_bounding_box_ = bounding_box_; } // set maximum and minimum size constexpr double max_size = 20.0; @@ -179,9 +177,6 @@ NormalVehicleTracker::NormalVehicleTracker( // initialize motion model motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } - - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step } bool NormalVehicleTracker::predict(const rclcpp::Time & time) @@ -216,11 +211,11 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO } // get offset measurement - int nearest_corner_index = utils::getNearestCornerOrSurface( + const int nearest_corner_index = utils::getNearestCornerOrSurface( tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); utils::calcAnchorPointOffset( - last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, - bbox_object, tracked_yaw, updating_object, tracking_offset_); + bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, + updating_object, tracking_offset_); // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { @@ -334,15 +329,13 @@ bool NormalVehicleTracker::measureWithShape( return false; } - constexpr double gain = 0.1; + constexpr double gain = 0.5; constexpr double gain_inv = 1.0 - gain; // update object 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; - last_input_bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; // set maximum and minimum size constexpr double max_size = 20.0; @@ -354,11 +347,19 @@ bool NormalVehicleTracker::measureWithShape( // update motion model motion_model_.updateExtendedState(bounding_box_.length); - // // update offset into object position - // motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); - // // update offset - // tracking_offset_.x() = gain_inv * tracking_offset_.x(); - // tracking_offset_.y() = gain_inv * tracking_offset_.y(); + // update offset into object position + { + // rotate back the offset vector from object coordinate to global coordinate + const double yaw = motion_model_.getStateElement(IDX::YAW); + const double offset_x_global = + tracking_offset_.x() * std::cos(yaw) - tracking_offset_.y() * std::sin(yaw); + const double offset_y_global = + tracking_offset_.x() * std::sin(yaw) + tracking_offset_.y() * std::cos(yaw); + motion_model_.adjustPosition(-gain * offset_x_global, -gain * offset_y_global); + // update offset (object coordinate) + tracking_offset_.x() = gain_inv * tracking_offset_.x(); + tracking_offset_.y() = gain_inv * tracking_offset_.y(); + } return true; } @@ -392,9 +393,6 @@ bool NormalVehicleTracker::measure( measureWithPose(updating_object); measureWithShape(updating_object); - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step - return true; } @@ -416,15 +414,6 @@ bool NormalVehicleTracker::getTrackedObject( return false; } - // recover bounding box from tracking point - const double dl = bounding_box_.length - last_input_bounding_box_.length; - const double dw = bounding_box_.width - last_input_bounding_box_.width; - const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( - pose_with_cov.pose.position.x, pose_with_cov.pose.position.y, - motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); - pose_with_cov.pose.position.x = recovered_pose.x(); - pose_with_cov.pose.position.y = recovered_pose.y(); - // position pose_with_cov.pose.position.z = z_;