feat(perception_utils): add new package (tier4#1377)

* feat(perception_utils): add new package

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* add test

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* add more functions

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix ci build error

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* Update common/perception_utils/include/perception_utils/geometry.hpp

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* fix ci error

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

common/perception_utils/CMakeLists.txt

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

common/perception_utils/include/perception_utils/geometry.hpp

@@ -0,0 +1,60 @@
+// 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 PERCEPTION_UTILS__GEOMETRY_HPP_
+#define PERCEPTION_UTILS__GEOMETRY_HPP_
+
+#include <autoware_auto_perception_msgs/msg/detected_object.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
+#include <autoware_auto_perception_msgs/msg/tracked_object.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+namespace perception_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_auto_perception_msgs::msg::DetectedObject & obj)
+{
+  return obj.kinematics.pose_with_covariance.pose;
+}
+
+template <>
+inline geometry_msgs::msg::Pose getPose(
+  const autoware_auto_perception_msgs::msg::TrackedObject & obj)
+{
+  return obj.kinematics.pose_with_covariance.pose;
+}
+
+template <>
+inline geometry_msgs::msg::Pose getPose(
+  const autoware_auto_perception_msgs::msg::PredictedObject & obj)
+{
+  return obj.kinematics.initial_pose_with_covariance.pose;
+}
+}  // namespace perception_utils
+
+#endif  // PERCEPTION_UTILS__GEOMETRY_HPP_

common/perception_utils/include/perception_utils/perception_utils.hpp

@@ -0,0 +1,186 @@
+// 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 PERCEPTION_UTILS__PERCEPTION_UTILS_HPP_
+#define PERCEPTION_UTILS__PERCEPTION_UTILS_HPP_
+
+#include "perception_utils/geometry.hpp"
+#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
+#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"
+
+#include "autoware_auto_perception_msgs/msg/detected_objects.hpp"
+#include "autoware_auto_perception_msgs/msg/tracked_objects.hpp"
+#include "geometry_msgs/msg/transform.hpp"
+
+#include <boost/geometry.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <algorithm>
+#include <string>
+#include <vector>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+namespace
+{
+[[maybe_unused]] boost::optional<geometry_msgs::msg::Transform> 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("perception_utils"), ex.what());
+    return boost::none;
+  }
+}
+}  // namespace
+
+namespace perception_utils
+{
+std::uint8_t getHighestProbLabel(
+  const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification);
+
+autoware_auto_perception_msgs::msg::DetectedObject toDetectedObject(
+  const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object);
+
+autoware_auto_perception_msgs::msg::DetectedObjects toDetectedObjects(
+  const autoware_auto_perception_msgs::msg::TrackedObjects & tracked_objects);
+
+autoware_auto_perception_msgs::msg::TrackedObject toTrackedObject(
+  const autoware_auto_perception_msgs::msg::DetectedObject & detected_object);
+
+autoware_auto_perception_msgs::msg::TrackedObjects toTrackedObjects(
+  const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects);
+
+template <class T1, class T2>
+inline double get2dIoU(const T1 source_object, const T2 target_object)
+{
+  const auto & source_pose = getPose(source_object);
+  const auto & target_pose = getPose(target_object);
+
+  const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_pose, source_object.shape);
+  const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_pose, target_object.shape);
+
+  std::vector<tier4_autoware_utils::Polygon2d> union_polygons;
+  std::vector<tier4_autoware_utils::Polygon2d> intersection_polygons;
+  boost::geometry::union_(source_polygon, target_polygon, union_polygons);
+  boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons);
+
+  double intersection_area = 0.0;
+  double union_area = 0.0;
+  for (const auto & intersection_polygon : intersection_polygons) {
+    intersection_area += boost::geometry::area(intersection_polygon);
+  }
+  if (intersection_area == 0.0) return 0.0;
+
+  for (const auto & union_polygon : union_polygons) {
+    union_area += boost::geometry::area(union_polygon);
+  }
+
+  const double iou = union_area < 0.01 ? 0.0 : std::min(1.0, intersection_area / union_area);
+  return iou;
+}
+
+template <class T1, class T2>
+inline double get2dPrecision(const T1 source_object, const T2 target_object)
+{
+  const auto & source_pose = getPose(source_object);
+  const auto & target_pose = getPose(target_object);
+
+  const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_pose, source_object.shape);
+  const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_pose, target_object.shape);
+
+  std::vector<tier4_autoware_utils::Polygon2d> intersection_polygons;
+  boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons);
+
+  double intersection_area = 0.0;
+  double source_area = 0.0;
+  for (const auto & intersection_polygon : intersection_polygons) {
+    intersection_area += boost::geometry::area(intersection_polygon);
+  }
+  if (intersection_area == 0.0) return 0.0;
+
+  source_area = boost::geometry::area(source_polygon);
+
+  const double precision = std::min(1.0, intersection_area / source_area);
+  return precision;
+}
+
+template <class T1, class T2>
+inline double get2dRecall(const T1 source_object, const T2 target_object)
+{
+  const auto & source_pose = getPose(source_object);
+  const auto & target_pose = getPose(target_object);
+
+  const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_pose, source_object.shape);
+  const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_pose, target_object.shape);
+
+  std::vector<tier4_autoware_utils::Polygon2d> intersection_polygons;
+  boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons);
+
+  double intersection_area = 0.0;
+  double target_area = 0.0;
+  for (const auto & intersection_polygon : intersection_polygons) {
+    intersection_area += boost::geometry::area(intersection_polygon);
+  }
+  if (intersection_area == 0.0) return 0.0;
+
+  target_area += boost::geometry::area(target_polygon);
+
+  const double recall = std::min(1.0, intersection_area / target_area);
+  return recall;
+}
+
+template <class T>
+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 =
+        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) {
+      tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects);
+      tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
+      tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose);
+      // TODO(yukkysaito) transform covariance
+    }
+  }
+  return true;
+}
+}  // namespace perception_utils
+#endif  // PERCEPTION_UTILS__PERCEPTION_UTILS_HPP_

common/perception_utils/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>perception_utils</name>
+  <version>0.1.0</version>
+  <description>The perception_utils package</description>
+  <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>libboost-dev</depend>
+  <depend>rclcpp</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/perception_utils/src/perception_utils.cpp

@@ -0,0 +1,95 @@
+// 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 "perception_utils/perception_utils.hpp"
+
+namespace perception_utils
+{
+std::uint8_t getHighestProbLabel(
+  const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification)
+{
+  std::uint8_t label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN;
+  float highest_prob = 0.0;
+  for (const auto & _class : classification) {
+    if (highest_prob < _class.probability) {
+      highest_prob = _class.probability;
+      label = _class.label;
+    }
+  }
+  return label;
+}
+
+autoware_auto_perception_msgs::msg::DetectedObject toDetectedObject(
+  const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object)
+{
+  autoware_auto_perception_msgs::msg::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;
+}
+
+autoware_auto_perception_msgs::msg::DetectedObjects toDetectedObjects(
+  const autoware_auto_perception_msgs::msg::TrackedObjects & tracked_objects)
+{
+  autoware_auto_perception_msgs::msg::DetectedObjects detected_objects;
+  detected_objects.header = tracked_objects.header;
+
+  for (auto & tracked_object : tracked_objects.objects) {
+    detected_objects.objects.push_back(toDetectedObject(tracked_object));
+  }
+  return detected_objects;
+}
+
+autoware_auto_perception_msgs::msg::TrackedObject toTrackedObject(
+  const autoware_auto_perception_msgs::msg::DetectedObject & detected_object)
+{
+  autoware_auto_perception_msgs::msg::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;
+}
+
+autoware_auto_perception_msgs::msg::TrackedObjects toTrackedObjects(
+  const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects)
+{
+  autoware_auto_perception_msgs::msg::TrackedObjects tracked_objects;
+  tracked_objects.header = detected_objects.header;
+
+  for (auto & detected_object : detected_objects.objects) {
+    tracked_objects.objects.push_back(toTrackedObject(detected_object));
+  }
+  return tracked_objects;
+}
+}  // namespace perception_utils