feat(lidar_centerpoint): moved preprocessing to cuda

perception/lidar_apollo_instance_segmentation/src/detector.cpp

@@ -16,6 +16,8 @@
 
 #include "lidar_apollo_instance_segmentation/feature_map.hpp"
 
+#include <sensor_msgs/point_cloud2_iterator.hpp>
+
 #include <NvCaffeParser.h>
 #include <NvInfer.h>
 #include <pcl_conversions/pcl_conversions.h>
@@ -125,7 +127,27 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects(
 
   // convert from ros to pcl
   pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_pointcloud_raw_ptr(new pcl::PointCloud<pcl::PointXYZI>);
-  pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_raw_ptr);
+  // pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_raw_ptr);
+
+  auto pcl_pointcloud_raw = *pcl_pointcloud_raw_ptr;
+  pcl_pointcloud_raw.width = transformed_cloud.width;
+  pcl_pointcloud_raw.height = transformed_cloud.height;
+  pcl_pointcloud_raw.is_dense = transformed_cloud.is_dense == 1;
+  pcl_pointcloud_raw.resize(transformed_cloud.width * transformed_cloud.height);
+
+  sensor_msgs::PointCloud2ConstIterator<float> it_x(transformed_cloud, "x");
+  sensor_msgs::PointCloud2ConstIterator<float> it_y(transformed_cloud, "y");
+  sensor_msgs::PointCloud2ConstIterator<float> it_z(transformed_cloud, "z");
+  sensor_msgs::PointCloud2ConstIterator<uint8_t> it_intensity(transformed_cloud, "intensity");
+
+  for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_intensity) {
+    pcl::PointXYZI point;
+    point.x = *it_x;
+    point.y = *it_y;
+    point.z = *it_z;
+    point.intensity = static_cast<float>(*it_intensity);
+    pcl_pointcloud_raw.emplace_back(std::move(point));
+  }
 
   // generate feature map
   std::shared_ptr<FeatureMapInterface> feature_map_ptr =

perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp

@@ -123,9 +123,9 @@ bool LowIntensityClusterFilter::isValidatedCluster(const sensor_msgs::msg::Point
     RCLCPP_WARN(get_logger(), "Invalid point cloud data. point_step is less than 16.");
     return true;
   }
-  for (sensor_msgs::PointCloud2ConstIterator<float> iter(cluster, "intensity"); iter != iter.end();
-       ++iter) {
-    mean_intensity += *iter;
+  for (sensor_msgs::PointCloud2ConstIterator<uint8_t> iter(cluster, "intensity");
+       iter != iter.end(); ++iter) {
+    mean_intensity += static_cast<float>(*iter);
   }
   const size_t num_points = cluster.width * cluster.height;
   mean_intensity /= static_cast<double>(num_points);

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp

@@ -33,6 +33,10 @@ using point_cloud_msg_wrapper::PointCloud2Modifier;
 class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
 {
 protected:
+  using InputPointIndex = autoware_point_types::PointXYZIRCAEDTIndex;
+  using InputPointType = autoware_point_types::PointXYZIRCAEDT;
+  using OutputPointType = autoware_point_types::PointXYZIRC;
+
   virtual void filter(
     const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
 
@@ -75,8 +79,7 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
   }
 
   void setUpPointCloudFormat(
-    const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size,
-    size_t num_fields);
+    const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size);
 
 public:
   PCL_MAKE_ALIGNED_OPERATOR_NEW

sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -251,65 +251,68 @@
   // If there is a point that cannot be associated, record it to issue a warning
   bool twist_time_stamp_is_too_late = false;
   bool imu_time_stamp_is_too_late = false;
+  double global_point_stamp;
 
   for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) {
-    while (twist_it != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) {
+    global_point_stamp =
+      points.header.stamp.sec + 1e-9 * (points.header.stamp.nanosec + *it_time_stamp);
+    while (twist_it != std::end(twist_queue_) - 1 && global_point_stamp > twist_stamp) {
       ++twist_it;
       twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds();
     }
 
     float v{static_cast<float>(twist_it->twist.linear.x)};
     float w{static_cast<float>(twist_it->twist.angular.z)};
 
-    if (std::abs(*it_time_stamp - twist_stamp) > 0.1) {
+    if (std::abs(global_point_stamp - twist_stamp) > 0.1) {
       twist_time_stamp_is_too_late = true;
       v = 0.0f;
       w = 0.0f;
     }
 
     if (use_imu_ && !angular_velocity_queue_.empty()) {
-      while (imu_it != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) {
+      while (imu_it != std::end(angular_velocity_queue_) - 1 && global_point_stamp > imu_stamp) {
         ++imu_it;
         imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds();
       }
 
-      if (std::abs(*it_time_stamp - imu_stamp) > 0.1) {
+      if (std::abs(global_point_stamp - imu_stamp) > 0.1) {
         imu_time_stamp_is_too_late = true;
       } else {
         w = static_cast<float>(imu_it->vector.z);
       }
     }
 
     const auto time_offset = static_cast<float>(*it_time_stamp - prev_time_stamp_sec);
 
     point.setValue(*it_x, *it_y, *it_z);
 
     if (need_transform) {
       point = tf2_base_link_to_sensor_inv * point;
     }
 
     theta += w * time_offset;
     baselink_quat.setValue(
       0, 0, tier4_autoware_utils::sin(theta * 0.5f),
       tier4_autoware_utils::cos(theta * 0.5f));  // baselink_quat.setRPY(0.0, 0.0, theta);
     const float dis = v * time_offset;
     x += dis * tier4_autoware_utils::cos(theta);
     y += dis * tier4_autoware_utils::sin(theta);
 
     baselink_tf_odom.setOrigin(tf2::Vector3(x, y, 0.0));
     baselink_tf_odom.setRotation(baselink_quat);
 
     undistorted_point = baselink_tf_odom * point;
 
     if (need_transform) {
       undistorted_point = tf2_base_link_to_sensor * undistorted_point;
     }
 
     *it_x = static_cast<float>(undistorted_point.getX());
     *it_y = static_cast<float>(undistorted_point.getY());
     *it_z = static_cast<float>(undistorted_point.getZ());
 
-    prev_time_stamp_sec = *it_time_stamp;
+    prev_time_stamp_sec = global_point_stamp;
   }
 
   if (twist_time_stamp_is_too_late) {

sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp

@@ -18,6 +18,7 @@
 
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 
+#include <pcl/for_each_type.h>
 #include <pcl/search/pcl_search.h>
 
 #include <algorithm>
@@ -232,10 +233,10 @@ void RingOutlierFilterComponent::faster_filter(
     }
   }
 
-  setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4);
+  setUpPointCloudFormat(input, output, output_size);
 
   if (publish_outlier_pointcloud_) {
-    setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4);
+    setUpPointCloudFormat(input, outlier_points, outlier_points_size);
     outlier_pointcloud_publisher_->publish(outlier_points);
   }
 
@@ -297,8 +298,7 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba
 }
 
 void RingOutlierFilterComponent::setUpPointCloudFormat(
-  const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size,
-  size_t num_fields)
+  const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size)
 {
   formatted_points.data.resize(points_size);
   // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform
@@ -312,11 +312,15 @@ void RingOutlierFilterComponent::setUpPointCloudFormat(
   formatted_points.is_bigendian = input->is_bigendian;
   formatted_points.is_dense = input->is_dense;
 
-  sensor_msgs::PointCloud2Modifier pcd_modifier(formatted_points);
-  pcd_modifier.setPointCloud2Fields(
-    num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
-    sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32,
-    "intensity", 1, sensor_msgs::msg::PointField::FLOAT32);
+  // pcl::PCLPointCloud2 pcl_aux;
+  sensor_msgs::msg::PointCloud2 msg_aux;
+  // pcl_conversions::moveFromPCL(pcl_pc2, cloud);
+  pcl::toROSMsg(pcl::PointCloud<OutputPointType>(), msg_aux);
+  formatted_points.fields = msg_aux.fields;
+  // pcl::for_each_type<typename pcl::traits::fieldList<OutputPointType>::type>
+  // pcl::detail::FieldAdder<OutputPointType>(formatted_points.fields); pcl::for_each_type<typename
+  // pcl::traits::fieldList<OutputPointType>::type>
+  // (pcl::detail::FieldAdder<OutputPointType>(formatted_points.fields));
 }
 
 }  // namespace pointcloud_preprocessor