Ros2 v0.8.0 object merger (autowarefoundation#302)

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
+)

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 <list>
+#include <unordered_map>
+#include <vector>
+
+#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<int, int> & direct_assignment,
+    std::unordered_map<int, int> & 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_

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 <memory>
+
+#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<autoware_perception_msgs::msg::DynamicObjectWithFeatureArray>::SharedPtr
+    merged_object_pub_;
+  message_filters::Subscriber<autoware_perception_msgs::msg::DynamicObjectWithFeatureArray>
+  object0_sub_;
+  message_filters::Subscriber<autoware_perception_msgs::msg::DynamicObjectWithFeatureArray>
+  object1_sub_;
+  typedef message_filters::sync_policies::ApproximateTime<
+      autoware_perception_msgs::msg::DynamicObjectWithFeatureArray,
+      autoware_perception_msgs::msg::DynamicObjectWithFeatureArray>
+    SyncPolicy;
+  typedef message_filters::Synchronizer<SyncPolicy> Sync;
+  Sync sync_;
+  DataAssociation data_association_;
+};
+}  // namespace object_association
+
+#endif  // OBJECT_ASSOCIATION_MERGER__NODE_HPP_

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 <unordered_map>
+#include <vector>
+
+namespace assignment_problem
+{
+// See IMPORTANT NOTE at the top of the file.
+void MaximizeLinearAssignment(
+  const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
+  std::unordered_map<int, int> * reverse_assignment);
+}  // namespace assignment_problem
+
+#endif  // OBJECT_ASSOCIATION_MERGER__SUCCESSIVE_SHORTEST_PATH_HPP_

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 <cmath>
+#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_

perception/object_recognition/detection/object_merger/launch/object_association_merger.launch.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="input/object0" default="object0"/>
+  <arg name="input/object1" default="object1"/>
+  <arg name="output/object" default="merged_object"/>
+
+  <node pkg="object_merger" exec="object_association_merger_node" name="object_association_merger" output="screen">
+    <remap from="input/object0" to="$(var input/object0)"/>
+    <remap from="input/object1" to="$(var input/object1)"/>
+    <remap from="output/object" to="$(var output/object)"/>
+  </node>
+
+</launch>

perception/object_recognition/detection/object_merger/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>object_merger</name>
+  <version>0.1.0</version>
+  <description>The object_merger package</description>
+
+  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <depend>autoware_perception_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>pcl_conversions</depend>
+  <depend>libpcl-all-dev</depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_sensor_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>