From b3caa5cea50a6747ff77f96786396b3176840a74 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=BF=83=E5=88=9A?= <90366790+liuXinGangChina@users.noreply.github.com> Date: Wed, 5 Mar 2025 22:03:10 +0800 Subject: [PATCH] feat(autoware_object_recognition_utils): remove from universe (#10215) Signed-off-by: liuXinGangChina --- .../CHANGELOG.rst | 71 ---- .../CMakeLists.txt | 26 -- .../README.md | 6 - .../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 ----------------- 18 files changed, 2249 deletions(-) delete mode 100644 common/autoware_object_recognition_utils/CHANGELOG.rst delete mode 100644 common/autoware_object_recognition_utils/CMakeLists.txt delete mode 100644 common/autoware_object_recognition_utils/README.md delete mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/conversion.hpp delete mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/geometry.hpp delete mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp delete mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_classification.hpp delete mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_recognition_utils.hpp delete mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp delete mode 100644 common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/transform.hpp delete mode 100644 common/autoware_object_recognition_utils/package.xml delete mode 100644 common/autoware_object_recognition_utils/src/conversion.cpp delete mode 100644 common/autoware_object_recognition_utils/src/predicted_path_utils.cpp delete mode 100644 common/autoware_object_recognition_utils/test/src/test_conversion.cpp delete mode 100644 common/autoware_object_recognition_utils/test/src/test_geometry.cpp delete mode 100644 common/autoware_object_recognition_utils/test/src/test_matching.cpp delete mode 100644 common/autoware_object_recognition_utils/test/src/test_object_classification.cpp delete mode 100644 common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp diff --git a/common/autoware_object_recognition_utils/CHANGELOG.rst b/common/autoware_object_recognition_utils/CHANGELOG.rst deleted file mode 100644 index 81dd620b33bc9..0000000000000 --- a/common/autoware_object_recognition_utils/CHANGELOG.rst +++ /dev/null @@ -1,71 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_object_recognition_utils -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.42.0 (2025-03-03) -------------------- -* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base -* feat(autoware_utils): replace autoware_universe_utils with autoware_utils (`#10191 `_) -* chore: refine maintainer list (`#10110 `_) - * chore: remove Miura from maintainer - * chore: add Taekjin-san to perception_utils package maintainer - --------- -* Contributors: Fumiya Watanabe, Shunsuke Miura, 心刚 - -0.41.2 (2025-02-19) -------------------- -* chore: bump version to 0.41.1 (`#10088 `_) -* Contributors: Ryohsuke Mitsudome - -0.41.1 (2025-02-10) -------------------- - -0.41.0 (2025-01-29) -------------------- - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(object_recognition_utils): add autoware prefix to object_recognition_utils (`#8946 `_) -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_object_recognition_utils/CMakeLists.txt b/common/autoware_object_recognition_utils/CMakeLists.txt deleted file mode 100644 index 9e1276db8d635..0000000000000 --- a/common/autoware_object_recognition_utils/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -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 deleted file mode 100644 index 8d4eabd19c76d..0000000000000 --- a/common/autoware_object_recognition_utils/README.md +++ /dev/null @@ -1,6 +0,0 @@ -# autoware_object_recognition_utils - -## Purpose - -This package contains a library of common functions that are useful across the object recognition module. -This package may include functions for converting between different data types, msg types, and performing common operations on them. 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 deleted file mode 100644 index 5b3be48cc2598..0000000000000 --- a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/conversion.hpp +++ /dev/null @@ -1,33 +0,0 @@ -// 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 deleted file mode 100644 index a5acc210d92be..0000000000000 --- a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/geometry.hpp +++ /dev/null @@ -1,57 +0,0 @@ -// 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 deleted file mode 100644 index afeb56c1e6a5d..0000000000000 --- a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp +++ /dev/null @@ -1,131 +0,0 @@ -// 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 deleted file mode 100644 index 432abcbe10c55..0000000000000 --- a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_classification.hpp +++ /dev/null @@ -1,174 +0,0 @@ -// 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 deleted file mode 100644 index d73e2e4dc67db..0000000000000 --- a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/object_recognition_utils.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// 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 deleted file mode 100644 index 46a5676da7ca2..0000000000000 --- a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/predicted_path_utils.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// 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 deleted file mode 100644 index ced5956e079e6..0000000000000 --- a/common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/transform.hpp +++ /dev/null @@ -1,152 +0,0 @@ -// 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 deleted file mode 100644 index 97da7e3a92fc6..0000000000000 --- a/common/autoware_object_recognition_utils/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - autoware_object_recognition_utils - 0.42.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 deleted file mode 100644 index 6f8e6aa27cfb4..0000000000000 --- a/common/autoware_object_recognition_utils/src/conversion.cpp +++ /dev/null @@ -1,71 +0,0 @@ -// 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 deleted file mode 100644 index 102c0e7e2b487..0000000000000 --- a/common/autoware_object_recognition_utils/src/predicted_path_utils.cpp +++ /dev/null @@ -1,133 +0,0 @@ -// 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 deleted file mode 100644 index 5692c4c42f20d..0000000000000 --- a/common/autoware_object_recognition_utils/test/src/test_conversion.cpp +++ /dev/null @@ -1,273 +0,0 @@ -// 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 deleted file mode 100644 index f5165482a23bf..0000000000000 --- a/common/autoware_object_recognition_utils/test/src/test_geometry.cpp +++ /dev/null @@ -1,111 +0,0 @@ -// 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 deleted file mode 100644 index 270a23ff786e1..0000000000000 --- a/common/autoware_object_recognition_utils/test/src/test_matching.cpp +++ /dev/null @@ -1,286 +0,0 @@ -// 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 deleted file mode 100644 index 31d2a90f2e4a5..0000000000000 --- a/common/autoware_object_recognition_utils/test/src/test_object_classification.cpp +++ /dev/null @@ -1,302 +0,0 @@ -// 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 deleted file mode 100644 index 1386f3c460145..0000000000000 --- a/common/autoware_object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ /dev/null @@ -1,298 +0,0 @@ -// 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); - } -}