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 b19b0ba7ee10c..5842246bc1313 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 @@ -295,40 +295,43 @@ inline void calcAnchorPointOffset( * @param input_object: input convex hull objects * @param output_object: output bounding box objects */ -inline void convertConvexHullToBoundingBox( +inline bool convertConvexHullToBoundingBox( const autoware_auto_perception_msgs::msg::DetectedObject & input_object, autoware_auto_perception_msgs::msg::DetectedObject & output_object) { - 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::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + // check footprint size + if (input_object.shape.footprint.points.size() < 3) { + return false; + } + // look for bounding box boundary double max_x = 0; double max_y = 0; double min_x = 0; double min_y = 0; double max_z = 0; - - // look for bounding box boundary for (size_t i = 0; i < input_object.shape.footprint.points.size(); ++i) { - Eigen::Vector2d vertex{ - input_object.shape.footprint.points.at(i).x, input_object.shape.footprint.points.at(i).y}; - - const Eigen::Vector2d local_vertex = R_inv * (vertex - center); - max_x = std::max(max_x, local_vertex.x()); - max_y = std::max(max_y, local_vertex.y()); - min_x = std::min(min_x, local_vertex.x()); - min_y = std::min(min_y, local_vertex.y()); - - max_z = std::max(max_z, static_cast(input_object.shape.footprint.points.at(i).z)); + const double foot_x = input_object.shape.footprint.points.at(i).x; + const double foot_y = input_object.shape.footprint.points.at(i).y; + const double foot_z = input_object.shape.footprint.points.at(i).z; + max_x = std::max(max_x, foot_x); + max_y = std::max(max_y, foot_y); + min_x = std::min(min_x, foot_x); + min_y = std::min(min_y, foot_y); + max_z = std::max(max_z, foot_z); } // calc bounding box state const double length = max_x - min_x; const double width = max_y - min_y; const double height = max_z; + + // 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::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; @@ -341,6 +344,8 @@ inline void convertConvexHullToBoundingBox( output_object.shape.dimensions.x = length; output_object.shape.dimensions.y = width; output_object.shape.dimensions.z = height; + + return true; } inline bool getMeasurementYaw( diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 0698e4651b6bf..4e5f14fde6834 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -71,10 +71,12 @@ BicycleTracker::BicycleTracker( } else { bounding_box_ = {1.0, 0.5, 1.7}; } - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + // set maximum and minimum size + constexpr double max_size = 5.0; + constexpr double min_size = 0.3; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); // Set motion model parameters { @@ -172,7 +174,9 @@ autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingOb // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, updating_object); + if (!utils::convertConvexHullToBoundingBox(object, updating_object)) { + updating_object = object; + } } else { updating_object = object; } @@ -226,22 +230,38 @@ bool BicycleTracker::measureWithPose( bool BicycleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box - return true; + return false; } - constexpr double gain = 0.1; - constexpr double gain_inv = 1.0 - gain; + // check bound box size abnormality + constexpr double size_max = 30.0; // [m] + constexpr double size_min = 0.1; // [m] + if ( + bbox_object.shape.dimensions.x > size_max || bbox_object.shape.dimensions.y > size_max || + bbox_object.shape.dimensions.z > size_max) { + return false; + } else if ( + bbox_object.shape.dimensions.x < size_min || bbox_object.shape.dimensions.y < size_min || + bbox_object.shape.dimensions.z < size_min) { + return false; + } // 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; - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; + bounding_box_.length = gain_inv * bounding_box_.length + gain * bbox_object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * bbox_object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + gain * bbox_object.shape.dimensions.z; + + // set maximum and minimum size + constexpr double max_size = 5.0; + constexpr double min_size = 0.3; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); // update motion model motion_model_.updateExtendedState(bounding_box_.length); 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 e8ca8a47bbc78..15c6b2e722ed5 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 @@ -79,16 +79,24 @@ BigVehicleTracker::BigVehicleTracker( last_input_bounding_box_ = bounding_box_; } else { autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - utils::convertConvexHullToBoundingBox(object, bbox_object); - bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + RCLCPP_WARN( + logger_, + "BigVehicleTracker::BigVehicleTracker: Failed to convert convex hull to bounding box."); + bounding_box_ = {6.0, 2.0, 2.0}; // default value + } else { + bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, + bbox_object.shape.dimensions.z}; + } last_input_bounding_box_ = bounding_box_; } - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + // set maximum and minimum size + constexpr double max_size = 30.0; + constexpr double min_size = 1.0; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); // Set motion model parameters { @@ -195,7 +203,12 @@ autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatin // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); + if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + RCLCPP_WARN( + logger_, + "BigVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); + bbox_object = object; + } } else { bbox_object = object; } @@ -306,6 +319,20 @@ bool BigVehicleTracker::measureWithPose( bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + // do not update shape if the input is not a bounding box + return false; + } + + // check object size abnormality + constexpr double size_max = 40.0; // [m] + constexpr double size_min = 1.0; // [m] + if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) { + return false; + } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) { + return false; + } + constexpr double gain = 0.1; constexpr double gain_inv = 1.0 - gain; @@ -315,10 +342,13 @@ bool BigVehicleTracker::measureWithShape( 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 minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + + // set maximum and minimum size + constexpr double max_size = 30.0; + constexpr double min_size = 1.0; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); // update motion model motion_model_.updateExtendedState(bounding_box_.length); 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 54d6e84dd6656..bd04cc613ef12 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 @@ -79,16 +79,25 @@ NormalVehicleTracker::NormalVehicleTracker( last_input_bounding_box_ = bounding_box_; } else { autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - utils::convertConvexHullToBoundingBox(object, bbox_object); - bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + RCLCPP_WARN( + logger_, + "NormalVehicleTracker::NormalVehicleTracker: Failed to convert convex hull to bounding " + "box."); + bounding_box_ = {3.0, 2.0, 1.8}; // default value + } else { + bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, + bbox_object.shape.dimensions.z}; + } last_input_bounding_box_ = bounding_box_; } - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + // set maximum and minimum size + constexpr double max_size = 20.0; + constexpr double min_size = 1.0; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); // Set motion model parameters { @@ -195,7 +204,13 @@ autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpda // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); + if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + RCLCPP_WARN( + logger_, + "NormalVehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); + bbox_object = object; + } + } else { bbox_object = object; } @@ -306,6 +321,20 @@ bool NormalVehicleTracker::measureWithPose( bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + // do not update shape if the input is not a bounding box + return false; + } + + // check object size abnormality + constexpr double size_max = 30.0; // [m] + constexpr double size_min = 1.0; // [m] + if (object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max) { + return false; + } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min) { + return false; + } + constexpr double gain = 0.1; constexpr double gain_inv = 1.0 - gain; @@ -315,10 +344,13 @@ bool NormalVehicleTracker::measureWithShape( 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 minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); + + // set maximum and minimum size + constexpr double max_size = 20.0; + constexpr double min_size = 1.0; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); // update motion model motion_model_.updateExtendedState(bounding_box_.length); diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 0d4411b5266ad..fd9ef82dfdca3 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -66,19 +66,23 @@ PedestrianTracker::PedestrianTracker( // OBJECT SHAPE MODEL bounding_box_ = {0.5, 0.5, 1.7}; - cylinder_ = {0.3, 1.7}; + cylinder_ = {0.5, 1.7}; if (object.shape.type == autoware_auto_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_auto_perception_msgs::msg::Shape::CYLINDER) { cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + // do not update polygon shape } - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); - cylinder_.width = std::max(cylinder_.width, 0.3); - cylinder_.height = std::max(cylinder_.height, 0.3); + // set maximum and minimum size + constexpr double max_size = 5.0; + constexpr double min_size = 0.3; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); + cylinder_.width = std::min(std::max(cylinder_.width, min_size), max_size); + cylinder_.height = std::min(std::max(cylinder_.height, min_size), max_size); // Set motion model parameters { @@ -209,22 +213,46 @@ bool PedestrianTracker::measureWithShape( constexpr double gain_inv = 1.0 - gain; if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + // check bound box size abnormality + 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) { + return false; + } else if ( + object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min || + 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_auto_perception_msgs::msg::Shape::CYLINDER) { + // check cylinder size abnormality + constexpr double size_max = 30.0; // [m] + constexpr double size_min = 0.1; // [m] + if (object.shape.dimensions.x > size_max || object.shape.dimensions.z > size_max) { + return false; + } else if (object.shape.dimensions.x < size_min || object.shape.dimensions.z < size_min) { + return false; + } cylinder_.width = gain_inv * cylinder_.width + gain * object.shape.dimensions.x; cylinder_.height = gain_inv * cylinder_.height + gain * object.shape.dimensions.z; } else { + // do not update polygon shape return false; } - // set minimum size - bounding_box_.length = std::max(bounding_box_.length, 0.3); - bounding_box_.width = std::max(bounding_box_.width, 0.3); - bounding_box_.height = std::max(bounding_box_.height, 0.3); - cylinder_.width = std::max(cylinder_.width, 0.3); - cylinder_.height = std::max(cylinder_.height, 0.3); + // set maximum and minimum size + constexpr double max_size = 5.0; + constexpr double min_size = 0.3; + bounding_box_.length = std::min(std::max(bounding_box_.length, min_size), max_size); + bounding_box_.width = std::min(std::max(bounding_box_.width, min_size), max_size); + bounding_box_.height = std::min(std::max(bounding_box_.height, min_size), max_size); + cylinder_.width = std::min(std::max(cylinder_.width, min_size), max_size); + cylinder_.height = std::min(std::max(cylinder_.height, min_size), max_size); return true; }