Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(lidar_centerpoint): accelerated preprocessing for centerpoint #6989

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions perception/image_projection_based_fusion/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -127,6 +127,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)

ament_auto_add_library(pointpainting_lib SHARED
src/pointpainting_fusion/node.cpp
src/pointpainting_fusion/pointcloud_densification.cpp
src/pointpainting_fusion/pointpainting_trt.cpp
src/pointpainting_fusion/voxel_generator.cpp
)
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
// Copyright 2024 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 IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_
#define IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#ifdef ROS_DISTRO_GALACTIC
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#else
#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
#endif

#include <lidar_centerpoint/preprocess/pointcloud_densification.hpp>

#include <list>
#include <string>
#include <utility>

namespace image_projection_based_fusion
{
struct PointCloudWithTransform
{
sensor_msgs::msg::PointCloud2 pointcloud_msg;
Eigen::Affine3f affine_past2world;
};

class PointCloudDensification
{
public:
explicit PointCloudDensification(const centerpoint::DensificationParam & param);

bool enqueuePointCloud(
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer);

double getCurrentTimestamp() const { return current_timestamp_; }
Eigen::Affine3f getAffineWorldToCurrent() const { return affine_world2current_; }
std::list<PointCloudWithTransform>::iterator getPointCloudCacheIter()
{
return pointcloud_cache_.begin();
}
bool isCacheEnd(std::list<PointCloudWithTransform>::iterator iter)
{
return iter == pointcloud_cache_.end();
}
unsigned int pointcloud_cache_size() const { return param_.pointcloud_cache_size(); }

private:
void enqueue(const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine);
void dequeue();

centerpoint::DensificationParam param_;
double current_timestamp_{0.0};
Eigen::Affine3f affine_world2current_;
std::list<PointCloudWithTransform> pointcloud_cache_;
};

} // namespace image_projection_based_fusion

#endif // IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__POINTCLOUD_DENSIFICATION_HPP_
22 changes: 19 additions & 3 deletions ...sed_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp
100755 → 100644
Original file line number Diff line number Diff line change
Expand Up @@ -15,19 +15,35 @@
#ifndef IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__VOXEL_GENERATOR_HPP_
#define IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__VOXEL_GENERATOR_HPP_

#include <image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp>
#include <lidar_centerpoint/preprocess/pointcloud_densification.hpp>
#include <lidar_centerpoint/preprocess/voxel_generator.hpp>

#include <bitset>
#include <memory>
#include <vector>

namespace image_projection_based_fusion
{
class VoxelGenerator : public centerpoint::VoxelGenerator

class VoxelGenerator
{
public:
using centerpoint::VoxelGenerator::VoxelGenerator;
explicit VoxelGenerator(
const centerpoint::DensificationParam & param, const centerpoint::CenterPointConfig & config);

bool enqueuePointCloud(
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer);

std::size_t generateSweepPoints(std::vector<float> & points);

protected:
std::unique_ptr<PointCloudDensification> pd_ptr_{nullptr};

std::size_t generateSweepPoints(std::vector<float> & points) override;
centerpoint::CenterPointConfig config_;
std::array<float, 6> range_;
std::array<int, 3> grid_size_;
std::array<float, 3> recip_voxel_size_;
};
} // namespace image_projection_based_fusion

Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,103 @@
// Copyright 2024 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 "image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp"

#include <pcl_ros/transforms.hpp>

#include <boost/optional.hpp>

#include <pcl_conversions/pcl_conversions.h>
#ifdef ROS_DISTRO_GALACTIC
#include <tf2_eigen/tf2_eigen.h>
#else
#include <tf2_eigen/tf2_eigen.hpp>
#endif

#include <string>
#include <utility>

namespace
{
boost::optional<geometry_msgs::msg::Transform> getTransform(
const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id,
const std::string & source_frame_id, const rclcpp::Time & time)
{
try {
geometry_msgs::msg::TransformStamped transform_stamped;
transform_stamped = tf_buffer.lookupTransform(
target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5));
return transform_stamped.transform;
} catch (tf2::TransformException & ex) {
RCLCPP_WARN_STREAM(rclcpp::get_logger("lidar_centerpoint"), ex.what());
return boost::none;
}
}

Eigen::Affine3f transformToEigen(const geometry_msgs::msg::Transform & t)
{
Eigen::Affine3f a;
a.matrix() = tf2::transformToEigen(t).matrix().cast<float>();
return a;
}

} // namespace

namespace image_projection_based_fusion
{
PointCloudDensification::PointCloudDensification(const centerpoint::DensificationParam & param)
: param_(param)
{
}

bool PointCloudDensification::enqueuePointCloud(
const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const tf2_ros::Buffer & tf_buffer)
{
const auto header = pointcloud_msg.header;

if (param_.pointcloud_cache_size() > 1) {
auto transform_world2current =
getTransform(tf_buffer, header.frame_id, param_.world_frame_id(), header.stamp);
if (!transform_world2current) {
return false;
}
auto affine_world2current = transformToEigen(transform_world2current.get());

enqueue(pointcloud_msg, affine_world2current);
} else {
enqueue(pointcloud_msg, Eigen::Affine3f::Identity());
}

dequeue();

return true;
}

void PointCloudDensification::enqueue(
const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine_world2current)
{
affine_world2current_ = affine_world2current;
current_timestamp_ = rclcpp::Time(msg.header.stamp).seconds();
PointCloudWithTransform pointcloud = {msg, affine_world2current.inverse()};
pointcloud_cache_.push_front(pointcloud);
}

void PointCloudDensification::dequeue()
{
if (pointcloud_cache_.size() > param_.pointcloud_cache_size()) {
pointcloud_cache_.pop_back();
}
}

} // namespace image_projection_based_fusion
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,32 @@

namespace image_projection_based_fusion
{

VoxelGenerator::VoxelGenerator(
const centerpoint::DensificationParam & param, const centerpoint::CenterPointConfig & config)
: config_(config)
{
pd_ptr_ = std::make_unique<PointCloudDensification>(param);
range_[0] = config.range_min_x_;
range_[1] = config.range_min_y_;
range_[2] = config.range_min_z_;
range_[3] = config.range_max_x_;
range_[4] = config.range_max_y_;
range_[5] = config.range_max_z_;
grid_size_[0] = config.grid_size_x_;
grid_size_[1] = config.grid_size_y_;
grid_size_[2] = config.grid_size_z_;
recip_voxel_size_[0] = 1 / config.voxel_size_x_;
recip_voxel_size_[1] = 1 / config.voxel_size_y_;
recip_voxel_size_[2] = 1 / config.voxel_size_z_;
}

bool VoxelGenerator::enqueuePointCloud(
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer)
{
return pd_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer);
}

size_t VoxelGenerator::generateSweepPoints(std::vector<float> & points)
{
Eigen::Vector3f point_current, point_past;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,15 +15,14 @@
#ifndef LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_
#define LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_

#include <lidar_centerpoint/cuda_utils.hpp>
#include <lidar_centerpoint/network/network_trt.hpp>
#include <lidar_centerpoint/postprocess/postprocess_kernel.hpp>
#include <lidar_centerpoint/preprocess/voxel_generator.hpp>

#include <sensor_msgs/msg/point_cloud2.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include "lidar_centerpoint/cuda_utils.hpp"
#include "lidar_centerpoint/network/network_trt.hpp"
#include "lidar_centerpoint/postprocess/postprocess_kernel.hpp"
#include "lidar_centerpoint/preprocess/voxel_generator.hpp"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"

#include "sensor_msgs/msg/point_cloud2.hpp"

#include <memory>
#include <string>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@
#ifndef LIDAR_CENTERPOINT__CUDA_UTILS_HPP_
#define LIDAR_CENTERPOINT__CUDA_UTILS_HPP_

#include <cuda_runtime_api.h>
#include "cuda_runtime_api.h"

#include <memory>
#include <sstream>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,10 +17,10 @@

#include <Eigen/Core>

#include <autoware_perception_msgs/msg/detected_object_kinematics.hpp>
#include <autoware_perception_msgs/msg/detected_objects.hpp>
#include <autoware_perception_msgs/msg/object_classification.hpp>
#include <autoware_perception_msgs/msg/shape.hpp>
#include "autoware_perception_msgs/msg/detected_object_kinematics.hpp"
#include "autoware_perception_msgs/msg/detected_objects.hpp"
#include "autoware_perception_msgs/msg/object_classification.hpp"
#include "autoware_perception_msgs/msg/shape.hpp"

#include <vector>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,8 @@
#ifndef LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_
#define LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_

#include <lidar_centerpoint/centerpoint_config.hpp>
#include <lidar_centerpoint/network/tensorrt_wrapper.hpp>
#include "lidar_centerpoint/centerpoint_config.hpp"
#include "lidar_centerpoint/network/tensorrt_wrapper.hpp"

#include <vector>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,8 @@
#ifndef LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_
#define LIDAR_CENTERPOINT__NETWORK__SCATTER_KERNEL_HPP_

#include <cuda.h>
#include <cuda_runtime_api.h>
#include "cuda.h"
#include "cuda_runtime_api.h"

namespace centerpoint
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,9 @@
#ifndef LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_
#define LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_

#include <lidar_centerpoint/centerpoint_config.hpp>
#include <tensorrt_common/tensorrt_common.hpp>

#include <NvInfer.h>
#include "NvInfer.h"
#include "lidar_centerpoint/centerpoint_config.hpp"
#include "tensorrt_common/tensorrt_common.hpp"

#include <iostream>
#include <memory>
Expand All @@ -32,7 +31,7 @@ class TensorRTWrapper
public:
explicit TensorRTWrapper(const CenterPointConfig & config);

~TensorRTWrapper();
virtual ~TensorRTWrapper();

bool init(
const std::string & onnx_path, const std::string & engine_path, const std::string & precision);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,13 +15,13 @@
#ifndef LIDAR_CENTERPOINT__NODE_HPP_
#define LIDAR_CENTERPOINT__NODE_HPP_

#include "lidar_centerpoint/centerpoint_trt.hpp"
#include "lidar_centerpoint/detection_class_remapper.hpp"
#include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp"

#include <autoware/universe_utils/ros/debug_publisher.hpp>
#include <autoware/universe_utils/ros/published_time_publisher.hpp>
#include <autoware/universe_utils/system/stop_watch.hpp>
#include <lidar_centerpoint/centerpoint_trt.hpp>
#include <lidar_centerpoint/detection_class_remapper.hpp>
#include <rclcpp/rclcpp.hpp>

#include <autoware_perception_msgs/msg/detected_object_kinematics.hpp>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,9 +15,8 @@
#ifndef LIDAR_CENTERPOINT__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_
#define LIDAR_CENTERPOINT__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_

#include <lidar_centerpoint/utils.hpp>

#include <thrust/device_vector.h>
#include "lidar_centerpoint/utils.hpp"
#include "thrust/device_vector.h"

namespace centerpoint
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,12 +15,11 @@
#ifndef LIDAR_CENTERPOINT__POSTPROCESS__POSTPROCESS_KERNEL_HPP_
#define LIDAR_CENTERPOINT__POSTPROCESS__POSTPROCESS_KERNEL_HPP_

#include <lidar_centerpoint/centerpoint_config.hpp>
#include <lidar_centerpoint/utils.hpp>

#include <cuda.h>
#include <cuda_runtime_api.h>
#include <thrust/device_vector.h>
#include "cuda.h"
#include "cuda_runtime_api.h"
#include "lidar_centerpoint/centerpoint_config.hpp"
#include "lidar_centerpoint/utils.hpp"
#include "thrust/device_vector.h"

#include <vector>

Expand Down
Loading
Loading