change parameter back to default and do small refactor

Signed-off-by: vividf <[email protected]>

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.

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<bool>("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,19 +280,24 @@ 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;
   Eigen::Vector4f undistorted_point_eigen;
   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) {