feat(tier4_autoware_utils): add and use transformPointcloud (autowarefoundation#3359)

* feat(tier4_autoware_utils): add and use transformPointcloud

Signed-off-by: Kaan Çolak <kaancolak95@gmail.com>

---------

Signed-off-by: Kaan Çolak <kaancolak95@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: kaancolak

common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp

@@ -38,5 +38,6 @@
 #include "tier4_autoware_utils/ros/uuid_helper.hpp"
 #include "tier4_autoware_utils/ros/wait_for_param.hpp"
 #include "tier4_autoware_utils/system/stop_watch.hpp"
+#include "tier4_autoware_utils/transform/transforms.hpp"
 
 #endif  // TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_

common/tier4_autoware_utils/include/tier4_autoware_utils/transform/transforms.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 TIER4_AUTOWARE_UTILS__TRANSFORM__TRANSFORMS_HPP_
+#define TIER4_AUTOWARE_UTILS__TRANSFORM__TRANSFORMS_HPP_
+
+#include <Eigen/Core>
+#include <rclcpp/rclcpp.hpp>
+
+#include <pcl/common/transforms.h>
+#include <pcl/point_cloud.h>
+
+namespace tier4_autoware_utils
+{
+template <typename PointT>
+void transformPointCloud(
+  const pcl::PointCloud<PointT> & cloud_in, pcl::PointCloud<PointT> & cloud_out,
+  const Eigen::Matrix<float, 4, 4> & transform)
+{
+  if (cloud_in.empty() || cloud_in.width == 0) {
+    RCLCPP_WARN(rclcpp::get_logger("transformPointCloud"), "input point cloud is empty!");
+  } else {
+    pcl::transformPointCloud(cloud_in, cloud_out, transform);
+  }
+}
+
+template <typename PointT>
+void transformPointCloud(
+  const pcl::PointCloud<PointT> & cloud_in, pcl::PointCloud<PointT> & cloud_out,
+  const Eigen::Affine3f & transform)
+{
+  if (cloud_in.empty() || cloud_in.width == 0) {
+    RCLCPP_WARN(rclcpp::get_logger("transformPointCloud"), "input point cloud is empty!");
+  } else {
+    pcl::transformPointCloud(cloud_in, cloud_out, transform);
+  }
+}
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__TRANSFORM__TRANSFORMS_HPP_

common/tier4_autoware_utils/package.xml

@@ -20,6 +20,7 @@
   <depend>diagnostic_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>libboost-dev</depend>
+  <depend>pcl_ros</depend>
   <depend>rclcpp</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>

common/tier4_autoware_utils/test/src/transform/test_transform.cpp

@@ -0,0 +1,59 @@
+// 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.
+
+#include "tier4_autoware_utils/transform/transforms.hpp"
+
+#include <gtest/gtest.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <thread>
+
+TEST(system, transform_point_cloud)
+{
+  pcl::PointCloud<pcl::PointXYZI> cloud;
+  cloud.push_back(pcl::PointXYZI(10.055880, -42.758892, -10.636949, 4));
+  cloud.push_back(pcl::PointXYZI(23.282284, -29.485722, -11.468469, 5));
+
+  Eigen::Matrix<float, 4, 4> transform;
+  transform << 0.834513, -0.550923, -0.008474, 89571.148438, 0.550986, 0.834372, 0.015428,
+    42301.179688, -0.001429, -0.017543, 0.999845, -3.157415, 0.000000, 0.000000, 0.000000, 1.000000;
+
+  pcl::PointCloud<pcl::PointXYZI> cloud_transformed;
+  tier4_autoware_utils::transformPointCloud(cloud, cloud_transformed, transform);
+
+  pcl::PointXYZI pt1_gt(89603.187500, 42270.878906, -13.056946, 4);
+
+  constexpr float float_error = 0.0001;
+  EXPECT_NEAR(cloud_transformed[0].x, pt1_gt.x, float_error);
+  EXPECT_NEAR(cloud_transformed[0].y, pt1_gt.y, float_error);
+  EXPECT_NEAR(cloud_transformed[0].z, pt1_gt.z, float_error);
+  EXPECT_EQ(cloud_transformed[0].intensity, pt1_gt.intensity);
+}
+
+TEST(system, empty_point_cloud)
+{
+  pcl::PointCloud<pcl::PointXYZI> cloud;
+
+  Eigen::Matrix<float, 4, 4> transform;
+  transform << 0.834513, -0.550923, -0.008474, 89571.148438, 0.550986, 0.834372, 0.015428,
+    42301.179688, -0.001429, -0.017543, 0.999845, -3.157415, 0.000000, 0.000000, 0.000000, 1.000000;
+
+  pcl::PointCloud<pcl::PointXYZI> cloud_transformed;
+
+  EXPECT_NO_THROW(tier4_autoware_utils::transformPointCloud(cloud, cloud_transformed, transform));
+  EXPECT_NO_FATAL_FAILURE(
+    tier4_autoware_utils::transformPointCloud(cloud, cloud_transformed, transform));
+  EXPECT_EQ(cloud_transformed.size(), 0ul);
+}

localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp

@@ -22,6 +22,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
+#include <tier4_autoware_utils/transform/transforms.hpp>
 
 #include <autoware_map_msgs/srv/get_differential_point_cloud_map.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>

localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp

@@ -22,6 +22,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/marker_helper.hpp>
+#include <tier4_autoware_utils/transform/transforms.hpp>
 
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <tier4_localization_msgs/srv/pose_with_covariance_stamped.hpp>

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -260,7 +260,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped MapUpdateModule::align_using_monte
     ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array);
 
     auto sensor_points_mapTF_ptr = std::make_shared<pcl::PointCloud<PointSource>>();
-    pcl::transformPointCloud(*ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose);
+    tier4_autoware_utils::transformPointCloud(
+      *ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose);
     publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_mapTF_ptr);
   }
 

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -20,6 +20,7 @@
 #include "ndt_scan_matcher/util_func.hpp"
 
 #include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/transform/transforms.hpp>
 
 #include <pcl_conversions/pcl_conversions.h>
 
@@ -437,7 +438,7 @@ void NDTScanMatcher::callback_sensor_points(
 
   pcl::shared_ptr<pcl::PointCloud<PointSource>> sensor_points_mapTF_ptr(
     new pcl::PointCloud<PointSource>);
-  pcl::transformPointCloud(
+  tier4_autoware_utils::transformPointCloud(
     *sensor_points_baselinkTF_ptr, *sensor_points_mapTF_ptr, ndt_result.pose);
   publish_point_cloud(sensor_ros_time, map_frame_, sensor_points_mapTF_ptr);
 
@@ -495,7 +496,7 @@ void NDTScanMatcher::transform_sensor_measurement(
     tier4_autoware_utils::transform2pose(*TF_target_to_source_ptr);
   const Eigen::Matrix4f base_to_sensor_matrix =
     pose_to_matrix4f(target_to_source_pose_stamped.pose);
-  pcl::transformPointCloud(
+  tier4_autoware_utils::transformPointCloud(
     *sensor_points_input_ptr, *sensor_points_output_ptr, base_to_sensor_matrix);
 }
 

localization/ndt_scan_matcher/src/pose_initialization_module.cpp

@@ -111,7 +111,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped PoseInitializationModule::align_us
     ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array);
 
     auto sensor_points_mapTF_ptr = std::make_shared<pcl::PointCloud<PointSource>>();
-    pcl::transformPointCloud(*ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose);
+    tier4_autoware_utils::transformPointCloud(
+      *ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose);
     publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_mapTF_ptr);
   }
 

perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp

@@ -23,8 +23,8 @@
 #include <cuda_utils/stream_unique_ptr.hpp>
 #include <tensorrt_common/tensorrt_common.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
+#include <tier4_autoware_utils/transform/transforms.hpp>
 
-#include <pcl/common/transforms.h>
 #include <tf2_ros/buffer_interface.h>
 #include <tf2_ros/transform_listener.h>
 

perception/lidar_apollo_instance_segmentation/src/detector.cpp

@@ -83,7 +83,7 @@ bool LidarApolloInstanceSegmentation::transformCloud(
         target_frame_, input.header.frame_id, input.header.stamp, std::chrono::milliseconds(500));
       Eigen::Matrix4f affine_matrix =
         tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
-      pcl::transformPointCloud(pcl_input, pcl_transformed_cloud, affine_matrix);
+      tier4_autoware_utils::transformPointCloud(pcl_input, pcl_transformed_cloud, affine_matrix);
       transformed_cloud.header.frame_id = target_frame_;
       pcl_transformed_cloud.header.frame_id = target_frame_;
     } catch (tf2::TransformException & ex) {
@@ -98,7 +98,8 @@ bool LidarApolloInstanceSegmentation::transformCloud(
   pcl::PointCloud<pcl::PointXYZI> pointcloud_with_z_offset;
   Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset));
   Eigen::Matrix4f z_up_transform = z_up_translation.matrix();
-  pcl::transformPointCloud(pcl_transformed_cloud, pcl_transformed_cloud, z_up_transform);
+  tier4_autoware_utils::transformPointCloud(
+    pcl_transformed_cloud, pcl_transformed_cloud, z_up_transform);
 
   pcl::toROSMsg(pcl_transformed_cloud, transformed_cloud);
 

perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp

@@ -145,7 +145,8 @@ void ApolloLidarSegmentation::transformCloud(
         tf_buffer_->lookupTransform(target_frame_, input.header.frame_id, time_point);
       Eigen::Matrix4f affine_matrix =
         tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
-      pcl::transformPointCloud(in_cluster, transformed_cloud_cluster, affine_matrix);
+      tier4_autoware_utils::transformPointCloud(
+        in_cluster, transformed_cloud_cluster, affine_matrix);
       transformed_cloud_cluster.header.frame_id = target_frame_;
     } else {
       transformed_cloud_cluster = in_cluster;
@@ -155,7 +156,7 @@ void ApolloLidarSegmentation::transformCloud(
     if (z_offset != 0) {
       Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset));
       Eigen::Matrix4f z_up_transform = z_up_translation.matrix();
-      pcl::transformPointCloud(
+      tier4_autoware_utils::transformPointCloud(
         transformed_cloud_cluster, transformed_cloud_cluster, z_up_transform);
     }
 

planning/behavior_velocity_planner/src/node.cpp

@@ -270,7 +270,7 @@ void BehaviorVelocityPlannerNode::onNoGroundPointCloud(
   Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast<float>();
   pcl::PointCloud<pcl::PointXYZ>::Ptr pc_transformed(new pcl::PointCloud<pcl::PointXYZ>);
   if (!pc.empty()) {
-    pcl::transformPointCloud(pc, *pc_transformed, affine);
+    tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine);
   }
 
   {

planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp

@@ -252,7 +252,8 @@ pcl::PointCloud<pcl::PointXYZ> transformPointCloud(
   pcl::PointCloud<pcl::PointXYZ> pointcloud_pcl;
   pcl::fromROSMsg(input_pointcloud, pointcloud_pcl);
   pcl::PointCloud<pcl::PointXYZ> pointcloud_pcl_transformed;
-  pcl::transformPointCloud(pointcloud_pcl, pointcloud_pcl_transformed, transform_matrix);
+  tier4_autoware_utils::transformPointCloud(
+    pointcloud_pcl, pointcloud_pcl_transformed, transform_matrix);
 
   return pointcloud_pcl_transformed;
 }

planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp

@@ -20,6 +20,7 @@
 #include <motion_utils/motion_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+#include <tier4_autoware_utils/transform/transforms.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>

planning/surround_obstacle_checker/src/node.cpp

@@ -346,7 +346,8 @@ boost::optional<Obstacle> SurroundObstacleCheckerNode::getNearestObstacleByPoint
   Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.get().transform).cast<float>();
   pcl::PointCloud<pcl::PointXYZ> transformed_pointcloud;
   pcl::fromROSMsg(*pointcloud_ptr_, transformed_pointcloud);
-  pcl::transformPointCloud(transformed_pointcloud, transformed_pointcloud, isometry);
+  tier4_autoware_utils::transformPointCloud(
+    transformed_pointcloud, transformed_pointcloud, isometry);
 
   const auto ego_polygon = createSelfPolygon(vehicle_info_);