From 02a6a754dc63b9a4518a1befe87c9e7dd3c271b0 Mon Sep 17 00:00:00 2001 From: Yohei Mishina <66298900+YoheiMishina@users.noreply.github.com> Date: Sat, 4 Dec 2021 13:02:30 +0900 Subject: [PATCH] fate: add object merger package (#107) * Ros2 v0.8.0 object merger (#302) * Fix typo in perception module (#440) * add use_sim-time option (#454) * Unify Apache-2.0 license name (#1242) * Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 * Perception components (#1368) * [bev_optical_flow]: component node Signed-off-by: wep21 * [object_merger]: component node Signed-off-by: wep21 * [object_range_splitter]: component node Signed-off-by: wep21 * [shape_estimation]: component node Signed-off-by: wep21 * [map_based_prediction]: component node Signed-off-by: wep21 * [naive_path_prediction]: component node Signed-off-by: wep21 * [roi_image_saver]: component node Signed-off-by: wep21 * [lidar_apollo_instance_segmentation]: component node Signed-off-by: wep21 * [object_flow_fusion]: component node Signed-off-by: wep21 * [traffic_light_map_based_detector]: component node Signed-off-by: wep21 * [dynamic_object_visualization]: component node Signed-off-by: wep21 * Fix typo Signed-off-by: wep21 * Feature/porting mussp with vendor (#1323) * change to mussp (#1180) * change to mussp * add license * add license * change to solver plugin * fix bug * cosmetic change * modified to follow ROS2 coding style * Use mussp vendor Signed-off-by: wep21 * Fix dependencies in package.xml Signed-off-by: wep21 * Add mussp_vendor into build_depends.repos Signed-off-by: wep21 * Fix lint Signed-off-by: wep21 * Fix typo Signed-off-by: wep21 * Fix typo Signed-off-by: wep21 * Fix lint Signed-off-by: wep21 * Use fork instead of vendor package Signed-off-by: wep21 * Cleanup CMakeLists.txt Signed-off-by: wep21 * Remove comment Signed-off-by: wep21 * Sort package dependencies in alphabetical order Signed-off-by: wep21 Co-authored-by: Yukihiro Saito Co-authored-by: Keisuke Shima * Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA * add sort-package-xml hook in pre-commit (#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Detection by tracker (#1910) * initial commit * backup * apply format * cosmetic change * implement divided under segmenterd clusters * cosmetic change * bug fix * bug fix * bug fix * modify launch * add debug and bug fix * bug fix * bug fix * add no found tracked object * modify parameters and cmake * bug fix * remove debug info * add readme * modify clustering launch * run pre-commit * cosmetic change * cosmetic change * cosmetic change * apply markdownlint * modify launch * modify for cpplint * modify qos * change int to size_T * bug fix * change perception qos * Update perception/object_recognition/detection/detection_by_tracker/package.xml Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * cosmetic change * cosmetic change * fix launch * Update perception/object_recognition/detection/detection_by_tracker/src/utils.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * modify header include order * change include order * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * change to std::optional * cosmetic change * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * bug fix * modify readme Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * Change formatter to clang-format and black (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake * port object merger (#524) * support for autoware_auto_perception_msgs * use existence_probability * add README of object_merger (#621) * add README of object_merger * add comment about score * fix typo * Update README.md Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: tkimura4 Co-authored-by: Yukihiro Saito Co-authored-by: Keisuke Shima Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Hiroki OTA Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Yusuke Muramatsu --- perception/object_merger/CMakeLists.txt | 47 +++ perception/object_merger/README.md | 72 ++++ .../data_association.hpp | 52 +++ .../object_association_merger/node.hpp | 60 +++ .../successive_shortest_path.hpp | 29 ++ .../object_association_merger/utils/utils.hpp | 32 ++ .../object_association_merger.launch.xml | 14 + perception/object_merger/package.xml | 28 ++ .../data_association/data_association.cpp | 368 ++++++++++++++++ .../successive_shortest_path.cpp | 396 ++++++++++++++++++ .../src/object_association_merger/node.cpp | 95 +++++ .../object_association_merger/utils/utils.cpp | 54 +++ 12 files changed, 1247 insertions(+) create mode 100644 perception/object_merger/CMakeLists.txt create mode 100644 perception/object_merger/README.md create mode 100644 perception/object_merger/include/object_association_merger/data_association.hpp create mode 100644 perception/object_merger/include/object_association_merger/node.hpp create mode 100644 perception/object_merger/include/object_association_merger/successive_shortest_path.hpp create mode 100644 perception/object_merger/include/object_association_merger/utils/utils.hpp create mode 100644 perception/object_merger/launch/object_association_merger.launch.xml create mode 100644 perception/object_merger/package.xml create mode 100644 perception/object_merger/src/object_association_merger/data_association/data_association.cpp create mode 100644 perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp create mode 100644 perception/object_merger/src/object_association_merger/node.cpp create mode 100644 perception/object_merger/src/object_association_merger/utils/utils.cpp diff --git a/perception/object_merger/CMakeLists.txt b/perception/object_merger/CMakeLists.txt new file mode 100644 index 0000000000000..e8b1b78d3e071 --- /dev/null +++ b/perception/object_merger/CMakeLists.txt @@ -0,0 +1,47 @@ +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) +endif() + +include_directories( + include +) + +ament_auto_add_library(object_association_merger SHARED + 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 +) + +target_link_libraries(object_association_merger ${PCL_LIBRARIES}) + +rclcpp_components_register_node(object_association_merger + PLUGIN "object_association::ObjectAssociationMergerNode" + EXECUTABLE object_association_merger_node +) + +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_merger/README.md b/perception/object_merger/README.md new file mode 100644 index 0000000000000..6f52416df75e2 --- /dev/null +++ b/perception/object_merger/README.md @@ -0,0 +1,72 @@ +# object_merger + +## Purpose + +object_merger is a package for merging detected objects from two methods by data association. + +## Inner-workings / Algorithms + +The successive shortest path algorithm is used to solve the data association problem (the minimum-cost flow problem). The cost is calculated by the distance between two objects and gate functions are applied to reset cost, s.t. the maximum distance, the maximum area and the minimum area. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| --------------- | ----------------------------------------------------- | ----------------- | +| `input/object0` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection objects | +| `input/object1` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection objects | + +### Output + +| Name | Type | Description | +| --------------- | ----------------------------------------------------- | ---------------- | +| `output/object` | `autoware_auto_perception_msgs::msg::DetectedObjects` | modified Objects | + +## Parameters + +No Parameters. + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + +Data association algorithm was the same as that of multi_object_tracker, but the algorithm of multi_object_tracker was already updated. diff --git a/perception/object_merger/include/object_association_merger/data_association.hpp b/perception/object_merger/include/object_association_merger/data_association.hpp new file mode 100644 index 0000000000000..869d52949e4af --- /dev/null +++ b/perception/object_merger/include/object_association_merger/data_association.hpp @@ -0,0 +1,52 @@ +// 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 +#include + +#include +#include +#include +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_assign_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_auto_perception_msgs::msg::DetectedObjects & object0, + const autoware_auto_perception_msgs::msg::DetectedObjects & object1); + virtual ~DataAssociation() {} +}; + +#endif // OBJECT_ASSOCIATION_MERGER__DATA_ASSOCIATION_HPP_ diff --git a/perception/object_merger/include/object_association_merger/node.hpp b/perception/object_merger/include/object_association_merger/node.hpp new file mode 100644 index 0000000000000..0558659c2d28b --- /dev/null +++ b/perception/object_merger/include/object_association_merger/node.hpp @@ -0,0 +1,60 @@ +// 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 + +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace object_association +{ +class ObjectAssociationMergerNode : public rclcpp::Node +{ +public: + explicit ObjectAssociationMergerNode(const rclcpp::NodeOptions & node_options); + +private: + void objectsCallback( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_object0_msg, + const autoware_auto_perception_msgs::msg::DetectedObjects::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_auto_perception_msgs::msg::DetectedObjects, + autoware_auto_perception_msgs::msg::DetectedObjects> + 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_merger/include/object_association_merger/successive_shortest_path.hpp b/perception/object_merger/include/object_association_merger/successive_shortest_path.hpp new file mode 100644 index 0000000000000..7fdeb2f0fe729 --- /dev/null +++ b/perception/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_merger/include/object_association_merger/utils/utils.hpp b/perception/object_merger/include/object_association_merger/utils/utils.hpp new file mode 100644 index 0000000000000..c686244d1dd4b --- /dev/null +++ b/perception/object_merger/include/object_association_merger/utils/utils.hpp @@ -0,0 +1,32 @@ +// 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 +#include + +#include + +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_auto_perception_msgs::msg::Shape & shape); +} // namespace utils + +#endif // OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ diff --git a/perception/object_merger/launch/object_association_merger.launch.xml b/perception/object_merger/launch/object_association_merger.launch.xml new file mode 100644 index 0000000000000..92730f8070848 --- /dev/null +++ b/perception/object_merger/launch/object_association_merger.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/perception/object_merger/package.xml b/perception/object_merger/package.xml new file mode 100644 index 0000000000000..f3468511bc077 --- /dev/null +++ b/perception/object_merger/package.xml @@ -0,0 +1,28 @@ + + + object_merger + 0.1.0 + The object_merger package + + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + autoware_auto_perception_msgs + libpcl-all-dev + message_filters + pcl_conversions + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_ros + tf2_sensor_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/object_merger/src/object_association_merger/data_association/data_association.cpp b/perception/object_merger/src/object_association_merger/data_association/data_association.cpp new file mode 100644 index 0000000000000..7b17db0b4650b --- /dev/null +++ b/perception/object_merger/src/object_association_merger/data_association/data_association.cpp @@ -0,0 +1,368 @@ +// 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 + +#include +#include +#include + +DataAssociation::DataAssociation() : score_threshold_(0.1) +{ + can_assign_matrix_ = Eigen::MatrixXi::Identity(20, 20); + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 1; + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0; + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 1; + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 1; + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0; + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 1; + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 1; + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0; + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 1; + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 1; + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0; + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) = 1; + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0; + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) = 1; + can_assign_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0; + max_dist_matrix_ = Eigen::MatrixXd::Constant(20, 20, 1.0); + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 4.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 3.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 3.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 3.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 3.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 3.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 3.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 3.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 3.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 3.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 3.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 3.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) = 2.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) = 2.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) = 2.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) = 2.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.0; + max_dist_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN) = 2.0; + max_area_matrix_ = Eigen::MatrixXd::Constant(20, 20, /* large number */ 10000.0); + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 5.0 * 5.0; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.2 * 5.5; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 2.2 * 5.5; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 2.5 * 7.9; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 2.7 * 12.0; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.5 * 7.9; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 2.2 * 5.5; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 2.5 * 7.9; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 2.7 * 12.0; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.7 * 12.0; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 2.2 * 5.5; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 2.5 * 7.9; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 2.7 * 12.0; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.5; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) = 2.5; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) = 3.0; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 3.0; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) = 2.5; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) = 3.0; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.0; + max_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN) = 2.0; + min_area_matrix_ = Eigen::MatrixXd::Constant(20, 20, /* small number */ 0.0); + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 1.2 * 3.0; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 1.2 * 3.0; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 1.5 * 4.0; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::CAR, + autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 2.0 * 5.0; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 1.5 * 4.0; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 1.2 * 3.0; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 1.5 * 4.0; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, + autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 2.0 * 5.0; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 2.0 * 5.0; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::CAR) = 1.2 * 3.0; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK) = 1.5 * 4.0; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BUS, + autoware_auto_perception_msgs::msg::ObjectClassification::BUS) = 2.0 * 5.0; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0.001; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) = 0.001; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) = 0.001; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0.001; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) = 0.001; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) = 0.001; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) = 0.001; + min_area_matrix_( + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN) = 0.001; +} + +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_auto_perception_msgs::msg::DetectedObjects & object0, + const autoware_auto_perception_msgs::msg::DetectedObjects & object1) +{ + Eigen::MatrixXd score_matrix = + Eigen::MatrixXd::Zero(object1.objects.size(), object0.objects.size()); + for (size_t object1_idx = 0; object1_idx < object1.objects.size(); ++object1_idx) { + for (size_t object0_idx = 0; object0_idx < object0.objects.size(); ++object0_idx) { + double score = 0.0; + if (can_assign_matrix_( + object1.objects.at(object1_idx).classification.front().label, + object0.objects.at(object0_idx).classification.front().label)) { + const double max_dist = max_dist_matrix_( + object1.objects.at(object1_idx).classification.front().label, + object0.objects.at(object0_idx).classification.front().label); + const double max_area = max_area_matrix_( + object1.objects.at(object1_idx).classification.front().label, + object0.objects.at(object0_idx).classification.front().label); + const double min_area = min_area_matrix_( + object1.objects.at(object1_idx).classification.front().label, + object0.objects.at(object0_idx).classification.front().label); + const double dist = getDistance( + object0.objects.at(object0_idx).kinematics.pose_with_covariance.pose.position, + object1.objects.at(object1_idx).kinematics.pose_with_covariance.pose.position); + const double area0 = utils::getArea(object0.objects.at(object0_idx).shape); + const double area1 = utils::getArea(object1.objects.at(object1_idx).shape); + // the score (=cost) is reversed in ssp solver + 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_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp new file mode 100644 index 0000000000000..ef8a582604bc8 --- /dev/null +++ b/perception/object_merger/src/object_association_merger/data_association/successive_shortest_path/successive_shortest_path.cpp @@ -0,0 +1,396 @@ +// 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 +#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_dummies; + if (sparse_cost) { + n_dummies = n_agents; + } else { + n_dummies = 0; + } + + int source = 0; + int sink = n_agents + n_tasks + 1; + int n_nodes = n_agents + n_tasks + n_dummies + 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 (index: 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_dummies); + } else { + // Dummies + 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_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp new file mode 100644 index 0000000000000..e456aa1220c05 --- /dev/null +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -0,0 +1,95 @@ +// 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 +#include + +#include +#include +// #include +#include +#define EIGEN_MPL2_ONLY +#include +#include + +namespace object_association +{ +ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("cluster_data_association_node", node_options), + 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{1}); +} + +void ObjectAssociationMergerNode::objectsCallback( + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_object0_msg, + const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_object1_msg) +{ + // Guard + if (merged_object_pub_->get_subscription_count() < 1) { + return; + } + + // build output msg + autoware_auto_perception_msgs::msg::DetectedObjects 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->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->objects.at(direct_assignment.at(object0_idx)).existence_probability < + input_object0_msg->objects.at(object0_idx).existence_probability) { + output_msg.objects.push_back(input_object0_msg->objects.at(object0_idx)); + } else { + output_msg.objects.push_back( + input_object1_msg->objects.at(direct_assignment.at(object0_idx))); + } + } else { // not found + output_msg.objects.push_back(input_object0_msg->objects.at(object0_idx)); + } + } + for (size_t object1_idx = 0; object1_idx < input_object1_msg->objects.size(); ++object1_idx) { + if (reverse_assignment.find(object1_idx) != reverse_assignment.end()) { // found + } else { // not found + output_msg.objects.push_back(input_object1_msg->objects.at(object1_idx)); + } + } + + // publish output msg + merged_object_pub_->publish(output_msg); +} +} // namespace object_association + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(object_association::ObjectAssociationMergerNode) diff --git a/perception/object_merger/src/object_association_merger/utils/utils.cpp b/perception/object_merger/src/object_association_merger/utils/utils.cpp new file mode 100644 index 0000000000000..80e81d581fa9d --- /dev/null +++ b/perception/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 + +namespace utils +{ +double getArea(const autoware_auto_perception_msgs::msg::Shape & shape) +{ + double area = 0.0; + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + area = getRectangleArea(shape.dimensions); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + area = getCircleArea(shape.dimensions); + } else if (shape.type == autoware_auto_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