fix(voxel_grid_downsample_filter): support no intensity case (autowar…

…efoundation#4922) fix: support for no intensity field Signed-off-by: atsushi421 <[email protected]>
tier4 · Sep 15, 2023 · f7322de · f7322de
1 parent b2af8a5
commit f7322de
Showing 1 changed file with 6 additions and 1 deletion.
diff --git a/...ing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/...ing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp
@@ -36,7 +36,12 @@ void FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPt
   x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset;
   y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset;
   z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset;
-  intensity_offset_ = input->fields[pcl::getFieldIndex(*input, "intensity")].offset;
+  int intensity_index = pcl::getFieldIndex(*input, "intensity");
+  if (intensity_index != -1) {
+    intensity_offset_ = input->fields[intensity_index].offset;
+  } else {
+    intensity_offset_ = z_offset_ + sizeof(float);
+  }
   offset_initialized_ = true;
 }