feat(lidar_centerpoint): single inference lidar centerpoint node (aut…

…owarefoundation#1926)

* Eliminating the tf dependence when using single frame detection.
Untested

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* Implemented a node to run single inferences with lidar centerpoint

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* feat(lidar_centerpoint): single inference lidar centerpoint node

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* Exiting program after the single inference
Added the visualization script
Addded a small section in the documentation

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* Updated the node to reflect recent changes in the main branch

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* Update perception/lidar_centerpoint/scripts/visualize_resuls.py

Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>

* Update perception/lidar_centerpoint/scripts/visualize_resuls.py

Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>

* Update perception/lidar_centerpoint/scripts/visualize_resuls.py

Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>

* Moved the viz script into the launcher and it is now executed automatically

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* Update perception/lidar_centerpoint/src/single_inference_node.cpp

Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>

* Update perception/lidar_centerpoint/src/single_inference_node.cpp

Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>

* Update perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py

Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>

* Update perception/lidar_centerpoint/README.md

Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>

* Added documentation for the new visualization script

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* Updating license

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* Update perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py

Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>

* Apply suggestions from code review

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>
Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>

perception/lidar_centerpoint/CMakeLists.txt

@@ -179,6 +179,27 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
     EXECUTABLE lidar_centerpoint_node
   )
 
+  ## single inference node ##
+  ament_auto_add_library(single_inference_lidar_centerpoint_component SHARED
+    src/single_inference_node.cpp
+  )
+
+  target_link_libraries(single_inference_lidar_centerpoint_component
+    centerpoint_lib
+  )
+
+  rclcpp_components_register_node(single_inference_lidar_centerpoint_component
+    PLUGIN "centerpoint::SingleInferenceLidarCenterPointNode"
+    EXECUTABLE single_inference_lidar_centerpoint_node
+  )
+
+  install(PROGRAMS
+    scripts/lidar_centerpoint_visualizer.py
+    DESTINATION lib/${PROJECT_NAME}
+  )
+
+  ament_export_dependencies(ament_cmake_python)
+
   ament_auto_package(
     INSTALL_TO_SHARE
       launch

perception/lidar_centerpoint/README.md

@@ -52,6 +52,18 @@ We trained the models using <https://github.com/open-mmlab/mmdetection3d>.
 
 You can download the onnx format of trained models by clicking on the links below.
 
+## Standalone inference and visualization
+
+In addition to its use as a standard ROS node, `lidar_centerpoint` can also be used to perform inferences in an isolated manner.
+To do so, execute the following launcher, where `pcd_path` is the path of the pointcloud to be used for inference.
+
+```bash
+ros2 launch lidar_centerpoint single_inference_lidar_centerpoint.launch.xml pcd_path:=test_pointcloud.pcd detections_path:=test_detections.ply
+```
+
+`lidar_centerpoint` generates a `ply` file in the provided `detections_path`, which contains the detections as triangle meshes.
+These detections can be visualized by most 3D tools, but we also integrate a visualization UI using `Open3D` which is launched alongside `lidar_centerpoint`.
+
 ### Changelog
 
 #### v1 (2022/07/06)

perception/lidar_centerpoint/include/lidar_centerpoint/single_inference_node.hpp

@@ -0,0 +1,63 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LIDAR_CENTERPOINT__SINGLE_INFERENCE_NODE_HPP_
+#define LIDAR_CENTERPOINT__SINGLE_INFERENCE_NODE_HPP_
+
+#include <Eigen/Core>
+#include <lidar_centerpoint/centerpoint_trt.hpp>
+#include <lidar_centerpoint/detection_class_remapper.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_perception_msgs/msg/detected_object_kinematics.hpp>
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
+#include <autoware_auto_perception_msgs/msg/shape.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace centerpoint
+{
+
+class SingleInferenceLidarCenterPointNode : public rclcpp::Node
+{
+public:
+  explicit SingleInferenceLidarCenterPointNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  void detect(const std::string & pcd_path, const std::string & detections_path);
+  std::vector<Eigen::Vector3d> getVertices(
+    const autoware_auto_perception_msgs::msg::Shape & shape, const Eigen::Affine3d & pose) const;
+  void dumpDetectionsAsMesh(
+    const autoware_auto_perception_msgs::msg::DetectedObjects & objects_msg,
+    const std::string & output_path) const;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_{tf_buffer_};
+
+  float score_threshold_{0.0};
+  std::vector<std::string> class_names_;
+  bool rename_car_to_truck_and_bus_{false};
+  bool has_twist_{false};
+
+  DetectionClassRemapper detection_class_remapper_;
+
+  std::unique_ptr<CenterPointTRT> detector_ptr_{nullptr};
+};
+
+}  // namespace centerpoint
+
+#endif  // LIDAR_CENTERPOINT__SINGLE_INFERENCE_NODE_HPP_

perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="model_name" default="centerpoint_tiny" description="options: `centerpoint` or `centerpoint_tiny`"/>
+  <arg name="model_path" default="$(find-pkg-share lidar_centerpoint)/data"/>
+  <arg name="model_param_path" default="$(find-pkg-share lidar_centerpoint)/config/$(var model_name).param.yaml"/>
+  <arg name="class_remapper_param_path" default="$(find-pkg-share lidar_centerpoint)/config/detection_class_remapper.param.yaml"/>
+  <arg name="score_threshold" default="0.35"/>
+  <arg name="yaw_norm_threshold" default="0.0"/>
+  <arg name="has_twist" default="false"/>
+  <arg name="pcd_path" default="test.pcd"/>
+  <arg name="detections_path" default="test.ply"/>
+
+  <node pkg="lidar_centerpoint" exec="single_inference_lidar_centerpoint_node" name="lidar_centerpoint" output="screen">
+    <param name="score_threshold" value="$(var score_threshold)"/>
+    <param name="yaw_norm_threshold" value="$(var yaw_norm_threshold)"/>
+    <param name="densification_world_frame_id" value="map"/>
+    <param name="densification_num_past_frames" value="0"/>
+    <param name="trt_precision" value="fp16"/>
+    <param name="has_twist" value="$(var has_twist)"/>
+    <param name="encoder_onnx_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"/>
+    <param name="encoder_engine_path" value="$(var model_path)/pts_voxel_encoder_$(var model_name).engine"/>
+    <param name="head_onnx_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"/>
+    <param name="head_engine_path" value="$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"/>
+    <param from="$(var model_param_path)"/>
+    <param from="$(var class_remapper_param_path)"/>
+
+    <param name="pcd_path" value="$(var pcd_path)"/>
+    <param name="detections_path" value="$(var detections_path)"/>
+  </node>
+
+  <node pkg="lidar_centerpoint" exec="lidar_centerpoint_visualizer.py" name="lidar_centerpoint_visualizer" output="screen">
+    <param name="pcd_path" value="$(var pcd_path)"/>
+    <param name="detections_path" value="$(var detections_path)"/>
+  </node>
+</launch>

perception/lidar_centerpoint/package.xml

@@ -8,12 +8,14 @@
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
 
   <build_depend>autoware_cmake</build_depend>
 
   <depend>autoware_auto_perception_msgs</depend>
   <depend>pcl_ros</depend>
   <depend>perception_utils</depend>
+  <depend>python3-open3d-pip</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2_eigen</depend>

perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python
+# Copyright 2022 TIER IV, Inc. All rights reserved.
+#
+# 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.
+
+import os
+import time
+
+import open3d as o3d
+import rclpy
+from rclpy.node import Node
+
+
+def main(args=None):
+
+    rclpy.init(args=args)
+
+    node = Node("lidar_centerpoint_visualizer")
+    node.declare_parameter("pcd_path", rclpy.Parameter.Type.STRING)
+    node.declare_parameter("detections_path", rclpy.Parameter.Type.STRING)
+
+    pcd_path = node.get_parameter("pcd_path").get_parameter_value().string_value
+    detections_path = node.get_parameter("detections_path").get_parameter_value().string_value
+
+    while not os.path.exists(pcd_path) and not os.path.exists(detections_path):
+        time.sleep(1.0)
+
+        if not rclpy.ok():
+            rclpy.shutdown()
+            return
+
+    mesh = o3d.io.read_triangle_mesh(detections_path)
+    pcd = o3d.io.read_point_cloud(pcd_path)
+
+    mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0])
+
+    detection_lines = o3d.geometry.LineSet.create_from_triangle_mesh(mesh)
+    detection_lines.paint_uniform_color([1.0, 0.0, 1.0])
+
+    o3d.visualization.draw_geometries([mesh_frame, pcd, detection_lines])
+
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+
+    main()