From 02a0e1943393f488ecc45f5e862b29efadf3d001 Mon Sep 17 00:00:00 2001 From: liuXinGangChina Date: Mon, 3 Mar 2025 15:01:30 +0800 Subject: [PATCH] feat(autoware_object_recognition_utils): move to core, autoware_object_recognition_utils, initial commit: v0.0 Signed-off-by: liuXinGangChina --- .../CMakeLists.txt | 26 ++ .../README.md | 49 +++ .../object_recognition_utils/conversion.hpp | 33 ++ .../object_recognition_utils/geometry.hpp | 57 ++++ .../object_recognition_utils/matching.hpp | 131 ++++++++ .../object_classification.hpp | 174 ++++++++++ .../object_recognition_utils.hpp | 25 ++ .../predicted_path_utils.hpp | 66 ++++ .../object_recognition_utils/transform.hpp | 152 +++++++++ .../package.xml | 34 ++ .../src/conversion.cpp | 71 ++++ .../src/predicted_path_utils.cpp | 133 ++++++++ .../test/src/test_conversion.cpp | 273 ++++++++++++++++ .../test/src/test_geometry.cpp | 111 +++++++ .../test/src/test_matching.cpp | 286 +++++++++++++++++ .../test/src/test_object_classification.cpp | 302 ++++++++++++++++++ .../test/src/test_predicted_path_utils.cpp | 298 +++++++++++++++++ 17 files changed, 2221 insertions(+) create mode 100644 common/autoware_object_recognition_utils/CMakeLists.txt create mode 100644 common/autoware_object_recognition_utils/README.md create mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/conversion.hpp create mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/geometry.hpp create mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp create mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_classification.hpp create mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_recognition_utils.hpp create mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp create mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/transform.hpp create mode 100644 common/autoware_object_recognition_utils/package.xml create mode 100644 common/autoware_object_recognition_utils/src/conversion.cpp create mode 100644 common/autoware_object_recognition_utils/src/predicted_path_utils.cpp create mode 100644 common/autoware_object_recognition_utils/test/src/test_conversion.cpp create mode 100644 common/autoware_object_recognition_utils/test/src/test_geometry.cpp create mode 100644 common/autoware_object_recognition_utils/test/src/test_matching.cpp create mode 100644 common/autoware_object_recognition_utils/test/src/test_object_classification.cpp create mode 100644 common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp diff --git a/common/autoware_object_recognition_utils/CMakeLists.txt b/common/autoware_object_recognition_utils/CMakeLists.txt new file mode 100644 index 0000000000..9e1276db8d --- /dev/null +++ b/common/autoware_object_recognition_utils/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_object_recognition_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Boost REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/predicted_path_utils.cpp + src/conversion.cpp +) + +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + + file(GLOB_RECURSE test_files test/**/*.cpp) + + ament_add_ros_isolated_gtest(test_object_recognition_utils ${test_files}) + + target_link_libraries(test_object_recognition_utils + ${PROJECT_NAME} + ) +endif() + +ament_auto_package() diff --git a/common/autoware_object_recognition_utils/README.md b/common/autoware_object_recognition_utils/README.md new file mode 100644 index 0000000000..634e53e000 --- /dev/null +++ b/common/autoware_object_recognition_utils/README.md @@ -0,0 +1,49 @@ +# autoware_object_recognition_utils + +## Overview + +This package contains a library of common functions that are useful across the perception module and planning module. + + +## Design + +### Conversion + +Ensuring accurate and efficient converting between DetectedObject and TrackedObject types. + +### Geometry + +It provides specialized implementations for each object type (e.g., DetectedObject, TrackedObject, and PredictedObject) to extract the pose information. + +### Matching + +It provides utility functions for calculating geometrical metrics, such as 2D IoU (Intersection over Union), GIoU (Generalized IoU), Precision, and Recall for objects. It also provides helper functions for computing areas of intersections, unions, and convex hulls of polygon + +### Object Classification + +Designed for processing and classifying detected objects, it implements the following functionalities: + +* Handling of vehicle category checks +* Conversion between string class names and numerical labels +* Probability-based classification selection +* String representation of object labels + +### Predicted Path Utils + +Provideing utility functions for handling predicted paths of objects. It includes the following functionalities: + +* calcInterpolatedPose: Calculates an interpolated pose from a predicted path based on a given time. +* resamplePredictedPath (version 1): Resamples a predicted path according to a specified time vector, optionally using spline interpolation for smoother results. +* resamplePredictedPath (version 2): Resamples a predicted path at regular time intervals, including the terminal point, with optional spline interpolation. + +## Usage + +include all-in-one header files if multiple functionalities are needed: +```cpp +#include +``` + +include specific header files if only a subset of functionalities is needed: +```cpp +#include +``` \ No newline at end of file diff --git a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/conversion.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/conversion.hpp new file mode 100644 index 0000000000..5b3be48cc2 --- /dev/null +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/conversion.hpp @@ -0,0 +1,33 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ + +#include +#include + +namespace autoware::object_recognition_utils +{ +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; + +DetectedObject toDetectedObject(const TrackedObject & tracked_object); +DetectedObjects toDetectedObjects(const TrackedObjects & tracked_objects); +TrackedObject toTrackedObject(const DetectedObject & detected_object); +} // namespace autoware::object_recognition_utils + +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ diff --git a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/geometry.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/geometry.hpp new file mode 100644 index 0000000000..a5acc210d9 --- /dev/null +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/geometry.hpp @@ -0,0 +1,57 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ + +#include +#include +#include +#include + +namespace autoware::object_recognition_utils +{ +template +geometry_msgs::msg::Pose getPose([[maybe_unused]] const T & p) +{ + static_assert(sizeof(T) == 0, "Only specializations of getPose can be used."); + throw std::logic_error("Only specializations of getPose can be used."); +} + +template <> +inline geometry_msgs::msg::Pose getPose(const geometry_msgs::msg::Pose & p) +{ + return p; +} + +template <> +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::DetectedObject & obj) +{ + return obj.kinematics.pose_with_covariance.pose; +} + +template <> +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::TrackedObject & obj) +{ + return obj.kinematics.pose_with_covariance.pose; +} + +template <> +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::PredictedObject & obj) +{ + return obj.kinematics.initial_pose_with_covariance.pose; +} +} // namespace autoware::object_recognition_utils + +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ diff --git a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp new file mode 100644 index 0000000000..afeb56c1e6 --- /dev/null +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp @@ -0,0 +1,131 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ + +#include "autoware/object_recognition_utils/geometry.hpp" +#include "autoware_utils/geometry/boost_geometry.hpp" +#include "autoware_utils/geometry/boost_polygon_utils.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::object_recognition_utils +{ +using autoware_utils::Polygon2d; +// minimum area to avoid division by zero +static const double MIN_AREA = 1e-6; + +inline double getConvexShapeArea(const Polygon2d & source_polygon, const Polygon2d & target_polygon) +{ + boost::geometry::model::multi_polygon union_polygons; + boost::geometry::union_(source_polygon, target_polygon, union_polygons); + + autoware_utils::Polygon2d hull; + boost::geometry::convex_hull(union_polygons, hull); + return boost::geometry::area(hull); +} + +inline double getSumArea(const std::vector & polygons) +{ + return std::accumulate(polygons.begin(), polygons.end(), 0.0, [](double acc, Polygon2d p) { + return acc + boost::geometry::area(p); + }); +} + +inline double getIntersectionArea( + const Polygon2d & source_polygon, const Polygon2d & target_polygon) +{ + std::vector intersection_polygons; + boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons); + return getSumArea(intersection_polygons); +} + +inline double getUnionArea(const Polygon2d & source_polygon, const Polygon2d & target_polygon) +{ + std::vector union_polygons; + boost::geometry::union_(source_polygon, target_polygon, union_polygons); + return getSumArea(union_polygons); +} + +template +double get2dIoU(const T1 source_object, const T2 target_object, const double min_union_area = 0.01) +{ + const auto source_polygon = autoware_utils::to_polygon2d(source_object); + if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; + const auto target_polygon = autoware_utils::to_polygon2d(target_object); + if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; + + const double intersection_area = getIntersectionArea(source_polygon, target_polygon); + if (intersection_area < MIN_AREA) return 0.0; + const double union_area = getUnionArea(source_polygon, target_polygon); + + const double iou = + union_area < min_union_area ? 0.0 : std::min(1.0, intersection_area / union_area); + return iou; +} + +template +double get2dGeneralizedIoU(const T1 & source_object, const T2 & target_object) +{ + const auto source_polygon = autoware_utils::to_polygon2d(source_object); + if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; + const auto target_polygon = autoware_utils::to_polygon2d(target_object); + if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; + + const double intersection_area = getIntersectionArea(source_polygon, target_polygon); + const double union_area = getUnionArea(source_polygon, target_polygon); + const double convex_shape_area = getConvexShapeArea(source_polygon, target_polygon); + + const double iou = union_area < 0.01 ? 0.0 : std::min(1.0, intersection_area / union_area); + return iou - (convex_shape_area - union_area) / convex_shape_area; +} + +template +double get2dPrecision(const T1 source_object, const T2 target_object) +{ + const auto source_polygon = autoware_utils::to_polygon2d(source_object); + const double source_area = boost::geometry::area(source_polygon); + if (source_area < MIN_AREA) return 0.0; + const auto target_polygon = autoware_utils::to_polygon2d(target_object); + if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; + + const double intersection_area = getIntersectionArea(source_polygon, target_polygon); + if (intersection_area < MIN_AREA) return 0.0; + + return std::min(1.0, intersection_area / source_area); +} + +template +double get2dRecall(const T1 source_object, const T2 target_object) +{ + const auto source_polygon = autoware_utils::to_polygon2d(source_object); + if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; + const auto target_polygon = autoware_utils::to_polygon2d(target_object); + const double target_area = boost::geometry::area(target_polygon); + if (target_area < MIN_AREA) return 0.0; + + const double intersection_area = getIntersectionArea(source_polygon, target_polygon); + if (intersection_area < MIN_AREA) return 0.0; + + return std::min(1.0, intersection_area / target_area); +} +} // namespace autoware::object_recognition_utils + +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__MATCHING_HPP_ diff --git a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_classification.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_classification.hpp new file mode 100644 index 0000000000..432abcbe10 --- /dev/null +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_classification.hpp @@ -0,0 +1,174 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ + +#include "autoware_perception_msgs/msg/object_classification.hpp" + +#include +#include + +namespace autoware::object_recognition_utils +{ +using autoware_perception_msgs::msg::ObjectClassification; + +inline ObjectClassification getHighestProbClassification( + const std::vector & object_classifications) +{ + if (object_classifications.empty()) { + return ObjectClassification{}; + } + return *std::max_element( + std::begin(object_classifications), std::end(object_classifications), + [](const auto & a, const auto & b) { return a.probability < b.probability; }); +} + +inline std::uint8_t getHighestProbLabel( + const std::vector & object_classifications) +{ + auto classification = getHighestProbClassification(object_classifications); + return classification.label; +} + +inline bool isVehicle(const uint8_t label) +{ + return label == ObjectClassification::BICYCLE || label == ObjectClassification::BUS || + label == ObjectClassification::CAR || label == ObjectClassification::MOTORCYCLE || + label == ObjectClassification::TRAILER || label == ObjectClassification::TRUCK; +} + +inline bool isVehicle(const ObjectClassification & object_classification) +{ + return isVehicle(object_classification.label); +} + +inline bool isVehicle(const std::vector & object_classifications) +{ + auto highest_prob_label = getHighestProbLabel(object_classifications); + return isVehicle(highest_prob_label); +} + +inline bool isCarLikeVehicle(const uint8_t label) +{ + return label == ObjectClassification::BUS || label == ObjectClassification::CAR || + label == ObjectClassification::TRAILER || label == ObjectClassification::TRUCK; +} + +inline bool isCarLikeVehicle(const ObjectClassification & object_classification) +{ + return isCarLikeVehicle(object_classification.label); +} + +inline bool isCarLikeVehicle(const std::vector & object_classifications) +{ + auto highest_prob_label = getHighestProbLabel(object_classifications); + return isCarLikeVehicle(highest_prob_label); +} + +inline bool isLargeVehicle(const uint8_t label) +{ + return label == ObjectClassification::BUS || label == ObjectClassification::TRAILER || + label == ObjectClassification::TRUCK; +} + +inline bool isLargeVehicle(const ObjectClassification & object_classification) +{ + return isLargeVehicle(object_classification.label); +} + +inline bool isLargeVehicle(const std::vector & object_classifications) +{ + auto highest_prob_label = getHighestProbLabel(object_classifications); + return isLargeVehicle(highest_prob_label); +} + +inline uint8_t toLabel(const std::string & class_name) +{ + if (class_name == "UNKNOWN") { + return ObjectClassification::UNKNOWN; + } else if (class_name == "CAR") { + return ObjectClassification::CAR; + } else if (class_name == "TRUCK") { + return ObjectClassification::TRUCK; + } else if (class_name == "BUS") { + return ObjectClassification::BUS; + } else if (class_name == "TRAILER") { + return ObjectClassification::TRAILER; + } else if (class_name == "MOTORCYCLE") { + return ObjectClassification::MOTORCYCLE; + } else if (class_name == "BICYCLE") { + return ObjectClassification::BICYCLE; + } else if (class_name == "PEDESTRIAN") { + return ObjectClassification::PEDESTRIAN; + } else { + throw std::runtime_error("Invalid Classification label."); + } +} + +inline ObjectClassification toObjectClassification( + const std::string & class_name, float probability) +{ + ObjectClassification classification; + classification.label = toLabel(class_name); + classification.probability = probability; + return classification; +} + +inline std::vector toObjectClassifications( + const std::string & class_name, float probability) +{ + std::vector classifications; + classifications.push_back(toObjectClassification(class_name, probability)); + return classifications; +} + +inline std::string convertLabelToString(const uint8_t label) +{ + if (label == ObjectClassification::UNKNOWN) { + return "UNKNOWN"; + } else if (label == ObjectClassification::CAR) { + return "CAR"; + } else if (label == ObjectClassification::TRUCK) { + return "TRUCK"; + } else if (label == ObjectClassification::BUS) { + return "BUS"; + } else if (label == ObjectClassification::TRAILER) { + return "TRAILER"; + } else if (label == ObjectClassification::MOTORCYCLE) { + return "MOTORCYCLE"; + } else if (label == ObjectClassification::BICYCLE) { + return "BICYCLE"; + } else if (label == ObjectClassification::PEDESTRIAN) { + return "PEDESTRIAN"; + } else { + return "UNKNOWN"; + } +} + +inline std::string convertLabelToString(const ObjectClassification object_classification) +{ + return convertLabelToString(object_classification.label); +} + +inline std::string convertLabelToString( + const std::vector object_classifications) +{ + auto highest_prob_label = getHighestProbLabel(object_classifications); + return convertLabelToString(highest_prob_label); +} + +} // namespace autoware::object_recognition_utils + +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ diff --git a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_recognition_utils.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_recognition_utils.hpp new file mode 100644 index 0000000000..d73e2e4dc6 --- /dev/null +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_recognition_utils.hpp @@ -0,0 +1,25 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ + +#include "autoware/object_recognition_utils/conversion.hpp" +#include "autoware/object_recognition_utils/geometry.hpp" +#include "autoware/object_recognition_utils/matching.hpp" +#include "autoware/object_recognition_utils/object_classification.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/object_recognition_utils/transform.hpp" + +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__OBJECT_RECOGNITION_UTILS_HPP_ diff --git a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp new file mode 100644 index 0000000000..46a5676da7 --- /dev/null +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp @@ -0,0 +1,66 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ + +#include "autoware_utils/geometry/geometry.hpp" + +#include +#include + +#include + +#include + +namespace autoware::object_recognition_utils +{ +/** + * @brief Calculate Interpolated Pose from predicted path by time + * @param path Input predicted path + * @param relative_time time at interpolated point. This should be within [0.0, + * time_step*(num_of_path_points)] + * @return interpolated pose + */ +boost::optional calcInterpolatedPose( + const autoware_perception_msgs::msg::PredictedPath & path, const double relative_time); + +/** + * @brief Resampling predicted path by time step vector. Note this function does not substitute + * original time step + * @param path Input predicted path + * @param resampled_time resampled time at each resampling point. Each time should be within [0.0, + * time_step*(num_of_path_points)] + * @return resampled path + */ +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, + const std::vector & resampled_time, const bool use_spline_for_xy = true, + const bool use_spline_for_z = false); + +/** + * @brief Resampling predicted path by sampling time interval. Note that this function samples + * terminal point as well as points by sampling time interval + * @param path Input predicted path + * @param sampling_time_interval sampling time interval for each point + * @param sampling_horizon sampling time horizon + * @return resampled path + */ +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, + const double sampling_horizon, const bool use_spline_for_xy = true, + const bool use_spline_for_z = false); +} // namespace autoware::object_recognition_utils + +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ diff --git a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/transform.hpp b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/transform.hpp new file mode 100644 index 0000000000..ced5956e07 --- /dev/null +++ b/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/transform.hpp @@ -0,0 +1,152 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ +#define AUTOWARE__OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ + +#include + +#include +#include +#include + +#include + +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +#include + +namespace detail +{ +[[maybe_unused]] inline boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return self_transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("object_recognition_utils"), ex.what()); + return boost::none; + } +} + +[[maybe_unused]] inline boost::optional getTransformMatrix( + const std::string & in_target_frame, const std_msgs::msg::Header & in_cloud_header, + const tf2_ros::Buffer & tf_buffer) +{ + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer.lookupTransform( + in_target_frame, in_cloud_header.frame_id, in_cloud_header.stamp, + rclcpp::Duration::from_seconds(1.0)); + Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + return mat; + } catch (tf2::TransformException & e) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("detail::getTransformMatrix"), e.what()); + return boost::none; + } +} +} // namespace detail + +namespace autoware::object_recognition_utils +{ +template +bool transformObjects( + const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + T & output_msg) +{ + output_msg = input_msg; + + // transform to world coordinate + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + { + const auto ros_target2objects_world = detail::getTransform( + tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); + } + for (auto & object : output_msg.objects) { + auto & pose_with_cov = object.kinematics.pose_with_covariance; + tf2::fromMsg(pose_with_cov.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); + // transform covariance, only the frame difference + pose_with_cov.covariance = + tf2::transformCovariance(pose_with_cov.covariance, tf_target2objects_world); + } + } + return true; +} +template +bool transformObjectsWithFeature( + const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + T & output_msg) +{ + output_msg = input_msg; + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + const auto ros_target2objects_world = detail::getTransform( + tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + const auto tf_matrix = detail::getTransformMatrix(target_frame_id, input_msg.header, tf_buffer); + if (!tf_matrix) { + RCLCPP_WARN( + rclcpp::get_logger("object_recognition_utils:"), "failed to get transformed matrix"); + return false; + } + for (auto & feature_object : output_msg.feature_objects) { + // transform object + tf2::fromMsg( + feature_object.object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + tf2::toMsg(tf_target2objects, feature_object.object.kinematics.pose_with_covariance.pose); + + // transform cluster + sensor_msgs::msg::PointCloud2 transformed_cluster; + pcl_ros::transformPointCloud(*tf_matrix, feature_object.feature.cluster, transformed_cluster); + transformed_cluster.header.frame_id = target_frame_id; + feature_object.feature.cluster = transformed_cluster; + } + output_msg.header.frame_id = target_frame_id; + return true; + } + return true; +} +} // namespace autoware::object_recognition_utils + +#endif // AUTOWARE__OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ diff --git a/common/autoware_object_recognition_utils/package.xml b/common/autoware_object_recognition_utils/package.xml new file mode 100644 index 0000000000..258ecf56c0 --- /dev/null +++ b/common/autoware_object_recognition_utils/package.xml @@ -0,0 +1,34 @@ + + + + autoware_object_recognition_utils + 0.0.0 + The autoware_object_recognition_utils package + Takayuki Murooka + Yoshi Ri + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_interpolation + autoware_perception_msgs + autoware_utils + geometry_msgs + libboost-dev + pcl_conversions + pcl_ros + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/autoware_object_recognition_utils/src/conversion.cpp b/common/autoware_object_recognition_utils/src/conversion.cpp new file mode 100644 index 0000000000..6f8e6aa27c --- /dev/null +++ b/common/autoware_object_recognition_utils/src/conversion.cpp @@ -0,0 +1,71 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/object_recognition_utils/conversion.hpp" + +namespace autoware::object_recognition_utils +{ +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; + +DetectedObject toDetectedObject(const TrackedObject & tracked_object) +{ + DetectedObject detected_object; + detected_object.existence_probability = tracked_object.existence_probability; + + detected_object.classification = tracked_object.classification; + + detected_object.kinematics.pose_with_covariance = tracked_object.kinematics.pose_with_covariance; + detected_object.kinematics.has_position_covariance = true; + detected_object.kinematics.orientation_availability = + tracked_object.kinematics.orientation_availability; + detected_object.kinematics.twist_with_covariance = + tracked_object.kinematics.twist_with_covariance; + detected_object.kinematics.has_twist = true; + detected_object.kinematics.has_twist_covariance = true; + + detected_object.shape = tracked_object.shape; + return detected_object; +} + +DetectedObjects toDetectedObjects(const TrackedObjects & tracked_objects) +{ + autoware_perception_msgs::msg::DetectedObjects detected_objects; + detected_objects.header = tracked_objects.header; + + for (const auto & tracked_object : tracked_objects.objects) { + detected_objects.objects.push_back(toDetectedObject(tracked_object)); + } + return detected_objects; +} + +TrackedObject toTrackedObject(const DetectedObject & detected_object) +{ + TrackedObject tracked_object; + tracked_object.existence_probability = detected_object.existence_probability; + + tracked_object.classification = detected_object.classification; + + tracked_object.kinematics.pose_with_covariance = detected_object.kinematics.pose_with_covariance; + tracked_object.kinematics.twist_with_covariance = + detected_object.kinematics.twist_with_covariance; + tracked_object.kinematics.orientation_availability = + detected_object.kinematics.orientation_availability; + + tracked_object.shape = detected_object.shape; + return tracked_object; +} +} // namespace autoware::object_recognition_utils diff --git a/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp b/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp new file mode 100644 index 0000000000..102c0e7e2b --- /dev/null +++ b/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp @@ -0,0 +1,133 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" + +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spherical_linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" + +#include +#include + +namespace autoware::object_recognition_utils +{ +boost::optional calcInterpolatedPose( + const autoware_perception_msgs::msg::PredictedPath & path, const double relative_time) +{ + // Check if relative time is in the valid range + if (path.path.empty() || relative_time < 0.0) { + return boost::none; + } + + constexpr double epsilon = 1e-6; + const double & time_step = rclcpp::Duration(path.time_step).seconds(); + for (size_t path_idx = 1; path_idx < path.path.size(); ++path_idx) { + const auto & pt = path.path.at(path_idx); + const auto & prev_pt = path.path.at(path_idx - 1); + if (relative_time - epsilon < time_step * path_idx) { + const double offset = relative_time - time_step * (path_idx - 1); + const double ratio = std::clamp(offset / time_step, 0.0, 1.0); + return autoware_utils::calc_interpolated_pose(prev_pt, pt, ratio, false); + } + } + + return boost::none; +} + +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, + const std::vector & resampled_time, const bool use_spline_for_xy, + const bool use_spline_for_z) +{ + if (path.path.empty() || resampled_time.empty()) { + throw std::invalid_argument("input path or resampled_time is empty"); + } + + const double & time_step = rclcpp::Duration(path.time_step).seconds(); + std::vector input_time(path.path.size()); + std::vector x(path.path.size()); + std::vector y(path.path.size()); + std::vector z(path.path.size()); + std::vector quat(path.path.size()); + for (size_t i = 0; i < path.path.size(); ++i) { + input_time.at(i) = time_step * i; + x.at(i) = path.path.at(i).position.x; + y.at(i) = path.path.at(i).position.y; + z.at(i) = path.path.at(i).position.z; + quat.at(i) = path.path.at(i).orientation; + } + + const auto lerp = [&](const auto & input) { + return autoware::interpolation::lerp(input_time, input, resampled_time); + }; + const auto spline = [&](const auto & input) { + return autoware::interpolation::spline(input_time, input, resampled_time); + }; + const auto slerp = [&](const auto & input) { + return autoware::interpolation::slerp(input_time, input, resampled_time); + }; + + const auto interpolated_x = use_spline_for_xy ? spline(x) : lerp(x); + const auto interpolated_y = use_spline_for_xy ? spline(y) : lerp(y); + const auto interpolated_z = use_spline_for_z ? spline(z) : lerp(z); + const auto interpolated_quat = slerp(quat); + + autoware_perception_msgs::msg::PredictedPath resampled_path; + const auto resampled_size = std::min(resampled_path.path.max_size(), resampled_time.size()); + resampled_path.confidence = path.confidence; + resampled_path.path.resize(resampled_size); + + // Set Position + for (size_t i = 0; i < resampled_size; ++i) { + const auto p = autoware_utils::create_point( + interpolated_x.at(i), interpolated_y.at(i), interpolated_z.at(i)); + resampled_path.path.at(i).position = p; + resampled_path.path.at(i).orientation = interpolated_quat.at(i); + } + + return resampled_path; +} + +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, + const double sampling_horizon, const bool use_spline_for_xy, const bool use_spline_for_z) +{ + if (path.path.empty()) { + throw std::invalid_argument("Predicted Path is empty"); + } + + if (sampling_time_interval <= 0.0 || sampling_horizon <= 0.0) { + throw std::invalid_argument("sampling time interval or sampling time horizon is negative"); + } + + // Calculate Horizon + const double predicted_horizon = + rclcpp::Duration(path.time_step).seconds() * static_cast(path.path.size() - 1); + const double horizon = std::min(predicted_horizon, sampling_horizon); + + // Get sampling time vector + constexpr double epsilon = 1e-6; + std::vector sampling_time_vector; + for (double t = 0.0; t < horizon + epsilon; t += sampling_time_interval) { + sampling_time_vector.push_back(t); + } + + // Resample and substitute time interval + auto resampled_path = + resamplePredictedPath(path, sampling_time_vector, use_spline_for_xy, use_spline_for_z); + resampled_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval); + return resampled_path; +} +} // namespace autoware::object_recognition_utils diff --git a/common/autoware_object_recognition_utils/test/src/test_conversion.cpp b/common/autoware_object_recognition_utils/test/src/test_conversion.cpp new file mode 100644 index 0000000000..5692c4c42f --- /dev/null +++ b/common/autoware_object_recognition_utils/test/src/test_conversion.cpp @@ -0,0 +1,273 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/object_recognition_utils/conversion.hpp" +#include "autoware_utils/geometry/geometry.hpp" + +#include + +namespace +{ +geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const double z) +{ + geometry_msgs::msg::Point32 p; + p.x = x; + p.y = y; + p.z = z; + return p; +} + +autoware_perception_msgs::msg::ObjectClassification createObjectClassification( + const std::uint8_t label, const double probability) +{ + autoware_perception_msgs::msg::ObjectClassification classification; + classification.label = label; + classification.probability = probability; + + return classification; +} +} // namespace + +// NOTE: covariance is not checked +TEST(conversion, test_toDetectedObject) +{ + using autoware::object_recognition_utils::toDetectedObject; + using autoware_perception_msgs::msg::ObjectClassification; + + autoware_perception_msgs::msg::TrackedObject tracked_obj; + // existence probability + tracked_obj.existence_probability = 1.0; + // classification + tracked_obj.classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); + tracked_obj.classification.push_back( + createObjectClassification(ObjectClassification::TRUCK, 0.8)); + tracked_obj.classification.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); + // kinematics + tracked_obj.kinematics.pose_with_covariance.pose.position.x = 1.0; + tracked_obj.kinematics.pose_with_covariance.pose.position.y = 2.0; + tracked_obj.kinematics.pose_with_covariance.pose.position.z = 3.0; + tracked_obj.kinematics.pose_with_covariance.pose.orientation.x = 4.0; + tracked_obj.kinematics.pose_with_covariance.pose.orientation.y = 5.0; + tracked_obj.kinematics.pose_with_covariance.pose.orientation.z = 6.0; + tracked_obj.kinematics.pose_with_covariance.pose.orientation.w = 7.0; + tracked_obj.kinematics.twist_with_covariance.twist.linear.x = 1.0; + tracked_obj.kinematics.twist_with_covariance.twist.linear.y = 2.0; + tracked_obj.kinematics.twist_with_covariance.twist.linear.z = 3.0; + tracked_obj.kinematics.twist_with_covariance.twist.angular.x = 4.0; + tracked_obj.kinematics.twist_with_covariance.twist.angular.y = 5.0; + tracked_obj.kinematics.twist_with_covariance.twist.angular.z = 6.0; + tracked_obj.kinematics.orientation_availability = 1; + // shape + tracked_obj.shape.type = 1; + tracked_obj.shape.dimensions.x = 1.0; + tracked_obj.shape.dimensions.y = 2.0; + tracked_obj.shape.dimensions.z = 3.0; + tracked_obj.shape.footprint.points.push_back(createPoint32(1.0, 2.0, 3.0)); + tracked_obj.shape.footprint.points.push_back(createPoint32(2.0, 4.0, 6.0)); + + const auto detected_obj = toDetectedObject(tracked_obj); + + // existence probability + EXPECT_DOUBLE_EQ(tracked_obj.existence_probability, detected_obj.existence_probability); + // classification + EXPECT_EQ(tracked_obj.classification.size(), detected_obj.classification.size()); + EXPECT_EQ(tracked_obj.classification.at(0).label, detected_obj.classification.at(0).label); + EXPECT_DOUBLE_EQ( + tracked_obj.classification.at(0).probability, detected_obj.classification.at(0).probability); + EXPECT_EQ(tracked_obj.classification.at(1).label, detected_obj.classification.at(1).label); + EXPECT_DOUBLE_EQ( + tracked_obj.classification.at(1).probability, detected_obj.classification.at(1).probability); + EXPECT_EQ(tracked_obj.classification.at(2).label, detected_obj.classification.at(2).label); + EXPECT_DOUBLE_EQ( + tracked_obj.classification.at(2).probability, detected_obj.classification.at(2).probability); + // kinematics + EXPECT_DOUBLE_EQ( + tracked_obj.kinematics.pose_with_covariance.pose.position.x, + detected_obj.kinematics.pose_with_covariance.pose.position.x); + EXPECT_DOUBLE_EQ( + tracked_obj.kinematics.pose_with_covariance.pose.position.y, + detected_obj.kinematics.pose_with_covariance.pose.position.y); + EXPECT_DOUBLE_EQ( + tracked_obj.kinematics.pose_with_covariance.pose.position.z, + detected_obj.kinematics.pose_with_covariance.pose.position.z); + EXPECT_DOUBLE_EQ( + tracked_obj.kinematics.pose_with_covariance.pose.orientation.x, + detected_obj.kinematics.pose_with_covariance.pose.orientation.x); + EXPECT_DOUBLE_EQ( + tracked_obj.kinematics.pose_with_covariance.pose.orientation.y, + detected_obj.kinematics.pose_with_covariance.pose.orientation.y); + EXPECT_DOUBLE_EQ( + tracked_obj.kinematics.pose_with_covariance.pose.orientation.z, + detected_obj.kinematics.pose_with_covariance.pose.orientation.z); + EXPECT_DOUBLE_EQ( + tracked_obj.kinematics.pose_with_covariance.pose.orientation.w, + detected_obj.kinematics.pose_with_covariance.pose.orientation.w); + EXPECT_DOUBLE_EQ( + tracked_obj.kinematics.twist_with_covariance.twist.linear.x, + detected_obj.kinematics.twist_with_covariance.twist.linear.x); + EXPECT_DOUBLE_EQ( + tracked_obj.kinematics.twist_with_covariance.twist.linear.y, + detected_obj.kinematics.twist_with_covariance.twist.linear.y); + EXPECT_DOUBLE_EQ( + tracked_obj.kinematics.twist_with_covariance.twist.linear.z, + detected_obj.kinematics.twist_with_covariance.twist.linear.z); + EXPECT_DOUBLE_EQ( + tracked_obj.kinematics.twist_with_covariance.twist.angular.x, + detected_obj.kinematics.twist_with_covariance.twist.angular.x); + EXPECT_DOUBLE_EQ( + tracked_obj.kinematics.twist_with_covariance.twist.angular.y, + detected_obj.kinematics.twist_with_covariance.twist.angular.y); + EXPECT_DOUBLE_EQ( + tracked_obj.kinematics.twist_with_covariance.twist.angular.z, + detected_obj.kinematics.twist_with_covariance.twist.angular.z); + EXPECT_EQ( + tracked_obj.kinematics.orientation_availability, + detected_obj.kinematics.orientation_availability); + EXPECT_EQ(detected_obj.kinematics.has_position_covariance, true); + EXPECT_EQ(detected_obj.kinematics.has_twist, true); + EXPECT_EQ(detected_obj.kinematics.has_twist_covariance, true); + // shape + EXPECT_EQ(tracked_obj.shape.type, detected_obj.shape.type); + EXPECT_DOUBLE_EQ(tracked_obj.shape.dimensions.x, detected_obj.shape.dimensions.x); + EXPECT_DOUBLE_EQ(tracked_obj.shape.dimensions.y, detected_obj.shape.dimensions.y); + EXPECT_DOUBLE_EQ(tracked_obj.shape.dimensions.z, detected_obj.shape.dimensions.z); + EXPECT_EQ(tracked_obj.shape.footprint.points.size(), detected_obj.shape.footprint.points.size()); + EXPECT_DOUBLE_EQ( + tracked_obj.shape.footprint.points.at(0).x, detected_obj.shape.footprint.points.at(0).x); + EXPECT_DOUBLE_EQ( + tracked_obj.shape.footprint.points.at(0).y, detected_obj.shape.footprint.points.at(0).y); + EXPECT_DOUBLE_EQ( + tracked_obj.shape.footprint.points.at(0).z, detected_obj.shape.footprint.points.at(0).z); + EXPECT_DOUBLE_EQ( + tracked_obj.shape.footprint.points.at(1).x, detected_obj.shape.footprint.points.at(1).x); + EXPECT_DOUBLE_EQ( + tracked_obj.shape.footprint.points.at(1).y, detected_obj.shape.footprint.points.at(1).y); + EXPECT_DOUBLE_EQ( + tracked_obj.shape.footprint.points.at(1).z, detected_obj.shape.footprint.points.at(1).z); +} + +// NOTE: covariance is not checked +TEST(conversion, test_toTrackedObject) +{ + using autoware::object_recognition_utils::toTrackedObject; + using autoware_perception_msgs::msg::ObjectClassification; + + autoware_perception_msgs::msg::DetectedObject detected_obj; + // existence probability + detected_obj.existence_probability = 1.0; + // classification + detected_obj.classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); + detected_obj.classification.push_back( + createObjectClassification(ObjectClassification::TRUCK, 0.8)); + detected_obj.classification.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); + // kinematics + detected_obj.kinematics.pose_with_covariance.pose.position.x = 1.0; + detected_obj.kinematics.pose_with_covariance.pose.position.y = 2.0; + detected_obj.kinematics.pose_with_covariance.pose.position.z = 3.0; + detected_obj.kinematics.pose_with_covariance.pose.orientation.x = 4.0; + detected_obj.kinematics.pose_with_covariance.pose.orientation.y = 5.0; + detected_obj.kinematics.pose_with_covariance.pose.orientation.z = 6.0; + detected_obj.kinematics.pose_with_covariance.pose.orientation.w = 7.0; + detected_obj.kinematics.twist_with_covariance.twist.linear.x = 1.0; + detected_obj.kinematics.twist_with_covariance.twist.linear.y = 2.0; + detected_obj.kinematics.twist_with_covariance.twist.linear.z = 3.0; + detected_obj.kinematics.twist_with_covariance.twist.angular.x = 4.0; + detected_obj.kinematics.twist_with_covariance.twist.angular.y = 5.0; + detected_obj.kinematics.twist_with_covariance.twist.angular.z = 6.0; + detected_obj.kinematics.orientation_availability = 1; + // shape + detected_obj.shape.type = 1; + detected_obj.shape.dimensions.x = 1.0; + detected_obj.shape.dimensions.y = 2.0; + detected_obj.shape.dimensions.z = 3.0; + detected_obj.shape.footprint.points.push_back(createPoint32(1.0, 2.0, 3.0)); + detected_obj.shape.footprint.points.push_back(createPoint32(2.0, 4.0, 6.0)); + + const auto tracked_obj = toTrackedObject(detected_obj); + + // existence probability + EXPECT_DOUBLE_EQ(detected_obj.existence_probability, tracked_obj.existence_probability); + // classification + EXPECT_EQ(detected_obj.classification.size(), tracked_obj.classification.size()); + EXPECT_EQ(detected_obj.classification.at(0).label, tracked_obj.classification.at(0).label); + EXPECT_DOUBLE_EQ( + detected_obj.classification.at(0).probability, tracked_obj.classification.at(0).probability); + EXPECT_EQ(detected_obj.classification.at(1).label, tracked_obj.classification.at(1).label); + EXPECT_DOUBLE_EQ( + detected_obj.classification.at(1).probability, tracked_obj.classification.at(1).probability); + EXPECT_EQ(detected_obj.classification.at(2).label, tracked_obj.classification.at(2).label); + EXPECT_DOUBLE_EQ( + detected_obj.classification.at(2).probability, tracked_obj.classification.at(2).probability); + // kinematics + EXPECT_DOUBLE_EQ( + detected_obj.kinematics.pose_with_covariance.pose.position.x, + tracked_obj.kinematics.pose_with_covariance.pose.position.x); + EXPECT_DOUBLE_EQ( + detected_obj.kinematics.pose_with_covariance.pose.position.y, + tracked_obj.kinematics.pose_with_covariance.pose.position.y); + EXPECT_DOUBLE_EQ( + detected_obj.kinematics.pose_with_covariance.pose.position.z, + tracked_obj.kinematics.pose_with_covariance.pose.position.z); + EXPECT_DOUBLE_EQ( + detected_obj.kinematics.pose_with_covariance.pose.orientation.x, + tracked_obj.kinematics.pose_with_covariance.pose.orientation.x); + EXPECT_DOUBLE_EQ( + detected_obj.kinematics.pose_with_covariance.pose.orientation.y, + tracked_obj.kinematics.pose_with_covariance.pose.orientation.y); + EXPECT_DOUBLE_EQ( + detected_obj.kinematics.pose_with_covariance.pose.orientation.z, + tracked_obj.kinematics.pose_with_covariance.pose.orientation.z); + EXPECT_DOUBLE_EQ( + detected_obj.kinematics.pose_with_covariance.pose.orientation.w, + tracked_obj.kinematics.pose_with_covariance.pose.orientation.w); + EXPECT_DOUBLE_EQ( + detected_obj.kinematics.twist_with_covariance.twist.linear.x, + tracked_obj.kinematics.twist_with_covariance.twist.linear.x); + EXPECT_DOUBLE_EQ( + detected_obj.kinematics.twist_with_covariance.twist.linear.y, + tracked_obj.kinematics.twist_with_covariance.twist.linear.y); + EXPECT_DOUBLE_EQ( + detected_obj.kinematics.twist_with_covariance.twist.linear.z, + tracked_obj.kinematics.twist_with_covariance.twist.linear.z); + EXPECT_DOUBLE_EQ( + detected_obj.kinematics.twist_with_covariance.twist.angular.x, + tracked_obj.kinematics.twist_with_covariance.twist.angular.x); + EXPECT_DOUBLE_EQ( + detected_obj.kinematics.twist_with_covariance.twist.angular.y, + tracked_obj.kinematics.twist_with_covariance.twist.angular.y); + EXPECT_DOUBLE_EQ( + detected_obj.kinematics.twist_with_covariance.twist.angular.z, + tracked_obj.kinematics.twist_with_covariance.twist.angular.z); + EXPECT_EQ( + detected_obj.kinematics.orientation_availability, + tracked_obj.kinematics.orientation_availability); + // shape + EXPECT_EQ(detected_obj.shape.type, tracked_obj.shape.type); + EXPECT_DOUBLE_EQ(detected_obj.shape.dimensions.x, tracked_obj.shape.dimensions.x); + EXPECT_DOUBLE_EQ(detected_obj.shape.dimensions.y, tracked_obj.shape.dimensions.y); + EXPECT_DOUBLE_EQ(detected_obj.shape.dimensions.z, tracked_obj.shape.dimensions.z); + EXPECT_EQ(detected_obj.shape.footprint.points.size(), tracked_obj.shape.footprint.points.size()); + EXPECT_DOUBLE_EQ( + detected_obj.shape.footprint.points.at(0).x, tracked_obj.shape.footprint.points.at(0).x); + EXPECT_DOUBLE_EQ( + detected_obj.shape.footprint.points.at(0).y, tracked_obj.shape.footprint.points.at(0).y); + EXPECT_DOUBLE_EQ( + detected_obj.shape.footprint.points.at(0).z, tracked_obj.shape.footprint.points.at(0).z); + EXPECT_DOUBLE_EQ( + detected_obj.shape.footprint.points.at(1).x, tracked_obj.shape.footprint.points.at(1).x); + EXPECT_DOUBLE_EQ( + detected_obj.shape.footprint.points.at(1).y, tracked_obj.shape.footprint.points.at(1).y); + EXPECT_DOUBLE_EQ( + detected_obj.shape.footprint.points.at(1).z, tracked_obj.shape.footprint.points.at(1).z); +} diff --git a/common/autoware_object_recognition_utils/test/src/test_geometry.cpp b/common/autoware_object_recognition_utils/test/src/test_geometry.cpp new file mode 100644 index 0000000000..f5165482a2 --- /dev/null +++ b/common/autoware_object_recognition_utils/test/src/test_geometry.cpp @@ -0,0 +1,111 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/object_recognition_utils/geometry.hpp" + +#include + +#include + +TEST(geometry, getPose) +{ + using autoware::object_recognition_utils::getPose; + + const double x_ans = 1.0; + const double y_ans = 2.0; + const double z_ans = 3.0; + const double q_x_ans = 0.1; + const double q_y_ans = 0.2; + const double q_z_ans = 0.3; + const double q_w_ans = 0.4; + + { + geometry_msgs::msg::Pose p; + p.position.x = x_ans; + p.position.y = y_ans; + p.position.z = z_ans; + p.orientation.x = q_x_ans; + p.orientation.y = q_y_ans; + p.orientation.z = q_z_ans; + p.orientation.w = q_w_ans; + const geometry_msgs::msg::Pose p_out = getPose(p); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); + } + + { + autoware_perception_msgs::msg::DetectedObject p; + p.kinematics.pose_with_covariance.pose.position.x = x_ans; + p.kinematics.pose_with_covariance.pose.position.y = y_ans; + p.kinematics.pose_with_covariance.pose.position.z = z_ans; + p.kinematics.pose_with_covariance.pose.orientation.x = q_x_ans; + p.kinematics.pose_with_covariance.pose.orientation.y = q_y_ans; + p.kinematics.pose_with_covariance.pose.orientation.z = q_z_ans; + p.kinematics.pose_with_covariance.pose.orientation.w = q_w_ans; + + const geometry_msgs::msg::Pose p_out = getPose(p); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); + } + + { + autoware_perception_msgs::msg::TrackedObject p; + p.kinematics.pose_with_covariance.pose.position.x = x_ans; + p.kinematics.pose_with_covariance.pose.position.y = y_ans; + p.kinematics.pose_with_covariance.pose.position.z = z_ans; + p.kinematics.pose_with_covariance.pose.orientation.x = q_x_ans; + p.kinematics.pose_with_covariance.pose.orientation.y = q_y_ans; + p.kinematics.pose_with_covariance.pose.orientation.z = q_z_ans; + p.kinematics.pose_with_covariance.pose.orientation.w = q_w_ans; + + const geometry_msgs::msg::Pose p_out = getPose(p); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); + } + + { + autoware_perception_msgs::msg::PredictedObject p; + p.kinematics.initial_pose_with_covariance.pose.position.x = x_ans; + p.kinematics.initial_pose_with_covariance.pose.position.y = y_ans; + p.kinematics.initial_pose_with_covariance.pose.position.z = z_ans; + p.kinematics.initial_pose_with_covariance.pose.orientation.x = q_x_ans; + p.kinematics.initial_pose_with_covariance.pose.orientation.y = q_y_ans; + p.kinematics.initial_pose_with_covariance.pose.orientation.z = q_z_ans; + p.kinematics.initial_pose_with_covariance.pose.orientation.w = q_w_ans; + + const geometry_msgs::msg::Pose p_out = getPose(p); + EXPECT_DOUBLE_EQ(p_out.position.x, x_ans); + EXPECT_DOUBLE_EQ(p_out.position.y, y_ans); + EXPECT_DOUBLE_EQ(p_out.position.z, z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.x, q_x_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.y, q_y_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.z, q_z_ans); + EXPECT_DOUBLE_EQ(p_out.orientation.w, q_w_ans); + } +} diff --git a/common/autoware_object_recognition_utils/test/src/test_matching.cpp b/common/autoware_object_recognition_utils/test/src/test_matching.cpp new file mode 100644 index 0000000000..270a23ff78 --- /dev/null +++ b/common/autoware_object_recognition_utils/test/src/test_matching.cpp @@ -0,0 +1,286 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/object_recognition_utils/matching.hpp" +#include "autoware_utils/geometry/geometry.hpp" + +#include + +#include + +using autoware_utils::Point2d; +using autoware_utils::Point3d; + +constexpr double epsilon = 1e-06; + +namespace +{ +geometry_msgs::msg::Pose createPose(const double x, const double y, const double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = geometry_msgs::build().x(x).y(y).z(0.0); + p.orientation = autoware_utils::create_quaternion_from_yaw(yaw); + return p; +} +} // namespace + +TEST(matching, test_get2dIoU) +{ + using autoware::object_recognition_utils::get2dIoU; + using autoware_perception_msgs::msg::DetectedObject; + + const double quart_circle = 0.16237976320958225; + + { // non overlapped + DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + DetectedObject target_obj; + target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.dimensions.x = 1.0; + + const double iou = get2dIoU(source_obj, target_obj); + EXPECT_DOUBLE_EQ(iou, 0.0); + } + + { // partially overlapped + DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + DetectedObject target_obj; + target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.dimensions.x = 1.0; + + const double iou = get2dIoU(source_obj, target_obj); + EXPECT_NEAR(iou, quart_circle / (1.0 + quart_circle * 3), epsilon); + } + + { // fully overlapped + DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + DetectedObject target_obj; + target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.dimensions.x = 1.0; + + const double iou = get2dIoU(source_obj, target_obj); + EXPECT_DOUBLE_EQ(iou, quart_circle * 4); + } +} + +TEST(object_recognition_utils, test_get2dGeneralizedIoU) +{ + using autoware::object_recognition_utils::get2dGeneralizedIoU; + // TODO(Shin-kyoto): + // get2dGeneralizedIoU uses outer points of each polygon. + // But these points contain an sampling error of outer line, + // so get2dGeneralizedIoU includes an error of about 0.03. + // Therefore, in this test, epsilon is set to 0.04. + constexpr double epsilon_giou = 4 * 1e-02; + const double quart_circle = 0.16237976320958225; + + { // non overlapped + autoware_perception_msgs::msg::DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 0.0, M_PI); + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + autoware_perception_msgs::msg::DetectedObject target_obj; + target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.dimensions.x = 1.0; + + const double giou = get2dGeneralizedIoU(source_obj, target_obj); + EXPECT_NEAR(giou, (2 * quart_circle - 1) / (2 * quart_circle + 2), epsilon_giou); // not 0 + } + + { // partially overlapped + autoware_perception_msgs::msg::DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.0, M_PI); + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + autoware_perception_msgs::msg::DetectedObject target_obj; + target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.dimensions.x = 1.0; + + const double giou = get2dGeneralizedIoU(source_obj, target_obj); + EXPECT_NEAR(giou, 2 * quart_circle / (2 * quart_circle + 1), epsilon_giou); + } + + { // fully overlapped + autoware_perception_msgs::msg::DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + autoware_perception_msgs::msg::DetectedObject target_obj; + target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.dimensions.x = 1.0; + + const double giou = get2dGeneralizedIoU(source_obj, target_obj); + EXPECT_NEAR(giou, quart_circle * 4, epsilon_giou); // giou equals iou + } +} + +TEST(matching, test_get2dPrecision) +{ + using autoware::object_recognition_utils::get2dPrecision; + using autoware_perception_msgs::msg::DetectedObject; + const double quart_circle = 0.16237976320958225; + + { // non overlapped + DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + DetectedObject target_obj; + target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.dimensions.x = 1.0; + + const double precision = get2dPrecision(source_obj, target_obj); + EXPECT_DOUBLE_EQ(precision, 0.0); + + // reverse source and target object + const double reversed_precision = get2dPrecision(target_obj, source_obj); + EXPECT_DOUBLE_EQ(reversed_precision, 0.0); + } + + { // partially overlapped + DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + DetectedObject target_obj; + target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.dimensions.x = 1.0; + + const double precision = get2dPrecision(source_obj, target_obj); + EXPECT_NEAR(precision, quart_circle, epsilon); + + // reverse source and target object + const double reversed_precision = get2dPrecision(target_obj, source_obj); + EXPECT_NEAR(reversed_precision, 1 / 4.0, epsilon); + } + + { // fully overlapped + DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + DetectedObject target_obj; + target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.dimensions.x = 1.0; + + const double precision = get2dPrecision(source_obj, target_obj); + EXPECT_DOUBLE_EQ(precision, quart_circle * 4); + + // reverse source and target object + const double reversed_precision = get2dPrecision(target_obj, source_obj); + EXPECT_DOUBLE_EQ(reversed_precision, 1.0); + } +} + +TEST(matching, test_get2dRecall) +{ + using autoware::object_recognition_utils::get2dRecall; + using autoware_perception_msgs::msg::DetectedObject; + const double quart_circle = 0.16237976320958225; + + { // non overlapped + DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + DetectedObject target_obj; + target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.dimensions.x = 1.0; + + const double recall = get2dRecall(source_obj, target_obj); + EXPECT_DOUBLE_EQ(recall, 0.0); + + // reverse source and target object + const double reversed_recall = get2dRecall(target_obj, source_obj); + EXPECT_DOUBLE_EQ(reversed_recall, 0.0); + } + + { // partially overlapped + DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + DetectedObject target_obj; + target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.dimensions.x = 1.0; + + const double recall = get2dRecall(source_obj, target_obj); + EXPECT_NEAR(recall, 1 / 4.0, epsilon); + + // reverse source and target object + const double reversed_recall = get2dRecall(target_obj, source_obj); + EXPECT_NEAR(reversed_recall, quart_circle, epsilon); + } + + { // fully overlapped + DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + DetectedObject target_obj; + target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.dimensions.x = 1.0; + + const double recall = get2dRecall(source_obj, target_obj); + EXPECT_DOUBLE_EQ(recall, 1.0); + + // reverse source and target object + const double reversed_recall = get2dRecall(target_obj, source_obj); + EXPECT_DOUBLE_EQ(reversed_recall, quart_circle * 4); + } +} diff --git a/common/autoware_object_recognition_utils/test/src/test_object_classification.cpp b/common/autoware_object_recognition_utils/test/src/test_object_classification.cpp new file mode 100644 index 0000000000..31d2a90f2e --- /dev/null +++ b/common/autoware_object_recognition_utils/test/src/test_object_classification.cpp @@ -0,0 +1,302 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/object_recognition_utils/object_classification.hpp" + +#include + +#include + +constexpr double epsilon = 1e-06; + +namespace +{ +autoware_perception_msgs::msg::ObjectClassification createObjectClassification( + const std::uint8_t label, const double probability) +{ + autoware_perception_msgs::msg::ObjectClassification classification; + classification.label = label; + classification.probability = probability; + + return classification; +} + +} // namespace + +TEST(object_classification, test_getHighestProbLabel) +{ + using autoware::object_recognition_utils::getHighestProbLabel; + using autoware_perception_msgs::msg::ObjectClassification; + + { // empty + std::vector classifications; + std::uint8_t label = getHighestProbLabel(classifications); + EXPECT_EQ(label, ObjectClassification::UNKNOWN); + } + + { // normal case + std::vector classifications; + classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); + classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); + classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); + + std::uint8_t label = getHighestProbLabel(classifications); + EXPECT_EQ(label, ObjectClassification::TRUCK); + } + + { // labels with the same probability + std::vector classifications; + classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); + classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); + classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); + + std::uint8_t label = getHighestProbLabel(classifications); + EXPECT_EQ(label, ObjectClassification::CAR); + } +} + +// Test isVehicle +TEST(object_classification, test_isVehicle) +{ + using autoware::object_recognition_utils::isVehicle; + using autoware_perception_msgs::msg::ObjectClassification; + + { // True Case with uint8_t + EXPECT_TRUE(isVehicle(ObjectClassification::BICYCLE)); + EXPECT_TRUE(isVehicle(ObjectClassification::BUS)); + EXPECT_TRUE(isVehicle(ObjectClassification::CAR)); + EXPECT_TRUE(isVehicle(ObjectClassification::MOTORCYCLE)); + EXPECT_TRUE(isVehicle(ObjectClassification::TRAILER)); + EXPECT_TRUE(isVehicle(ObjectClassification::TRUCK)); + } + + // False Case with uint8_t + { + EXPECT_FALSE(isVehicle(ObjectClassification::UNKNOWN)); + EXPECT_FALSE(isVehicle(ObjectClassification::PEDESTRIAN)); + } + + // True Case with object_classifications + { // normal case + std::vector classification; + classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); + classification.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); + classification.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); + EXPECT_TRUE(isVehicle(classification)); + } + + // False Case with object_classifications + { // false case + std::vector classification; + classification.push_back(createObjectClassification(ObjectClassification::PEDESTRIAN, 0.8)); + classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.7)); + EXPECT_FALSE(isVehicle(classification)); + } +} // TEST isVehicle + +// TEST isCarLikeVehicle +TEST(object_classification, test_isCarLikeVehicle) +{ + using autoware::object_recognition_utils::isCarLikeVehicle; + using autoware_perception_msgs::msg::ObjectClassification; + + { // True Case with uint8_t + EXPECT_TRUE(isCarLikeVehicle(ObjectClassification::BUS)); + EXPECT_TRUE(isCarLikeVehicle(ObjectClassification::CAR)); + EXPECT_TRUE(isCarLikeVehicle(ObjectClassification::TRAILER)); + EXPECT_TRUE(isCarLikeVehicle(ObjectClassification::TRUCK)); + } + + // False Case with uint8_t + { + EXPECT_FALSE(isCarLikeVehicle(ObjectClassification::UNKNOWN)); + EXPECT_FALSE(isCarLikeVehicle(ObjectClassification::BICYCLE)); + EXPECT_FALSE(isCarLikeVehicle(ObjectClassification::PEDESTRIAN)); + EXPECT_FALSE(isCarLikeVehicle(ObjectClassification::MOTORCYCLE)); + } + + // True Case with object_classifications + { // normal case + std::vector classification; + classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); + classification.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); + classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.7)); + EXPECT_TRUE(isCarLikeVehicle(classification)); + } + + // False Case with object_classifications + { // false case + std::vector classification; + classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); + classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.8)); + EXPECT_FALSE(isCarLikeVehicle(classification)); + } + + // Edge case + // When classification has more multiple labels with same maximum probability + // getHighestProbLabel() returns only first highest-scored label. + // so, in edge case it returns a label earlier added. + { // When car and non-car label has same probability + std::vector classification; + classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); + classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); + EXPECT_FALSE( + isCarLikeVehicle(classification)); // evaluated with earlier appended "MOTORCYCLE" label + } +} // TEST isCarLikeVehicle + +// TEST isLargeVehicle +TEST(object_classification, test_isLargeVehicle) +{ + using autoware::object_recognition_utils::isLargeVehicle; + using autoware_perception_msgs::msg::ObjectClassification; + + { // True Case with uint8_t + EXPECT_TRUE(isLargeVehicle(ObjectClassification::BUS)); + EXPECT_TRUE(isLargeVehicle(ObjectClassification::TRAILER)); + EXPECT_TRUE(isLargeVehicle(ObjectClassification::TRUCK)); + } + + // False Case with uint8_t + { + EXPECT_FALSE(isLargeVehicle(ObjectClassification::UNKNOWN)); + EXPECT_FALSE(isLargeVehicle(ObjectClassification::BICYCLE)); + EXPECT_FALSE(isLargeVehicle(ObjectClassification::PEDESTRIAN)); + EXPECT_FALSE(isLargeVehicle(ObjectClassification::MOTORCYCLE)); + EXPECT_FALSE(isLargeVehicle(ObjectClassification::CAR)); + } + + { // false case + std::vector classification; + classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); + classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.8)); + classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); + EXPECT_FALSE(isLargeVehicle(classification)); + } + + // Edge case + // When classification has more multiple labels with same maximum probability + // getHighestProbLabel() returns only first highest-scored label. + // so, in edge case it returns a label earlier added. + { // When large-vehicle and non-large-vehicle label has same probability + std::vector classification; + classification.push_back(createObjectClassification(ObjectClassification::BUS, 0.8)); + classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); + EXPECT_TRUE(isLargeVehicle(classification)); // evaluated with earlier appended "BUS" label + } +} // TEST isLargeVehicle + +TEST(object_classification, test_getHighestProbClassification) +{ + using autoware::object_recognition_utils::getHighestProbClassification; + using autoware_perception_msgs::msg::ObjectClassification; + + { // empty + std::vector classifications; + auto classification = getHighestProbClassification(classifications); + EXPECT_EQ(classification.label, ObjectClassification::UNKNOWN); + EXPECT_DOUBLE_EQ(classification.probability, 0.0); + } + + { // normal case + std::vector classifications; + classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); + classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); + classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); + + auto classification = getHighestProbClassification(classifications); + EXPECT_EQ(classification.label, ObjectClassification::TRUCK); + EXPECT_NEAR(classification.probability, 0.8, epsilon); + } + + { // labels with the same probability + std::vector classifications; + classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); + classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); + classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); + + auto classification = getHighestProbClassification(classifications); + EXPECT_EQ(classification.label, ObjectClassification::CAR); + EXPECT_NEAR(classification.probability, 0.8, epsilon); + } +} + +TEST(object_classification, test_fromString) +{ + using autoware::object_recognition_utils::toLabel; + using autoware::object_recognition_utils::toObjectClassification; + using autoware::object_recognition_utils::toObjectClassifications; + using autoware_perception_msgs::msg::ObjectClassification; + + // toLabel + { + EXPECT_EQ(toLabel("UNKNOWN"), ObjectClassification::UNKNOWN); + EXPECT_EQ(toLabel("CAR"), ObjectClassification::CAR); + EXPECT_EQ(toLabel("TRUCK"), ObjectClassification::TRUCK); + EXPECT_EQ(toLabel("BUS"), ObjectClassification::BUS); + EXPECT_EQ(toLabel("TRAILER"), ObjectClassification::TRAILER); + EXPECT_EQ(toLabel("MOTORCYCLE"), ObjectClassification::MOTORCYCLE); + EXPECT_EQ(toLabel("BICYCLE"), ObjectClassification::BICYCLE); + EXPECT_EQ(toLabel("PEDESTRIAN"), ObjectClassification::PEDESTRIAN); + EXPECT_THROW(toLabel(""), std::runtime_error); + } + + // Classification + { + auto classification = toObjectClassification("CAR", 0.7); + EXPECT_EQ(classification.label, ObjectClassification::CAR); + EXPECT_NEAR(classification.probability, 0.7, epsilon); + } + // Classifications + { + auto classifications = toObjectClassifications("CAR", 0.7); + EXPECT_EQ(classifications.at(0).label, ObjectClassification::CAR); + EXPECT_NEAR(classifications.at(0).probability, 0.7, epsilon); + } +} + +TEST(object_classification, test_convertLabelToString) +{ + using autoware::object_recognition_utils::convertLabelToString; + using autoware_perception_msgs::msg::ObjectClassification; + + // from label + { + EXPECT_EQ(convertLabelToString(ObjectClassification::UNKNOWN), "UNKNOWN"); + EXPECT_EQ(convertLabelToString(ObjectClassification::CAR), "CAR"); + EXPECT_EQ(convertLabelToString(ObjectClassification::TRUCK), "TRUCK"); + EXPECT_EQ(convertLabelToString(ObjectClassification::BUS), "BUS"); + EXPECT_EQ(convertLabelToString(ObjectClassification::TRAILER), "TRAILER"); + EXPECT_EQ(convertLabelToString(ObjectClassification::MOTORCYCLE), "MOTORCYCLE"); + EXPECT_EQ(convertLabelToString(ObjectClassification::BICYCLE), "BICYCLE"); + EXPECT_EQ(convertLabelToString(ObjectClassification::PEDESTRIAN), "PEDESTRIAN"); + } + + // from ObjectClassification + { + auto classification = createObjectClassification(ObjectClassification::CAR, 0.8); + + EXPECT_EQ(convertLabelToString(classification), "CAR"); + } + + // from ObjectClassifications + { + std::vector classifications; + classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); + classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); + classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); + + EXPECT_EQ(convertLabelToString(classifications), "TRUCK"); + } +} diff --git a/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp new file mode 100644 index 0000000000..1386f3c460 --- /dev/null +++ b/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -0,0 +1,298 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" +#include "autoware_utils/geometry/geometry.hpp" +#include "autoware_utils/math/unit_conversion.hpp" + +#include + +#include + +#include + +using autoware_utils::Point2d; +using autoware_utils::Point3d; + +constexpr double epsilon = 1e-06; + +namespace +{ +using autoware_perception_msgs::msg::PredictedPath; +using autoware_utils::create_point; +using autoware_utils::create_quaternion_from_rpy; +using autoware_utils::transform_point; + +geometry_msgs::msg::Pose createPose( + double x, double y, double z, double roll, double pitch, double yaw) +{ + geometry_msgs::msg::Pose p; + p.position = create_point(x, y, z); + p.orientation = create_quaternion_from_rpy(roll, pitch, yaw); + return p; +} + +PredictedPath createTestPredictedPath( + const size_t num_points, const double point_time_interval, const double vel, + const double init_theta = 0.0, const double delta_theta = 0.0) +{ + PredictedPath path; + path.confidence = 1.0; + path.time_step = rclcpp::Duration::from_seconds(point_time_interval); + + const double point_interval = vel * point_time_interval; + for (size_t i = 0; i < num_points; ++i) { + const double theta = init_theta + i * delta_theta; + const double x = i * point_interval * std::cos(theta); + const double y = i * point_interval * std::sin(theta); + + const auto p = createPose(x, y, 0.0, 0.0, 0.0, theta); + path.path.push_back(p); + } + return path; +} +} // namespace + +TEST(predicted_path_utils, testCalcInterpolatedPose) +{ + using autoware::object_recognition_utils::calcInterpolatedPose; + using autoware_utils::create_quaternion_from_rpy; + using autoware_utils::create_quaternion_from_yaw; + using autoware_utils::deg2rad; + + const auto path = createTestPredictedPath(100, 0.1, 1.0); + + // Normal Case (same point as the original point) + { + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + for (double t = 0.0; t < 9.0 + 1e-6; t += 1.0) { + const auto p = calcInterpolatedPose(path, t); + + EXPECT_TRUE(p); + EXPECT_NEAR(p->position.x, t * 1.0, epsilon); + EXPECT_NEAR(p->position.y, 0.0, epsilon); + EXPECT_NEAR(p->position.z, 0.0, epsilon); + EXPECT_NEAR(p->orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p->orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p->orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p->orientation.w, ans_quat.w, epsilon); + } + } + + // Normal Case (random case) + { + const auto ans_quat = create_quaternion_from_rpy(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); + for (double t = 0.0; t < 9.0; t += 0.3) { + const auto p = calcInterpolatedPose(path, t); + + EXPECT_TRUE(p); + EXPECT_NEAR(p->position.x, t * 1.0, epsilon); + EXPECT_NEAR(p->position.y, 0.0, epsilon); + EXPECT_NEAR(p->position.z, 0.0, epsilon); + EXPECT_NEAR(p->orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p->orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p->orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p->orientation.w, ans_quat.w, epsilon); + } + } + + // No Interpolation + { + // Negative time + { + const auto p = calcInterpolatedPose(path, -1.0); + EXPECT_FALSE(p); + } + + // Over the time horizon + { + const auto p = calcInterpolatedPose(path, 11.0); + EXPECT_FALSE(p); + } + + // Empty Path + { + PredictedPath empty_path; + const auto p = calcInterpolatedPose(empty_path, 5.0); + EXPECT_FALSE(p); + } + } +} + +TEST(predicted_path_utils, resamplePredictedPath_by_vector) +{ + using autoware::object_recognition_utils::resamplePredictedPath; + using autoware_utils::create_quaternion_from_rpy; + using autoware_utils::create_quaternion_from_yaw; + using autoware_utils::deg2rad; + + const auto path = createTestPredictedPath(10, 1.0, 1.0); + + // Resample Same Points + { + const std::vector resampling_vec = {0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0}; + const auto resampled_path = resamplePredictedPath(path, resampling_vec); + + EXPECT_EQ(resampled_path.path.size(), path.path.size()); + EXPECT_NEAR(path.confidence, resampled_path.confidence, epsilon); + + for (size_t i = 0; i < path.path.size(); ++i) { + EXPECT_NEAR(path.path.at(i).position.x, resampled_path.path.at(i).position.x, epsilon); + EXPECT_NEAR(path.path.at(i).position.y, resampled_path.path.at(i).position.y, epsilon); + EXPECT_NEAR(path.path.at(i).position.z, resampled_path.path.at(i).position.z, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.x, resampled_path.path.at(i).orientation.x, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.y, resampled_path.path.at(i).orientation.y, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.z, resampled_path.path.at(i).orientation.z, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.w, resampled_path.path.at(i).orientation.w, epsilon); + } + } + + // Resample random case + { + const std::vector resampling_vec = {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 1.3, 2.8, + 3.7, 4.2, 5.1, 6.9, 7.3, 8.5, 9.0}; + const auto resampled_path = resamplePredictedPath(path, resampling_vec); + + EXPECT_EQ(resampled_path.path.size(), resampling_vec.size()); + EXPECT_NEAR(path.confidence, resampled_path.confidence, epsilon); + + for (size_t i = 0; i < resampled_path.path.size(); ++i) { + EXPECT_NEAR(resampled_path.path.at(i).position.x, resampling_vec.at(i) * 1.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.x, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.w, 1.0, epsilon); + } + } + + // Resample the path with more than 100 points + { + std::vector resampling_vec(101); + for (size_t i = 0; i < 101; ++i) { + resampling_vec.at(i) = i * 0.05; + } + + const auto resampled_path = resamplePredictedPath(path, resampling_vec); + + EXPECT_NEAR(path.confidence, resampled_path.confidence, epsilon); + + for (size_t i = 0; i < resampled_path.path.size(); ++i) { + EXPECT_NEAR(resampled_path.path.at(i).position.x, resampling_vec.at(i), epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.x, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.w, 1.0, epsilon); + } + } + + // Some points are out of range + { + const std::vector resampling_vec = {-1.0, 0.0, 5.0, 9.0, 9.1}; + EXPECT_THROW(resamplePredictedPath(path, resampling_vec), std::invalid_argument); + } +} + +TEST(predicted_path_utils, resamplePredictedPath_by_sampling_time) +{ + using autoware::object_recognition_utils::resamplePredictedPath; + using autoware_utils::create_quaternion_from_rpy; + using autoware_utils::create_quaternion_from_yaw; + using autoware_utils::deg2rad; + + const auto path = createTestPredictedPath(10, 1.0, 1.0); + + // Sample same points + { + const auto resampled_path = resamplePredictedPath(path, 1.0, 9.0); + + EXPECT_EQ(resampled_path.path.size(), path.path.size()); + EXPECT_NEAR(rclcpp::Duration(resampled_path.time_step).seconds(), 1.0, epsilon); + for (size_t i = 0; i < path.path.size(); ++i) { + EXPECT_NEAR(path.path.at(i).position.x, resampled_path.path.at(i).position.x, epsilon); + EXPECT_NEAR(path.path.at(i).position.y, resampled_path.path.at(i).position.y, epsilon); + EXPECT_NEAR(path.path.at(i).position.z, resampled_path.path.at(i).position.z, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.x, resampled_path.path.at(i).orientation.x, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.y, resampled_path.path.at(i).orientation.y, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.z, resampled_path.path.at(i).orientation.z, epsilon); + EXPECT_NEAR(path.path.at(i).orientation.w, resampled_path.path.at(i).orientation.w, epsilon); + } + } + + // Fine sampling + { + const auto resampled_path = resamplePredictedPath(path, 0.1, 9.0); + + EXPECT_EQ(resampled_path.path.size(), static_cast(91)); + EXPECT_NEAR(rclcpp::Duration(resampled_path.time_step).seconds(), 0.1, epsilon); + for (size_t i = 0; i < resampled_path.path.size(); ++i) { + EXPECT_NEAR(resampled_path.path.at(i).position.x, 0.1 * i, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.x, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.w, 1.0, epsilon); + } + } + + // Coarse Sampling + { + const auto resampled_path = resamplePredictedPath(path, 2.0, 9.0); + + EXPECT_EQ(resampled_path.path.size(), static_cast(5)); + EXPECT_NEAR(rclcpp::Duration(resampled_path.time_step).seconds(), 2.0, epsilon); + for (size_t i = 0; i < resampled_path.path.size(); ++i) { + EXPECT_NEAR(resampled_path.path.at(i).position.x, 2.0 * i, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.x, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.w, 1.0, epsilon); + } + } + + // Shorter horizon + { + const auto resampled_path = resamplePredictedPath(path, 1.5, 7.0); + + EXPECT_EQ(resampled_path.path.size(), static_cast(5)); + EXPECT_NEAR(rclcpp::Duration(resampled_path.time_step).seconds(), 1.5, epsilon); + for (size_t i = 0; i < resampled_path.path.size(); ++i) { + EXPECT_NEAR(resampled_path.path.at(i).position.x, 1.5 * i, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).position.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.x, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.y, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.z, 0.0, epsilon); + EXPECT_NEAR(resampled_path.path.at(i).orientation.w, 1.0, epsilon); + } + } + + // No Sampling + { + // Negative resampling time or resampling time horizon + EXPECT_THROW(resamplePredictedPath(path, 0.0, 9.0), std::invalid_argument); + EXPECT_THROW(resamplePredictedPath(path, -1.0, 9.0), std::invalid_argument); + EXPECT_THROW(resamplePredictedPath(path, 1.0, 0.0), std::invalid_argument); + EXPECT_THROW(resamplePredictedPath(path, 1.0, -9.0), std::invalid_argument); + + PredictedPath empty_path; + EXPECT_THROW(resamplePredictedPath(empty_path, 1.0, 10.0), std::invalid_argument); + } +}