modify ring outlier filter node for supporting counter clockwise azimuth

Signed-off-by: vividf <[email protected]>
autowarefoundation · Mar 14, 2024 · 55682dd · 55682dd
1 parent fb2ab65
commit 55682dd
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 4 deletions.
diff --git a/...reprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/...reprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp
@@ -83,6 +83,7 @@ class DistortionCorrectorComponent : public rclcpp::Node
   std::string time_stamp_field_name_;
   bool use_imu_;
   bool update_azimuth_and_distance_;
+  float azimuth_factor_ = 100.0;
 };
 
 }  // namespace pointcloud_preprocessor

diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -315,9 +315,9 @@ bool DistortionCorrectorComponent::undistortPointCloud(
     *it_y = static_cast<float>(undistorted_point.getY());
     *it_z = static_cast<float>(undistorted_point.getZ());
 
-    if (update_azimuth_and_distance_) {
+    if (update_azimuth_and_distance_ && need_transform) {
       *it_distance = sqrt(*it_x * *it_x + *it_y * *it_y + *it_z * *it_z);
-      *it_azimuth = cv::fastAtan2(*it_y, *it_x) * 100;
+      *it_azimuth = cv::fastAtan2(*it_y, *it_x) * azimuth_factor_;
     }
     prev_time_stamp_sec = *it_time_stamp;
   }

diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp
@@ -116,8 +116,8 @@ void RingOutlierFilterComponent::faster_filter(
         *reinterpret_cast<const float *>(&input->data[current_data_idx + azimuth_offset]);
       const float & next_azimuth =
         *reinterpret_cast<const float *>(&input->data[next_data_idx + azimuth_offset]);
-      float azimuth_diff = next_azimuth - current_azimuth;
-      azimuth_diff = azimuth_diff < 0.f ? azimuth_diff + 36000.f : azimuth_diff;
+      float azimuth_diff = std::abs(next_azimuth - current_azimuth);
+      azimuth_diff = azimuth_diff > 18000.f ? 36000.f - azimuth_diff : azimuth_diff;
 
       const float & current_distance =
         *reinterpret_cast<const float *>(&input->data[current_data_idx + distance_offset]);