fix: correct azimuth offset (#112)

* fix: correct azimuth offset for vls128 Signed-off-by: badai-nguyen <[email protected]> * fix: correct azimuth offset for vlp16 Signed-off-by: badai-nguyen <[email protected]> * chore: update test's PCD Signed-off-by: badai-nguyen <[email protected]> --------- Signed-off-by: badai-nguyen <[email protected]> Co-authored-by: Abraham Monrroy Cano <[email protected]>
tier4 · Mar 19, 2024 · 5e587ba · 5e587ba
1 parent 99cb035
commit 5e587ba
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 16 deletions.
diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp
@@ -231,7 +231,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
               const float azimuth_corrected_f =
                 azimuth +
                 (azimuth_diff * ((dsr * VLP16_DSR_TOFFSET) + (firing * VLP16_FIRING_TOFFSET)) /
-                 VLP16_BLOCK_DURATION);
+                 VLP16_BLOCK_DURATION) - corrections.rot_correction * 180.0 / M_PI * 100;
               const uint16_t azimuth_corrected =
                 (static_cast<uint16_t>(round(azimuth_corrected_f))) % 36000;
 
@@ -247,13 +247,8 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
                 // Convert polar coordinates to Euclidean XYZ.
                 const float cos_vert_angle = corrections.cos_vert_correction;
                 const float sin_vert_angle = corrections.sin_vert_correction;
-                const float cos_rot_correction = corrections.cos_rot_correction;
-                const float sin_rot_correction = corrections.sin_rot_correction;
-
-                const float cos_rot_angle = cos_rot_table_[azimuth_corrected] * cos_rot_correction +
-                                            sin_rot_table_[azimuth_corrected] * sin_rot_correction;
-                const float sin_rot_angle = sin_rot_table_[azimuth_corrected] * cos_rot_correction -
-                                            cos_rot_table_[azimuth_corrected] * sin_rot_correction;
+                const float cos_rot_angle = cos_rot_table_[azimuth_corrected];
+                const float sin_rot_angle = sin_rot_table_[azimuth_corrected];
 
                 // Compute the distance in the xy plane (w/o accounting for rotation).
                 const float xy_distance = distance * cos_vert_angle;

diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp
@@ -250,7 +250,8 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p
 
           // Correct for the laser rotation as a function of timing during the firings.
           const float azimuth_corrected_f =
-            azimuth + (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]);
+            azimuth + (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]) 
+            - corrections.rot_correction * 180.0 / M_PI * 100;
           const uint16_t azimuth_corrected = ((uint16_t)round(azimuth_corrected_f)) % 36000;
 
           if (
@@ -268,13 +269,8 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p
               // convert polar coordinates to Euclidean XYZ.
               const float cos_vert_angle = corrections.cos_vert_correction;
               const float sin_vert_angle = corrections.sin_vert_correction;
-              const float cos_rot_correction = corrections.cos_rot_correction;
-              const float sin_rot_correction = corrections.sin_rot_correction;
-
-              const float cos_rot_angle = cos_rot_table_[azimuth_corrected] * cos_rot_correction +
-                                          sin_rot_table_[azimuth_corrected] * sin_rot_correction;
-              const float sin_rot_angle = sin_rot_table_[azimuth_corrected] * cos_rot_correction -
-                                          cos_rot_table_[azimuth_corrected] * sin_rot_correction;
+              const float cos_rot_angle = cos_rot_table_[azimuth_corrected];
+              const float sin_rot_angle = sin_rot_table_[azimuth_corrected];
 
               // Compute the distance in the xy plane (w/o accounting for rotation).
               const float xy_distance = distance * cos_vert_angle;

diff --git a/nebula_tests/data/velodyne/vls128/1614315746748918706.pcd b/nebula_tests/data/velodyne/vls128/1614315746748918706.pcd