forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request autowarefoundation#1700 from tier4/x2_gen2/v0.29.0…
…/centerpoint_cherrypicks feat(centerpoint): cherrypicked upstream changes for x2_gen2/v0.29.0
- Loading branch information
Showing
52 changed files
with
737 additions
and
266 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
72 changes: 72 additions & 0 deletions
72
...n/include/image_projection_based_fusion/pointpainting_fusion/pointcloud_densification.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
103 changes: 103 additions & 0 deletions
103
...ption/image_projection_based_fusion/src/pointpainting_fusion/pointcloud_densification.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
21 changes: 21 additions & 0 deletions
21
perception/lidar_centerpoint/config/centerpoint_sigma.param.yaml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,21 @@ | ||
| /**: | ||
| ros__parameters: | ||
|
|
||
| # weight files | ||
| encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx" | ||
| encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine" | ||
| head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" | ||
| head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" | ||
| trt_precision: fp16 | ||
| cloud_capacity: 2000000 | ||
| post_process_params: | ||
| # post-process params | ||
| circle_nms_dist_threshold: 0.5 | ||
| iou_nms_target_class_names: ["CAR"] | ||
| iou_nms_search_distance_2d: 10.0 | ||
| iou_nms_threshold: 0.1 | ||
| yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] | ||
| score_threshold: 0.35 | ||
| densification_params: | ||
| world_frame_id: map | ||
| num_past_frames: 1 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.