diff --git a/common/perception_utils/CMakeLists.txt b/common/perception_utils/CMakeLists.txt new file mode 100644 index 0000000000000..46f7ae74774e2 --- /dev/null +++ b/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() diff --git a/common/perception_utils/include/perception_utils/geometry.hpp b/common/perception_utils/include/perception_utils/geometry.hpp new file mode 100644 index 0000000000000..6262c323460cb --- /dev/null +++ b/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 +#include +#include +#include + +namespace perception_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_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_ diff --git a/common/perception_utils/include/perception_utils/perception_utils.hpp b/common/perception_utils/include/perception_utils/perception_utils.hpp new file mode 100644 index 0000000000000..70171a95d19ea --- /dev/null +++ b/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 + +#include +#include + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +namespace +{ +[[maybe_unused]] 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("perception_utils"), ex.what()); + return boost::none; + } +} +} // namespace + +namespace perception_utils +{ +std::uint8_t getHighestProbLabel( + const std::vector & 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 +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 union_polygons; + std::vector 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 +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 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 +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 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 +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_ diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml new file mode 100644 index 0000000000000..2abadbcd2fa84 --- /dev/null +++ b/common/perception_utils/package.xml @@ -0,0 +1,27 @@ + + + + perception_utils + 0.1.0 + The perception_utils package + Takayuki Murooka + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + autoware_auto_perception_msgs + geometry_msgs + libboost-dev + rclcpp + tier4_autoware_utils + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/perception_utils/src/perception_utils.cpp b/common/perception_utils/src/perception_utils.cpp new file mode 100644 index 0000000000000..2d9fa20da8458 --- /dev/null +++ b/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 & 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 diff --git a/common/perception_utils/test/src/test_geometry.cpp b/common/perception_utils/test/src/test_geometry.cpp new file mode 100644 index 0000000000000..d966b8b30aa2d --- /dev/null +++ b/common/perception_utils/test/src/test_geometry.cpp @@ -0,0 +1,111 @@ +// Copyright 2020 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/geometry.hpp" + +#include + +#include + +TEST(geometry, getPose) +{ + using perception_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_auto_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_auto_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_auto_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/perception_utils/test/src/test_perception_utils.cpp b/common/perception_utils/test/src/test_perception_utils.cpp new file mode 100644 index 0000000000000..889f92994b37a --- /dev/null +++ b/common/perception_utils/test/src/test_perception_utils.cpp @@ -0,0 +1,508 @@ +// Copyright 2020 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" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Point3d; + +constexpr double epsilon = 1e-06; + +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; +} + +geometry_msgs::msg::Pose createPose(const double x, const double y, const double yaw) +{ + geometry_msgs::msg::Pose p; + p.position.x = x; + p.position.y = y; + p.position.z = 0.0; + p.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + + return p; +} + +autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassification( + const std::uint8_t label, const double probability) +{ + autoware_auto_perception_msgs::msg::ObjectClassification classification; + classification.label = label; + classification.probability = probability; + + return classification; +} +} // namespace + +TEST(perception_utils, test_getHighestProbLabel) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + using perception_utils::getHighestProbLabel; + + { // empty + std::vector classification; + std::uint8_t label = getHighestProbLabel(classification); + EXPECT_EQ(label, ObjectClassification::UNKNOWN); + } + + { // 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)); + + std::uint8_t label = getHighestProbLabel(classification); + EXPECT_EQ(label, ObjectClassification::TRUCK); + } + + { // labels with the same probability + std::vector classification; + classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); + classification.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); + classification.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); + + std::uint8_t label = getHighestProbLabel(classification); + EXPECT_EQ(label, ObjectClassification::CAR); + } +} + +// NOTE: covariance is not checked +TEST(perception_utils, test_toDetectedObject) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + using perception_utils::toDetectedObject; + + autoware_auto_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(perception_utils, test_toTrackedObject) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + using perception_utils::toTrackedObject; + + autoware_auto_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); +} + +TEST(perception_utils, test_get2dIoU) +{ + using perception_utils::get2dIoU; + const double quart_circle = 0.16237976320958225; + + { // non overlapped + autoware_auto_perception_msgs::msg::DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); + source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + autoware_auto_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_auto_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 + autoware_auto_perception_msgs::msg::DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); + source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + autoware_auto_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_auto_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 + autoware_auto_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_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + autoware_auto_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_auto_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(perception_utils, test_get2dPrecision) +{ + using perception_utils::get2dPrecision; + const double quart_circle = 0.16237976320958225; + + { // non overlapped + autoware_auto_perception_msgs::msg::DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); + source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + autoware_auto_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_auto_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 + autoware_auto_perception_msgs::msg::DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); + source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + autoware_auto_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_auto_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 + autoware_auto_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_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + autoware_auto_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_auto_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(perception_utils, test_get2dRecall) +{ + using perception_utils::get2dRecall; + const double quart_circle = 0.16237976320958225; + + { // non overlapped + autoware_auto_perception_msgs::msg::DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); + source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + autoware_auto_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_auto_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 + autoware_auto_perception_msgs::msg::DetectedObject source_obj; + source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); + source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + autoware_auto_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_auto_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 + autoware_auto_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_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.dimensions.x = 1.0; + source_obj.shape.dimensions.y = 1.0; + + autoware_auto_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_auto_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); + } +}