diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index c512e06b3bf0e..2591266652827 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/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 diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index 72eb5498b71d4..777b3fc191111 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -52,6 +52,18 @@ We trained the models using . 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) diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/single_inference_node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/single_inference_node.hpp new file mode 100644 index 0000000000000..45181d55ed46a --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +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 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 class_names_; + bool rename_car_to_truck_and_bus_{false}; + bool has_twist_{false}; + + DetectionClassRemapper detection_class_remapper_; + + std::unique_ptr detector_ptr_{nullptr}; +}; + +} // namespace centerpoint + +#endif // LIDAR_CENTERPOINT__SINGLE_INFERENCE_NODE_HPP_ diff --git a/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml new file mode 100644 index 0000000000000..0f6923d5e6414 --- /dev/null +++ b/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index 4372b1848d08b..14cfaa7a72e3b 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -8,12 +8,14 @@ Apache License 2.0 ament_cmake_auto + ament_cmake_python autoware_cmake autoware_auto_perception_msgs pcl_ros perception_utils + python3-open3d-pip rclcpp rclcpp_components tf2_eigen diff --git a/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py b/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py new file mode 100755 index 0000000000000..3153351922219 --- /dev/null +++ b/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() diff --git a/perception/lidar_centerpoint/src/single_inference_node.cpp b/perception/lidar_centerpoint/src/single_inference_node.cpp new file mode 100644 index 0000000000000..77099ad8226a2 --- /dev/null +++ b/perception/lidar_centerpoint/src/single_inference_node.cpp @@ -0,0 +1,248 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_centerpoint/single_inference_node.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include + +namespace centerpoint +{ +SingleInferenceLidarCenterPointNode::SingleInferenceLidarCenterPointNode( + const rclcpp::NodeOptions & node_options) +: Node("lidar_center_point", node_options), tf_buffer_(this->get_clock()) +{ + const float score_threshold = + static_cast(this->declare_parameter("score_threshold", 0.35)); + const float circle_nms_dist_threshold = + static_cast(this->declare_parameter("circle_nms_dist_threshold", 1.5)); + const auto yaw_norm_thresholds = + this->declare_parameter>("yaw_norm_thresholds"); + const std::string densification_world_frame_id = + this->declare_parameter("densification_world_frame_id", "map"); + const int densification_num_past_frames = + this->declare_parameter("densification_num_past_frames", 1); + const std::string trt_precision = this->declare_parameter("trt_precision", "fp16"); + const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path"); + const std::string encoder_engine_path = + this->declare_parameter("encoder_engine_path"); + const std::string head_onnx_path = this->declare_parameter("head_onnx_path"); + const std::string head_engine_path = this->declare_parameter("head_engine_path"); + class_names_ = this->declare_parameter>("class_names"); + has_twist_ = this->declare_parameter("has_twist", false); + const std::size_t point_feature_size = + static_cast(this->declare_parameter("point_feature_size")); + const std::size_t max_voxel_size = + static_cast(this->declare_parameter("max_voxel_size")); + const auto point_cloud_range = this->declare_parameter>("point_cloud_range"); + const auto voxel_size = this->declare_parameter>("voxel_size"); + const std::size_t downsample_factor = + static_cast(this->declare_parameter("downsample_factor")); + const std::size_t encoder_in_feature_size = + static_cast(this->declare_parameter("encoder_in_feature_size")); + const auto allow_remapping_by_area_matrix = + this->declare_parameter>("allow_remapping_by_area_matrix"); + const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); + const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); + const auto pcd_path = this->declare_parameter("pcd_path"); + const auto detections_path = this->declare_parameter("detections_path"); + + detection_class_remapper_.setParameters( + allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + + NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); + NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); + DensificationParam densification_param( + densification_world_frame_id, densification_num_past_frames); + + if (point_cloud_range.size() != 6) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("single_inference_lidar_centerpoint"), + "The size of point_cloud_range != 6: use the default parameters."); + } + if (voxel_size.size() != 3) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("single_inference_lidar_centerpoint"), + "The size of voxel_size != 3: use the default parameters."); + } + CenterPointConfig config( + class_names_.size(), point_feature_size, max_voxel_size, point_cloud_range, voxel_size, + downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, + yaw_norm_thresholds); + detector_ptr_ = + std::make_unique(encoder_param, head_param, densification_param, config); + + detect(pcd_path, detections_path); + exit(0); +} + +std::vector SingleInferenceLidarCenterPointNode::getVertices( + const autoware_auto_perception_msgs::msg::Shape & shape, const Eigen::Affine3d & pose) const +{ + std::vector vertices; + Eigen::Vector3d vertex; + + vertex.x() = -shape.dimensions.x / 2.0; + vertex.y() = -shape.dimensions.y / 2.0; + vertex.z() = shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = -shape.dimensions.x / 2.0; + vertex.y() = shape.dimensions.y / 2.0; + vertex.z() = shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = -shape.dimensions.x / 2.0; + vertex.y() = shape.dimensions.y / 2.0; + vertex.z() = -shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = shape.dimensions.x / 2.0; + vertex.y() = shape.dimensions.y / 2.0; + vertex.z() = shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = shape.dimensions.x / 2.0; + vertex.y() = shape.dimensions.y / 2.0; + vertex.z() = -shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = shape.dimensions.x / 2.0; + vertex.y() = -shape.dimensions.y / 2.0; + vertex.z() = shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = shape.dimensions.x / 2.0; + vertex.y() = -shape.dimensions.y / 2.0; + vertex.z() = -shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + vertex.x() = -shape.dimensions.x / 2.0; + vertex.y() = -shape.dimensions.y / 2.0; + vertex.z() = -shape.dimensions.z / 2.0; + vertices.push_back(pose * vertex); + + return vertices; +} + +void SingleInferenceLidarCenterPointNode::detect( + const std::string & pcd_path, const std::string & detections_path) +{ + sensor_msgs::msg::PointCloud2 msg; + pcl::PointCloud::Ptr pc_ptr(new pcl::PointCloud()); + + pcl::io::loadPCDFile(pcd_path, *pc_ptr); + pcl::toROSMsg(*pc_ptr, msg); + msg.header.frame_id = "lidar_frame"; + + std::vector det_boxes3d; + bool is_success = detector_ptr_->detect(msg, tf_buffer_, det_boxes3d); + if (!is_success) { + return; + } + + autoware_auto_perception_msgs::msg::DetectedObjects output_msg; + output_msg.header = msg.header; + for (const auto & box3d : det_boxes3d) { + autoware_auto_perception_msgs::msg::DetectedObject obj; + box3DToDetectedObject(box3d, class_names_, has_twist_, obj); + output_msg.objects.emplace_back(obj); + } + + detection_class_remapper_.mapClasses(output_msg); + + dumpDetectionsAsMesh(output_msg, detections_path); + + RCLCPP_INFO( + rclcpp::get_logger("single_inference_lidar_centerpoint"), + "The detection results were saved as meshes in %s", detections_path.c_str()); +} + +void SingleInferenceLidarCenterPointNode::dumpDetectionsAsMesh( + const autoware_auto_perception_msgs::msg::DetectedObjects & objects_msg, + const std::string & output_path) const +{ + std::ofstream ofs(output_path, std::ofstream::out); + std::stringstream vertices_stream; + std::stringstream faces_stream; + int index = 0; + int num_detections = static_cast(objects_msg.objects.size()); + + ofs << "ply" << std::endl; + ofs << "format ascii 1.0" << std::endl; + ofs << "comment created by lidar_centerpoint" << std::endl; + ofs << "element vertex " << 8 * num_detections << std::endl; + ofs << "property float x" << std::endl; + ofs << "property float y" << std::endl; + ofs << "property float z" << std::endl; + ofs << "element face " << 12 * num_detections << std::endl; + ofs << "property list uchar uint vertex_indices" << std::endl; + ofs << "end_header" << std::endl; + + auto streamFace = [&faces_stream](int v1, int v2, int v3) { + faces_stream << std::to_string(3) << " " << std::to_string(v1) << " " << std::to_string(v2) + << " " << std::to_string(v3) << std::endl; + }; + + for (const auto & object : objects_msg.objects) { + Eigen::Affine3d pose_affine; + tf2::fromMsg(object.kinematics.pose_with_covariance.pose, pose_affine); + + std::vector vertices = getVertices(object.shape, pose_affine); + + for (const auto & vertex : vertices) { + vertices_stream << vertex.x() << " " << vertex.y() << " " << vertex.z() << std::endl; + } + + streamFace(index + 1, index + 3, index + 4); + streamFace(index + 3, index + 5, index + 6); + streamFace(index + 0, index + 7, index + 5); + streamFace(index + 7, index + 2, index + 4); + streamFace(index + 5, index + 3, index + 1); + streamFace(index + 7, index + 0, index + 2); + streamFace(index + 2, index + 1, index + 4); + streamFace(index + 4, index + 3, index + 6); + streamFace(index + 5, index + 7, index + 6); + streamFace(index + 6, index + 7, index + 4); + streamFace(index + 0, index + 5, index + 1); + index += 8; + } + + ofs << vertices_stream.str(); + ofs << faces_stream.str(); + + ofs.close(); +} + +} // namespace centerpoint + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(centerpoint::SingleInferenceLidarCenterPointNode)