diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp index cab62613cc4a7..8a1e7c7464ca2 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp +++ b/sensing/pointcloud_preprocessor/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 index 62969a93b2333..46996ef68e6bf 100644 --- 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(undistorted_point.getY()); *it_z = static_cast(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 index e0c8443cfe57b..710d694155624 100644 --- 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(&input->data[current_data_idx + azimuth_offset]); const float & next_azimuth = *reinterpret_cast(&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(&input->data[current_data_idx + distance_offset]);