diff --git a/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp index 17a385da7cb41..f40767274ebfd 100644 --- a/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp +++ b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/utils.hpp @@ -80,13 +80,6 @@ autoware_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( TrackedObject objectClassificationMerger( const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); -// probability merger -float probabilityMerger(const float main_prob, const float sub_prob, const MergePolicy policy); - -// shape merger -autoware_perception_msgs::msg::Shape shapeMerger( - const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); - // update tracked object void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj); diff --git a/perception/autoware_tracking_object_merger/src/utils/utils.cpp b/perception/autoware_tracking_object_merger/src/utils/utils.cpp index 3a24cc0f8321d..04d7f94f2886c 100644 --- a/perception/autoware_tracking_object_merger/src/utils/utils.cpp +++ b/perception/autoware_tracking_object_merger/src/utils/utils.cpp @@ -290,31 +290,6 @@ bool objectsHaveSameMotionDirections(const TrackedObject & main_obj, const Track } } -/** - * @brief compare two tracked objects yaw is reverted or not - * - * @param main_obj - * @param sub_obj - * @return true - * @return false - */ -bool objectsYawIsReverted(const TrackedObject & main_obj, const TrackedObject & sub_obj) -{ - // get yaw - const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation); - const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation); - // calc yaw diff - const auto yaw_diff = std::fabs(main_yaw - sub_yaw); - const auto normalized_yaw_diff = autoware::universe_utils::normalizeRadian(yaw_diff); // -pi ~ pi - // evaluate if yaw is reverted - constexpr double yaw_threshold = M_PI / 2.0; // 90 deg - if (std::abs(normalized_yaw_diff) >= yaw_threshold) { - return true; - } else { - return false; - } -} - // object kinematics merger // currently only support velocity fusion autoware_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( @@ -409,66 +384,6 @@ TrackedObject objectClassificationMerger( } } -// probability merger -float probabilityMerger(const float main_prob, const float sub_prob, const MergePolicy policy) -{ - if (policy == MergePolicy::SKIP) { - return main_prob; - } else if (policy == MergePolicy::OVERWRITE) { - return sub_prob; - } else if (policy == MergePolicy::FUSION) { - return static_cast(mean(main_prob, sub_prob)); - } else { - std::cerr << "unknown merge policy in probabilityMerger function." << std::endl; - return main_prob; - } -} - -// shape merger -autoware_perception_msgs::msg::Shape shapeMerger( - const autoware_perception_msgs::msg::Shape & main_obj_shape, - const autoware_perception_msgs::msg::Shape & sub_obj_shape, const MergePolicy policy) -{ - autoware_perception_msgs::msg::Shape output_shape; - // copy main object at first - output_shape = main_obj_shape; - - if (main_obj_shape.type != sub_obj_shape.type) { - // if shape type is different, return main object - return output_shape; - } - - if (policy == MergePolicy::SKIP) { - return output_shape; - } else if (policy == MergePolicy::OVERWRITE) { - return sub_obj_shape; - } else if (policy == MergePolicy::FUSION) { - // write down fusion method for each shape type - if (main_obj_shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - // if shape type is bounding box, merge bounding box - output_shape.dimensions.x = mean(main_obj_shape.dimensions.x, sub_obj_shape.dimensions.x); - output_shape.dimensions.y = mean(main_obj_shape.dimensions.y, sub_obj_shape.dimensions.y); - output_shape.dimensions.z = mean(main_obj_shape.dimensions.z, sub_obj_shape.dimensions.z); - return output_shape; - } else if (main_obj_shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { - // if shape type is cylinder, merge cylinder - // (TODO) implement - return output_shape; - } else if (main_obj_shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { - // if shape type is polygon, merge polygon - // (TODO) - return output_shape; - } else { - // when type is unknown, print warning and do nothing - std::cerr << "unknown shape type in shapeMerger function." << std::endl; - return output_shape; - } - } else { - std::cerr << "unknown merge policy in shapeMerger function." << std::endl; - return output_shape; - } -} - void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) { // do not update if motion direction is different