From 6b950c9acc48fbc7334770865236bbb5ce06b23d Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Tue, 9 Feb 2021 13:35:53 +0900 Subject: [PATCH] Ros2 v0.8.0 object merger (#302) --- .../detection/object_merger/CMakeLists.txt | 49 +++ .../data_association.hpp | 51 +++ .../object_association_merger/node.hpp | 63 +++ .../successive_shortest_path.hpp | 29 ++ .../object_association_merger/utils/utils.hpp | 31 ++ .../object_association_merger.launch.xml | 14 + .../detection/object_merger/package.xml | 27 ++ .../data_association/data_association.cpp | 377 +++++++++++++++++ .../successive_shortest_path.cpp | 399 ++++++++++++++++++ .../src/object_association_merger/main.cpp | 28 ++ .../src/object_association_merger/node.cpp | 98 +++++ .../object_association_merger/utils/utils.cpp | 54 +++ 12 files changed, 1220 insertions(+) create mode 100644 perception/object_recognition/detection/object_merger/CMakeLists.txt create mode 100644 perception/object_recognition/detection/object_merger/include/object_association_merger/data_association.hpp create mode 100644 perception/object_recognition/detection/object_merger/include/object_association_merger/node.hpp create mode 100644 perception/object_recognition/detection/object_merger/include/object_association_merger/successive_shortest_path.hpp create mode 100644 perception/object_recognition/detection/object_merger/include/object_association_merger/utils/utils.hpp create mode 100644 perception/object_recognition/detection/object_merger/launch/object_association_merger.launch.xml create mode 100644 perception/object_recognition/detection/object_merger/package.xml create mode 100644 perception/object_recognition/detection/object_merger/src/object_association_merger/data_association/data_association.cpp create mode 100644 perception/object_recognition/detection/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp create mode 100644 perception/object_recognition/detection/object_merger/src/object_association_merger/main.cpp create mode 100644 perception/object_recognition/detection/object_merger/src/object_association_merger/node.cpp create mode 100644 perception/object_recognition/detection/object_merger/src/object_association_merger/utils/utils.cpp diff --git a/perception/object_recognition/detection/object_merger/CMakeLists.txt b/perception/object_recognition/detection/object_merger/CMakeLists.txt new file mode 100644 index 0000000000000..194a659aaac0f --- /dev/null +++ b/perception/object_recognition/detection/object_merger/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.5) +project(object_merger) + +find_package(PCL REQUIRED COMPONENTS common filters) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter) +endif() + +include_directories( + include +) + +ament_auto_add_executable(object_association_merger_node + src/object_association_merger/utils/utils.cpp + src/object_association_merger/data_association/data_association.cpp + src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp + src/object_association_merger/node.cpp + src/object_association_merger/main.cpp +) +target_compile_definitions(object_association_merger_node PRIVATE ${PCL_DEFINITIONS}) +target_include_directories(object_association_merger_node PRIVATE ${PCL_INCLUDE_DIRS}) +# Unfortunately, this one can't be PRIVATE because only the plain or only the +# keyword (PRIVATE) signature of target_link_libraries can be used for one +# target, not both. The plain signature is already used inside +# `ament_target_dependencies` and possibly rosidl_target_interfaces. +target_link_libraries(object_association_merger_node ${PCL_LIBRARIES}) +target_link_directories(object_association_merger_node PRIVATE ${PCL_LIBRARY_DIRS}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +############# +## Install ## +############# + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/perception/object_recognition/detection/object_merger/include/object_association_merger/data_association.hpp b/perception/object_recognition/detection/object_merger/include/object_association_merger/data_association.hpp new file mode 100644 index 0000000000000..c191f3410b7d0 --- /dev/null +++ b/perception/object_recognition/detection/object_merger/include/object_association_merger/data_association.hpp @@ -0,0 +1,51 @@ +// 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. + +#ifndef OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION_HPP_ +#define OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION_HPP_ + +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include "Eigen/Core" +#include "Eigen/Geometry" + +#include "autoware_perception_msgs/msg/dynamic_object_with_feature_array.hpp" +class DataAssociation +{ +private: + double getDistance( + const geometry_msgs::msg::Point & point0, + const geometry_msgs::msg::Point & point1); + geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); + Eigen::MatrixXi can_assgin_matrix_; + Eigen::MatrixXd max_dist_matrix_; + Eigen::MatrixXd max_area_matrix_; + Eigen::MatrixXd min_area_matrix_; + const double score_threshold_; + +public: + DataAssociation(); + bool assign( + const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, + std::unordered_map & reverse_assignment); + Eigen::MatrixXd calcScoreMatrix( + const autoware_perception_msgs::msg::DynamicObjectWithFeatureArray & object0, + const autoware_perception_msgs::msg::DynamicObjectWithFeatureArray & object1); + virtual ~DataAssociation() {} +}; + +#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION_HPP_ diff --git a/perception/object_recognition/detection/object_merger/include/object_association_merger/node.hpp b/perception/object_recognition/detection/object_merger/include/object_association_merger/node.hpp new file mode 100644 index 0000000000000..f92bd8af164da --- /dev/null +++ b/perception/object_recognition/detection/object_merger/include/object_association_merger/node.hpp @@ -0,0 +1,63 @@ +// 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. + +#ifndef OBJECT_ASSOCIATION_MERGER__NODE_HPP_ +#define OBJECT_ASSOCIATION_MERGER__NODE_HPP_ + +#include + +#include "pcl_conversions/pcl_conversions.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "object_association_merger/data_association.hpp" +#include "autoware_perception_msgs/msg/dynamic_object_with_feature_array.hpp" +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" +#include "rclcpp/rclcpp.hpp" + +namespace object_association +{ +class ObjectAssociationMergerNode : public rclcpp::Node +{ +public: + ObjectAssociationMergerNode(); + ~ObjectAssociationMergerNode() = default; + +private: + void objectsCallback( + const autoware_perception_msgs::msg::DynamicObjectWithFeatureArray::ConstSharedPtr & + input_object0_msg, + const autoware_perception_msgs::msg::DynamicObjectWithFeatureArray::ConstSharedPtr & + input_object1_msg); + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + rclcpp::Publisher::SharedPtr + merged_object_pub_; + message_filters::Subscriber + object0_sub_; + message_filters::Subscriber + object1_sub_; + typedef message_filters::sync_policies::ApproximateTime< + autoware_perception_msgs::msg::DynamicObjectWithFeatureArray, + autoware_perception_msgs::msg::DynamicObjectWithFeatureArray> + SyncPolicy; + typedef message_filters::Synchronizer Sync; + Sync sync_; + DataAssociation data_association_; +}; +} // namespace object_association + +#endif // OBJECT_ASSOCIATION_MERGER__NODE_HPP_ diff --git a/perception/object_recognition/detection/object_merger/include/object_association_merger/successive_shortest_path.hpp b/perception/object_recognition/detection/object_merger/include/object_association_merger/successive_shortest_path.hpp new file mode 100644 index 0000000000000..7fdeb2f0fe729 --- /dev/null +++ b/perception/object_recognition/detection/object_merger/include/object_association_merger/successive_shortest_path.hpp @@ -0,0 +1,29 @@ +// 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. + +#ifndef OBJECT_ASSOCIATION_MERGER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#define OBJECT_ASSOCIATION_MERGER__SUCCESSIVE_SHORTEST_PATH_HPP_ + +#include +#include + +namespace assignment_problem +{ +// See IMPORTANT NOTE at the top of the file. +void MaximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment); +} // namespace assignment_problem + +#endif // OBJECT_ASSOCIATION_MERGER__SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/object_recognition/detection/object_merger/include/object_association_merger/utils/utils.hpp b/perception/object_recognition/detection/object_merger/include/object_association_merger/utils/utils.hpp new file mode 100644 index 0000000000000..12172896518f6 --- /dev/null +++ b/perception/object_recognition/detection/object_merger/include/object_association_merger/utils/utils.hpp @@ -0,0 +1,31 @@ +// 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. + +#ifndef OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ +#define OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ + +#include +#include "autoware_perception_msgs/msg/shape.hpp" +#include "geometry_msgs/msg/polygon.hpp" +#include "geometry_msgs/msg/vector3.hpp" + +namespace utils +{ +double getPolygonArea(const geometry_msgs::msg::Polygon & footprint); +double getRectangleArea(const geometry_msgs::msg::Vector3 & dimensions); +double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions); +double getArea(const autoware_perception_msgs::msg::Shape & shape); +} // namespace utils + +#endif // OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ diff --git a/perception/object_recognition/detection/object_merger/launch/object_association_merger.launch.xml b/perception/object_recognition/detection/object_merger/launch/object_association_merger.launch.xml new file mode 100644 index 0000000000000..92730f8070848 --- /dev/null +++ b/perception/object_recognition/detection/object_merger/launch/object_association_merger.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/perception/object_recognition/detection/object_merger/package.xml b/perception/object_recognition/detection/object_merger/package.xml new file mode 100644 index 0000000000000..dc2c092a90133 --- /dev/null +++ b/perception/object_recognition/detection/object_merger/package.xml @@ -0,0 +1,27 @@ + + + object_merger + 0.1.0 + The object_merger package + + Yukihiro Saito + Apache 2.0 + + ament_cmake_auto + autoware_perception_msgs + message_filters + pcl_conversions + libpcl-all-dev + rclcpp + sensor_msgs + tf2 + tf2_ros + tf2_sensor_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/perception/object_recognition/detection/object_merger/src/object_association_merger/data_association/data_association.cpp b/perception/object_recognition/detection/object_merger/src/object_association_merger/data_association/data_association.cpp new file mode 100644 index 0000000000000..64f3bfc43f81c --- /dev/null +++ b/perception/object_recognition/detection/object_merger/src/object_association_merger/data_association/data_association.cpp @@ -0,0 +1,377 @@ +// 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 +#include +#include + +#include "object_association_merger/data_association.hpp" +#include "object_association_merger/successive_shortest_path.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "object_association_merger/utils/utils.hpp" + +DataAssociation::DataAssociation() +: score_threshold_(0.1) +{ + can_assgin_matrix_ = Eigen::MatrixXi::Identity(20, 20); + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::UNKNOWN, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 1; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::TRUCK) = 1; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::BUS) = 1; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::CAR) = 1; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::BUS) = 1; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::CAR) = 1; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::TRUCK) = 1; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::BICYCLE, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::BICYCLE, + autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 1; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::MOTORBIKE, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::MOTORBIKE, + autoware_perception_msgs::msg::Semantic::BICYCLE) = 1; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::PEDESTRIAN, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; + can_assgin_matrix_( + autoware_perception_msgs::msg::Semantic::ANIMAL, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0; + max_dist_matrix_ = Eigen::MatrixXd::Constant(20, 20, 1.0); + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 4.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::CAR) = 3.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::TRUCK) = 3.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::BUS) = 3.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 3.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::CAR) = 3.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::TRUCK) = 3.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::BUS) = 3.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 3.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::CAR) = 3.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::TRUCK) = 3.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::BUS) = 3.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::BICYCLE, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::BICYCLE, + autoware_perception_msgs::msg::Semantic::BICYCLE) = 2.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::BICYCLE, + autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 2.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::MOTORBIKE, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::MOTORBIKE, + autoware_perception_msgs::msg::Semantic::BICYCLE) = 2.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::MOTORBIKE, + autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 2.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::PEDESTRIAN, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::PEDESTRIAN, + autoware_perception_msgs::msg::Semantic::PEDESTRIAN) = 2.0; + max_dist_matrix_( + autoware_perception_msgs::msg::Semantic::ANIMAL, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0; + max_area_matrix_ = Eigen::MatrixXd::Constant(20, 20, /* large number */ 10000.0); + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::UNKNOWN, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 5.0 * 5.0; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.2 * 5.5; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::CAR) = 2.2 * 5.5; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::TRUCK) = 2.5 * 7.9; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::BUS) = 2.7 * 12.0; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.5 * 7.9; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::CAR) = 2.2 * 5.5; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::TRUCK) = 2.5 * 7.9; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::BUS) = 2.7 * 12.0; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.7 * 12.0; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::CAR) = 2.2 * 5.5; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::TRUCK) = 2.5 * 7.9; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::BUS) = 2.7 * 12.0; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::BICYCLE, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.5; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::BICYCLE, + autoware_perception_msgs::msg::Semantic::BICYCLE) = 2.5; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::BICYCLE, + autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 3.0; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::MOTORBIKE, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 3.0; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::MOTORBIKE, + autoware_perception_msgs::msg::Semantic::BICYCLE) = 2.5; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::MOTORBIKE, + autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 3.0; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::PEDESTRIAN, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::PEDESTRIAN, + autoware_perception_msgs::msg::Semantic::PEDESTRIAN) = 2.0; + max_area_matrix_( + autoware_perception_msgs::msg::Semantic::ANIMAL, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0; + min_area_matrix_ = Eigen::MatrixXd::Constant(20, 20, /* small number */ 0.0); + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 1.2 * 3.0; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::CAR) = 1.2 * 3.0; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::TRUCK) = 1.5 * 4.0; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::CAR, + autoware_perception_msgs::msg::Semantic::BUS) = 2.0 * 5.0; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 1.5 * 4.0; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::CAR) = 1.2 * 3.0; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::TRUCK) = 1.5 * 4.0; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::TRUCK, + autoware_perception_msgs::msg::Semantic::BUS) = 2.0 * 5.0; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0 * 5.0; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::CAR) = 1.2 * 3.0; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::TRUCK) = 1.5 * 4.0; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::BUS, + autoware_perception_msgs::msg::Semantic::BUS) = 2.0 * 5.0; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::BICYCLE, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0.001; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::BICYCLE, + autoware_perception_msgs::msg::Semantic::BICYCLE) = 0.001; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::BICYCLE, + autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 0.001; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::MOTORBIKE, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0.001; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::MOTORBIKE, + autoware_perception_msgs::msg::Semantic::BICYCLE) = 0.001; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::MOTORBIKE, + autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 0.001; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::PEDESTRIAN, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0.001; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::PEDESTRIAN, + autoware_perception_msgs::msg::Semantic::PEDESTRIAN) = 0.001; + min_area_matrix_( + autoware_perception_msgs::msg::Semantic::ANIMAL, + autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0.5; +} + +bool DataAssociation::assign( + const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, + std::unordered_map & reverse_assignment) +{ + std::vector> score(src.rows()); + for (int row = 0; row < src.rows(); ++row) { + score.at(row).resize(src.cols()); + for (int col = 0; col < src.cols(); ++col) { + score.at(row).at(col) = src(row, col); + } + } + // Solve + assignment_problem::MaximizeLinearAssignment(score, &direct_assignment, &reverse_assignment); + + for (auto itr = direct_assignment.begin(); itr != direct_assignment.end(); ) { + if (src(itr->first, itr->second) < score_threshold_) { + itr = direct_assignment.erase(itr); + continue; + } else { + ++itr; + } + } + for (auto itr = reverse_assignment.begin(); itr != reverse_assignment.end(); ) { + if (src(itr->second, itr->first) < score_threshold_) { + itr = reverse_assignment.erase(itr); + continue; + } else { + ++itr; + } + } + return true; +} + +Eigen::MatrixXd DataAssociation::calcScoreMatrix( + const autoware_perception_msgs::msg::DynamicObjectWithFeatureArray & object0, + const autoware_perception_msgs::msg::DynamicObjectWithFeatureArray & object1) +{ + Eigen::MatrixXd score_matrix = + Eigen::MatrixXd::Zero(object1.feature_objects.size(), object0.feature_objects.size()); + for (size_t object1_idx = 0; object1_idx < object1.feature_objects.size(); ++object1_idx) { + for (size_t object0_idx = 0; object0_idx < object0.feature_objects.size(); ++object0_idx) { + double score = 0.0; + if (can_assgin_matrix_( + object1.feature_objects.at(object1_idx).object.semantic.type, + object0.feature_objects.at(object0_idx).object.semantic.type)) + { + const double max_dist = max_dist_matrix_( + object1.feature_objects.at(object1_idx).object.semantic.type, + object0.feature_objects.at(object0_idx).object.semantic.type); + const double max_area = + max_area_matrix_( + object1.feature_objects.at(object1_idx).object.semantic.type, + object0.feature_objects.at(object0_idx).object.semantic.type); + const double min_area = + min_area_matrix_( + object1.feature_objects.at(object1_idx).object.semantic.type, + object0.feature_objects.at(object0_idx).object.semantic.type); + const double dist = getDistance( + object0.feature_objects.at(object0_idx).object.state.pose_covariance.pose.position, + object1.feature_objects.at(object1_idx).object.state.pose_covariance.pose.position); + const double area0 = utils::getArea(object0.feature_objects.at(object0_idx).object.shape); + const double area1 = utils::getArea(object1.feature_objects.at(object1_idx).object.shape); + score = (max_dist - std::min(dist, max_dist)) / max_dist; + if (max_dist < dist) {score = 0.0;} + if (area0 < min_area || max_area < area0) {score = 0.0;} + if (area1 < min_area || max_area < area1) {score = 0.0;} + } + score_matrix(object1_idx, object0_idx) = score; + } + } + return score_matrix; +} + +double DataAssociation::getDistance( + const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1) +{ + const double diff_x = point1.x - point0.x; + const double diff_y = point1.y - point0.y; + // const double diff_z = point1.z - point0.z; + return std::sqrt(diff_x * diff_x + diff_y * diff_y); +} + +geometry_msgs::msg::Point DataAssociation::getCentroid( + const sensor_msgs::msg::PointCloud2 & pointcloud) +{ + geometry_msgs::msg::Point centroid; + centroid.x = 0; + centroid.y = 0; + centroid.z = 0; + for (sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"), + iter_y(pointcloud, "y"), iter_z(pointcloud, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) + { + centroid.x += *iter_x; + centroid.y += *iter_y; + centroid.z += *iter_z; + } + centroid.x = + centroid.x / (static_cast(pointcloud.height) * static_cast(pointcloud.width)); + centroid.y = + centroid.y / (static_cast(pointcloud.height) * static_cast(pointcloud.width)); + centroid.z = + centroid.z / (static_cast(pointcloud.height) * static_cast(pointcloud.width)); + return centroid; +} diff --git a/perception/object_recognition/detection/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/object_recognition/detection/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp new file mode 100644 index 0000000000000..4b754435db4f3 --- /dev/null +++ b/perception/object_recognition/detection/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp @@ -0,0 +1,399 @@ +// 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 "object_association_merger/successive_shortest_path.hpp" + +#include +#include +#include +#include +#include +#include +#include + +// #include +// #include +// #include +// #include +// #include + +namespace assignment_problem +{ +struct ResidualEdge +{ + // Destination node + const int dst; + int capacity; + const double cost; + int flow; + // Access to the reverse edge by adjacency_list.at(dst).at(reverse) + const int reverse; + + // ResidualEdge() + // : dst(0), capacity(0), cost(0), flow(0), reverse(0) {} + + ResidualEdge(int dst, int capacity, double cost, int flow, int reverse) + : dst(dst), capacity(capacity), cost(cost), flow(flow), reverse(reverse) + { + } +}; + +void MaximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) +{ + // NOTE: Need to set as default arguments + bool sparse_cost = true; + // bool sparse_cost = false; + + // Hyperparameters + // double MAX_COST = 6; + const double MAX_COST = 10; + const double INF_DIST = 10000000; + const double EPS = 1e-5; + + // When there is no agents or no tasks, terminate + if (cost.size() == 0 || cost.at(0).size() == 0) { + return; + } + + // Construct a bipartite graph from the cost matrix + int n_agents = cost.size(); + int n_tasks = cost.at(0).size(); + + int n_dummys; + if (sparse_cost) { + n_dummys = n_agents; + } else { + n_dummys = 0; + } + + int source = 0; + int sink = n_agents + n_tasks + 1; + int n_nodes = n_agents + n_tasks + n_dummys + 2; + + // // Print cost matrix + // std::cout << std::endl; + // for (int agent = 0; agent < n_agents; agent++) + // { + // for (int task = 0; task < n_tasks; task++) + // { + // std::cout << cost.at(agent).at(task) << " "; + // } + // std::cout << std::endl; + // } + + // std::chrono::system_clock::time_point start_time, end_time; + // start_time = std::chrono::system_clock::now(); + + // Adjacency list of residual graph (indice: nodes) + // - 0: source node + // - {1, ..., n_agents}: agent nodes + // - {n_agents+1, ..., n_agents+n_tasks}: task nodes + // - n_agents+n_tasks+1: sink node + // - {n_agents+n_tasks+2, ..., + // n_agents+n_tasks+1+n_agents}: dummy node (when sparse_cost is true) + std::vector> adjacency_list(n_nodes); + + // Reserve memory + for (int v = 0; v < n_nodes; ++v) { + if (v == source) { + // Source + adjacency_list.at(v).reserve(n_agents); + } else if (v <= n_agents) { + // Agents + adjacency_list.at(v).reserve(n_tasks + 1 + 1); + } else if (v <= n_agents + n_tasks) { + // Tasks + adjacency_list.at(v).reserve(n_agents + 1); + } else if (v == sink) { + // Sink + adjacency_list.at(v).reserve(n_tasks + n_dummys); + } else { + // Dummys + adjacency_list.at(v).reserve(2); + } + } + + // Add edges form source + for (int agent = 0; agent < n_agents; ++agent) { + // From source to agent + adjacency_list.at(source).emplace_back(agent + 1, 1, 0, 0, adjacency_list.at(agent + 1).size()); + // From agent to source + adjacency_list.at(agent + 1).emplace_back( + source, 0, 0, 0, adjacency_list.at(source).size() - 1); + } + + // Add edges from agents + for (int agent = 0; agent < n_agents; ++agent) { + for (int task = 0; task < n_tasks; ++task) { + if (!sparse_cost || cost.at(agent).at(task) > EPS) { + // From agent to task + adjacency_list.at(agent + 1).emplace_back( + task + n_agents + 1, 1, MAX_COST - cost.at(agent).at(task), 0, + adjacency_list.at(task + n_agents + 1).size()); + + // From task to agent + adjacency_list.at(task + n_agents + 1) + .emplace_back( + agent + 1, 0, cost.at(agent).at(task) - MAX_COST, 0, + adjacency_list.at(agent + 1).size() - 1); + } + } + } + + // Add edges form tasks + for (int task = 0; task < n_tasks; ++task) { + // From task to sink + adjacency_list.at(task + n_agents + 1) + .emplace_back(sink, 1, 0, 0, adjacency_list.at(sink).size()); + + // From sink to task + adjacency_list.at(sink).emplace_back( + task + n_agents + 1, 0, 0, 0, adjacency_list.at(task + n_agents + 1).size() - 1); + } + + // Add edges from dummy + if (sparse_cost) { + for (int agent = 0; agent < n_agents; ++agent) { + // From agent to dummy + adjacency_list.at(agent + 1).emplace_back( + agent + n_agents + n_tasks + 2, 1, MAX_COST, 0, + adjacency_list.at(agent + n_agents + n_tasks + 2).size()); + + // From dummy to agent + adjacency_list.at(agent + n_agents + n_tasks + 2) + .emplace_back(agent + 1, 0, -MAX_COST, 0, adjacency_list.at(agent + 1).size() - 1); + + // From dummy to sink + adjacency_list.at(agent + n_agents + n_tasks + 2) + .emplace_back(sink, 1, 0, 0, adjacency_list.at(sink).size()); + + // From sink to dummy + adjacency_list.at(sink).emplace_back( + agent + n_agents + n_tasks + 2, 0, 0, 0, + adjacency_list.at(agent + n_agents + n_tasks + 2).size() - 1); + } + } + + // // Print adjacency list + // std::cout << std::endl; + // for (int v = 0; v < n_nodes; v++) + // { + // std::cout << v << ": "; + // for (auto it_incident_edge = adjacency_list.at(v).cbegin(); + // it_incident_edge != adjacency_list.at(v).cend(); + // it_incident_edge++) + // { + // std::cout << "(" << it_incident_edge->first << ", " << + // it_incident_edge->second.cost << ")"; + // } + // std::cout << std::endl; + // } + + // end_time = std::chrono::system_clock::now(); + // double time = static_cast( + // std::chrono::duration_cast( + // end_time - start_time).count() / 1000.0); + // std::cout << " " << time << " "; + + // Maximum flow value + const int max_flow = std::min(n_agents, n_tasks); + + // Feasible potentials + std::vector potentials(n_nodes, 0); + + // Shortest path lengths + std::vector dists(n_nodes, INF_DIST); + + // Whether previously visited the node or not + std::vector is_visited(n_nodes, false); + + // Parent node () + std::vector> prevs(n_nodes); + + for (int i = 0; i < max_flow; ++i) { + // Initialize priority queue () + std::priority_queue< + std::pair, std::vector>, + std::greater>> + pqueue; + + // Reset all trajectory states + if (i > 0) { + std::fill(dists.begin(), dists.end(), INF_DIST); + std::fill(is_visited.begin(), is_visited.end(), false); + } + + // Start trajectory from the source node + pqueue.push(std::make_pair(0, source)); + dists.at(source) = 0; + + while (!pqueue.empty()) { + // Get the next element + std::pair cur_elem = pqueue.top(); + // std::cout << "[pop]: (" << cur_elem.first << ", " << cur_elem.second << ")" << std::endl; + pqueue.pop(); + + double cur_node_dist = cur_elem.first; + int cur_node = cur_elem.second; + + // If already visited node, skip and continue + if (is_visited.at(cur_node)) { + continue; + } + assert(cur_node_dist == dists.at(cur_node)); + + // Mark as visited + is_visited.at(cur_node) = true; + // Update potential + potentials.at(cur_node) += cur_node_dist; + + // When reached to the sink node, terminate. + if (cur_node == sink) { + break; + } + + // Loop over the incident nodes(/edges) + for (auto it_incident_edge = adjacency_list.at(cur_node).cbegin(); + it_incident_edge != adjacency_list.at(cur_node).cend(); it_incident_edge++) + { + // If the node is not visited and have capacity to increase flow, visit. + if (!is_visited.at(it_incident_edge->dst) && it_incident_edge->capacity > 0) { + // Calculate reduced cost + double reduced_cost = + it_incident_edge->cost + potentials.at(cur_node) - potentials.at(it_incident_edge->dst); + assert(reduced_cost >= 0); + if (dists.at(it_incident_edge->dst) > reduced_cost) { + dists.at(it_incident_edge->dst) = reduced_cost; + prevs.at(it_incident_edge->dst) = + std::make_pair(cur_node, it_incident_edge - adjacency_list.at(cur_node).cbegin()); + // std::cout << "[push]: (" << reduced_cost << ", " << next_v << ")" << std::endl; + pqueue.push(std::make_pair(reduced_cost, it_incident_edge->dst)); + } + } + } + } + + // Shortest path length to sink is greater than MAX_COST, + // which means no non-dummy routes left ,terminate + if (potentials.at(sink) >= MAX_COST) { + break; + } + + // Update potentials of unvisited nodes + for (int v = 0; v < n_nodes; ++v) { + if (!is_visited.at(v)) { + potentials.at(v) += dists.at(sink); + } + } + // //Print potentials + // for (int v = 0; v < n_nodes; ++v) + // { + // std::cout << potentials.at(v) << ", "; + // } + // std::cout << std::endl; + + // Increase/decrease flow and capacity along the shortest path from the source to the sink + int v = sink; + int prev_v; + while (v != source) { + ResidualEdge & e_forward = adjacency_list.at(prevs.at(v).first).at(prevs.at(v).second); + assert(e_forward.dst == v); + ResidualEdge & e_backward = adjacency_list.at(v).at(e_forward.reverse); + prev_v = e_backward.dst; + + if (e_backward.flow == 0) { + // Increase flow + // State A + assert(e_forward.capacity == 1); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 0); + assert(e_backward.flow == 0); + + e_forward.capacity -= 1; + e_forward.flow += 1; + e_backward.capacity += 1; + + // State B + assert(e_forward.capacity == 0); + assert(e_forward.flow == 1); + assert(e_backward.capacity == 1); + assert(e_backward.flow == 0); + } else { + // Decrease flow + // State B + assert(e_forward.capacity == 1); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 0); + assert(e_backward.flow == 1); + + e_forward.capacity -= 1; + e_backward.capacity += 1; + e_backward.flow -= 1; + + // State A + assert(e_forward.capacity == 0); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 1); + assert(e_backward.flow == 0); + } + + v = prev_v; + } + +#ifndef NDEBUG + // Check if the potentials are feasible potentials + for (int v = 0; v < n_nodes; ++v) { + for (auto it_incident_edge = adjacency_list.at(v).cbegin(); + it_incident_edge != adjacency_list.at(v).cend(); ++it_incident_edge) + { + if (it_incident_edge->capacity > 0) { + double reduced_cost = + it_incident_edge->cost + potentials.at(v) - potentials.at(it_incident_edge->dst); + assert(reduced_cost >= 0); + } + } + } +#endif + } + + // Output + for (int agent = 0; agent < n_agents; ++agent) { + for (auto it_incident_edge = adjacency_list.at(agent + 1).cbegin(); + it_incident_edge != adjacency_list.at(agent + 1).cend(); ++it_incident_edge) + { + int task = it_incident_edge->dst - (n_agents + 1); + + // If the flow value is 1 and task is not dummy, assign the task to the agent. + if (it_incident_edge->flow == 1 && 0 <= task && task < n_tasks) { + (*direct_assignment)[agent] = task; + (*reverse_assignment)[task] = agent; + break; + } + } + } + +#ifndef NDEBUG + // Check if the result is valid assignment + for (int agent = 0; agent < n_agents; ++agent) { + if (direct_assignment->find(agent) != direct_assignment->cend()) { + int task = (*direct_assignment).at(agent); + assert(direct_assignment->at(agent) == task); + assert(reverse_assignment->at(task) == agent); + } + } +#endif +} +} // namespace assignment_problem diff --git a/perception/object_recognition/detection/object_merger/src/object_association_merger/main.cpp b/perception/object_recognition/detection/object_merger/src/object_association_merger/main.cpp new file mode 100644 index 0000000000000..2b0c4a31372b3 --- /dev/null +++ b/perception/object_recognition/detection/object_merger/src/object_association_merger/main.cpp @@ -0,0 +1,28 @@ +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "object_association_merger/node.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} diff --git a/perception/object_recognition/detection/object_merger/src/object_association_merger/node.cpp b/perception/object_recognition/detection/object_merger/src/object_association_merger/node.cpp new file mode 100644 index 0000000000000..a3da49c35cf89 --- /dev/null +++ b/perception/object_recognition/detection/object_merger/src/object_association_merger/node.cpp @@ -0,0 +1,98 @@ +// 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 +#include + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "tf2/LinearMath/Transform.h" +#include "tf2/convert.h" +#include "tf2/transform_datatypes.h" +// #include "tf2_sensor_msgs/msg/tf2_sensor_msgs.hpp" +#include "object_association_merger/node.hpp" +#define EIGEN_MPL2_ONLY +#include "Eigen/Core" +#include "Eigen/Geometry" + +namespace object_association +{ +ObjectAssociationMergerNode::ObjectAssociationMergerNode() +: rclcpp::Node("cluster_data_association_node"), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_), + object0_sub_(this, "input/object0", rclcpp::QoS{1}.get_rmw_qos_profile()), + object1_sub_(this, "input/object1", rclcpp::QoS{1}.get_rmw_qos_profile()), + sync_(SyncPolicy(10), object0_sub_, object1_sub_) +{ + using std::placeholders::_1; + using std::placeholders::_2; + sync_.registerCallback(std::bind(&ObjectAssociationMergerNode::objectsCallback, this, _1, _2)); + + merged_object_pub_ = + create_publisher( + "output/object", rclcpp::QoS{10}); +} + +void ObjectAssociationMergerNode::objectsCallback( + const autoware_perception_msgs::msg::DynamicObjectWithFeatureArray::ConstSharedPtr & + input_object0_msg, + const autoware_perception_msgs::msg::DynamicObjectWithFeatureArray::ConstSharedPtr & + input_object1_msg) +{ + // Guard + if (merged_object_pub_->get_subscription_count() < 1) {return;} + + // build output msg + autoware_perception_msgs::msg::DynamicObjectWithFeatureArray output_msg; + output_msg.header = input_object0_msg->header; + + /* global nearest neighbor */ + std::unordered_map direct_assignment; + std::unordered_map reverse_assignment; + Eigen::MatrixXd score_matrix = + data_association_.calcScoreMatrix(*input_object1_msg, *input_object0_msg); + data_association_.assign(score_matrix, direct_assignment, reverse_assignment); + for (size_t object0_idx = 0; object0_idx < input_object0_msg->feature_objects.size(); + ++object0_idx) + { + if (direct_assignment.find(object0_idx) != direct_assignment.end()) { // found + // The one with the higher score will be hired. + if ( + input_object1_msg->feature_objects.at(direct_assignment.at(object0_idx)) + .object.semantic.confidence < + input_object0_msg->feature_objects.at(object0_idx).object.semantic.confidence) + { + output_msg.feature_objects.push_back(input_object0_msg->feature_objects.at(object0_idx)); + } else { + output_msg.feature_objects.push_back( + input_object1_msg->feature_objects.at(direct_assignment.at(object0_idx))); + } + } else { // not found + output_msg.feature_objects.push_back(input_object0_msg->feature_objects.at(object0_idx)); + } + } + for (size_t object1_idx = 0; object1_idx < input_object1_msg->feature_objects.size(); + ++object1_idx) + { + if (reverse_assignment.find(object1_idx) != reverse_assignment.end()) { // found + } else { // not found + output_msg.feature_objects.push_back(input_object1_msg->feature_objects.at(object1_idx)); + } + } + + // publish output msg + merged_object_pub_->publish(output_msg); +} +} // namespace object_association diff --git a/perception/object_recognition/detection/object_merger/src/object_association_merger/utils/utils.cpp b/perception/object_recognition/detection/object_merger/src/object_association_merger/utils/utils.cpp new file mode 100644 index 0000000000000..61fe722e7a78e --- /dev/null +++ b/perception/object_recognition/detection/object_merger/src/object_association_merger/utils/utils.cpp @@ -0,0 +1,54 @@ +// 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 "object_association_merger/utils/utils.hpp" + +namespace utils +{ +double getArea(const autoware_perception_msgs::msg::Shape & shape) +{ + double area = 0.0; + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + area = getRectangleArea(shape.dimensions); + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { + area = getCircleArea(shape.dimensions); + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { + area = getPolygonArea(shape.footprint); + } + return area; +} + +double getPolygonArea(const geometry_msgs::msg::Polygon & footprint) +{ + double area = 0.0; + + for (int i = 0; i < static_cast(footprint.points.size()); ++i) { + int j = (i + 1) % static_cast(footprint.points.size()); + area += 0.5 * (footprint.points.at(i).x * footprint.points.at(j).y - + footprint.points.at(j).x * footprint.points.at(i).y); + } + + return area; +} + +double getRectangleArea(const geometry_msgs::msg::Vector3 & dimensions) +{ + return static_cast(dimensions.x * dimensions.y); +} + +double getCircleArea(const geometry_msgs::msg::Vector3 & dimensions) +{ + return static_cast((dimensions.x / 2.0) * (dimensions.x / 2.0) * M_PI); +} +} // namespace utils