From 026edf316d4f030c4f572b0747581a1f46fb631c Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 21 May 2024 00:53:19 +0900 Subject: [PATCH] change parameter back to default and do small refactor Signed-off-by: vividf --- .../docs/distortion-corrector.md | 1 + .../distortion_corrector.cpp | 21 +++++++++---------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md index 805bd29f87b73..ed44976cd47e4 100644 --- a/sensing/pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/pointcloud_preprocessor/docs/distortion-corrector.md @@ -45,3 +45,4 @@ Please note that the processing time difference between the two distortion metho ## Assumptions / Known limits - The node requires that time synchronization works well between the pointcloud, twist, and IMU. +- If you want to use 3D distortion corrector without IMU, please check that the linear and angular velocity fields of your twist message are not empty. diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index feeb7c76dac1a..537fbb80a86c1 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -41,7 +41,7 @@ DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOpt // Parameter time_stamp_field_name_ = declare_parameter("time_stamp_field_name", "time_stamp"); use_imu_ = declare_parameter("use_imu", true); - use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction"); + use_3d_distortion_correction_ = declare_parameter("use_3d_distortion_correction", false); // Publisher undistorted_points_pub_ = @@ -267,12 +267,6 @@ bool DistortionCorrectorComponent::undistortPointCloud( // For performance, do not instantiate `rclcpp::Time` inside of the for-loop double twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds(); - // For performance, instantiate outside of the for-loop - tf2::Quaternion baselink_quat{}; - tf2::Transform baselink_tf_odom{}; - tf2::Vector3 point{}; - tf2::Vector3 undistorted_point{}; - // For performance, avoid transform computation if unnecessary bool need_transform = points.header.frame_id != base_link_frame_; @@ -286,10 +280,16 @@ bool DistortionCorrectorComponent::undistortPointCloud( bool twist_time_stamp_is_too_late = false; bool imu_time_stamp_is_too_late = false; + // For performance, instantiate outside of the for-loop + tf2::Quaternion baselink_quat{}; + tf2::Transform baselink_tf_odom{}; + // for 2d motion + tf2::Vector3 point{}; + tf2::Vector3 undistorted_point{}; float theta{0.0f}; - float x{0.0f}; - float y{0.0f}; + float x{0.0f}, y{0.0f}; + float v{0.0f}, w{0.0f}; // for 3d motion Eigen::Vector4f point_eigen; @@ -297,8 +297,7 @@ bool DistortionCorrectorComponent::undistortPointCloud( Eigen::Matrix4f transformation_matrix; Eigen::Matrix4f prev_transformation_matrix = Eigen::Matrix4f::Identity(); - float v_x = 0.0f, v_y = 0.0f, v_z = 0.0f, w_x = 0.0f, w_y = 0.0f, w_z = 0.0f; // 3d motion - float v = 0.0f, w = 0.0f; // 2d motion + float v_x{0.0f}, v_y = {0.0f}, v_z = {0.0f}, w_x = {0.0f}, w_y = {0.0f}, w_z = {0.0f}; 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) {