feat(autoware_object_recognition_utils): move package to core (#232)

Signed-off-by: liuXinGangChina <lxg19892021@gmail.com>

common/autoware_object_recognition_utils/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_object_recognition_utils)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Boost REQUIRED)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/predicted_path_utils.cpp
+  src/conversion.cpp
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_ros REQUIRED)
+
+  file(GLOB_RECURSE test_files test/**/*.cpp)
+
+  ament_add_ros_isolated_gtest(test_object_recognition_utils ${test_files})
+
+  target_link_libraries(test_object_recognition_utils
+    ${PROJECT_NAME}
+  )
+endif()
+
+ament_auto_package()

common/autoware_object_recognition_utils/README.md

@@ -0,0 +1,50 @@
+# autoware_object_recognition_utils
+
+## Overview
+
+This package contains a library of common functions that are useful across the perception module and planning module.
+
+## Design
+
+### Conversion
+
+Ensuring accurate and efficient converting between DetectedObject and TrackedObject types.
+
+### Geometry
+
+It provides specialized implementations for each object type (e.g., DetectedObject, TrackedObject, and PredictedObject) to extract the pose information.
+
+### Matching
+
+It provides utility functions for calculating geometrical metrics, such as 2D IoU (Intersection over Union), GIoU (Generalized IoU), Precision, and Recall for objects. It also provides helper functions for computing areas of intersections, unions, and convex hulls of polygon
+
+### Object Classification
+
+Designed for processing and classifying detected objects, it implements the following functionalities:
+
+- Handling of vehicle category checks
+- Conversion between string class names and numerical labels
+- Probability-based classification selection
+- String representation of object labels
+
+### Predicted Path Utils
+
+Providing utility functions for handling predicted paths of objects. It includes the following functionalities:
+
+- calcInterpolatedPose: Calculates an interpolated pose from a predicted path based on a given time.
+- resamplePredictedPath (version 1): Resamples a predicted path according to a specified time vector, optionally using spline interpolation for smoother results.
+- resamplePredictedPath (version 2): Resamples a predicted path at regular time intervals, including the terminal point, with optional spline interpolation.
+
+## Usage
+
+include all-in-one header files if multiple functionalities are needed:
+
+```cpp
+#include <autoware_object_recognition_utils/object_recognition_utils.hpp>
+```
+
+include specific header files if only a subset of functionalities is needed:
+
+```cpp
+#include <autoware_object_recognition_utils/object_classifier.hpp>
+```

common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/conversion.hpp

@@ -0,0 +1,33 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_
+#define AUTOWARE__OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_
+
+#include <autoware_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_perception_msgs/msg/tracked_objects.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);
+DetectedObjects toDetectedObjects(const TrackedObjects & tracked_objects);
+TrackedObject toTrackedObject(const DetectedObject & detected_object);
+}  // namespace autoware::object_recognition_utils
+
+#endif  // AUTOWARE__OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_

common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/geometry.hpp

@@ -0,0 +1,57 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_
+#define AUTOWARE__OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_
+
+#include <autoware_perception_msgs/msg/detected_object.hpp>
+#include <autoware_perception_msgs/msg/predicted_object.hpp>
+#include <autoware_perception_msgs/msg/tracked_object.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+namespace autoware::object_recognition_utils
+{
+template <class T>
+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_

common/autoware_object_recognition_utils/include/autoware/object_recognition_utils/matching.hpp

@@ -0,0 +1,131 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__OBJECT_RECOGNITION_UTILS__MATCHING_HPP_
+#define AUTOWARE__OBJECT_RECOGNITION_UTILS__MATCHING_HPP_
+
+#include "autoware/object_recognition_utils/geometry.hpp"
+#include "autoware_utils/geometry/boost_geometry.hpp"
+#include "autoware_utils/geometry/boost_polygon_utils.hpp"
+
+#include <boost/geometry.hpp>
+
+#include <algorithm>
+#include <numeric>
+#include <utility>
+#include <vector>
+
+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<Polygon2d> 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<Polygon2d> & 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<Polygon2d> 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<Polygon2d> union_polygons;
+  boost::geometry::union_(source_polygon, target_polygon, union_polygons);
+  return getSumArea(union_polygons);
+}
+
+template <class T1, class T2>
+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 <class T1, class T2>
+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 <class T1, class T2>
+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 <class T1, class T2>
+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_