search

at-wat / mcl_3dl

A ROS node to perform a probabilistic 3-D/6-DOF localization system for mobile robots with 3-D LIDAR(s). It implements pointcloud based Monte Carlo localization that uses a reference pointcloud as a map.
BSD 3-Clause "New" or "Revised" License
502 stars 118 forks source link

Fix angular part of covariance matrix #417

Closed f-fl0 closed 1 month ago

f-fl0 commented 1 month ago

In https://github.com/at-wat/mcl_3dl/blob/361a57f6c0ae5b1797f52827856f7829d6c9f0d2/src/mcl_3dl.cpp#L707-L750, cov is a 13x13 matrix which contains the covariance information about the 3D position and the orientation in quaternion (top left 7x7 sub matrix from cov) but it is used as a 6x6 matrix representing the covariance of the 3D position and the orientation in Euler angles (roll pitch yaw). Thus the covariance matrix reported in /amcl_pose is incorrect as well as the subsequent checks on cov e.g. cov[5][5] does not represent to variance of yaw. To fix this issue, the covariance matrix calculations are now done using the Euler angles directly rather than quaternion by overriding covElement in State6DOF.

at-wat commented 1 month ago

[165] PASSED on noetic

All tests passed ``` build/test_results/mcl_3dl/gtest-test_chunked_kdtree.xml: 2 tests build/test_results/mcl_3dl/gtest-test_cloud_accum.xml: 4 tests build/test_results/mcl_3dl/gtest-test_filter.xml: 6 tests build/test_results/mcl_3dl/gtest-test_imu_measurement_model_gravity.xml: 2 tests build/test_results/mcl_3dl/gtest-test_motion_prediction_model_differential_drive.xml: 8 tests build/test_results/mcl_3dl/gtest-test_nd.xml: 4 tests build/test_results/mcl_3dl/gtest-test_noise_generator.xml: 8 tests build/test_results/mcl_3dl/gtest-test_pf.xml: 14 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler.xml: 2 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler_with_normal.xml: 2 tests build/test_results/mcl_3dl/gtest-test_point_types.xml: 2 tests build/test_results/mcl_3dl/gtest-test_quat.xml: 8 tests build/test_results/mcl_3dl/gtest-test_raycast.xml: 6 tests build/test_results/mcl_3dl/gtest-test_raycast_dda.xml: 8 tests build/test_results/mcl_3dl/gtest-test_state_6dof.xml: 8 tests build/test_results/mcl_3dl/gtest-test_vec3.xml: 10 tests build/test_results/mcl_3dl/roslint-mcl_3dl.xml: 1 tests build/test_results/mcl_3dl/rostest-localization_rostest__without_imu_true.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_beam_label_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_beam_likelihood_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_debug_output_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_expansion_resetting_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_global_localization_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_landmark_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_localization_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_mcl_3dl_compat_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_tf_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_transform_rostest.xml: 1 tests build/test_results/mcl_3dl/rosunit-compare_pose.xml: 2 tests build/test_results/mcl_3dl/rosunit-compare_tf.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_beam_label.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_beam_likelihood.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_debug_output.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_expansion_resetting.xml: 4 tests build/test_results/mcl_3dl/rosunit-test_global_localization.xml: 4 tests build/test_results/mcl_3dl/rosunit-test_landmark.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat.xml: 8 tests build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests Summary: 136 tests, 0 errors, 0 failures, 0 skipped ```
codecov-commenter commented 1 month ago

Codecov Report

Attention: Patch coverage is 96.77419% with 1 line in your changes missing coverage. Please review.

Project coverage is 92.15%. Comparing base (361a57f) to head (f3a12d7). Report is 1 commits behind head on master.

Files with missing lines Patch % Lines
include/mcl_3dl/state_6dof.h 93.75% 1 Missing :warning:
Additional details and impacted files ```diff @@ Coverage Diff @@ ## master #417 +/- ## ========================================== - Coverage 92.17% 92.15% -0.02% ========================================== Files 30 30 Lines 2134 2155 +21 ========================================== + Hits 1967 1986 +19 - Misses 167 169 +2 ```

:umbrella: View full report in Codecov by Sentry.
:loudspeaker: Have feedback on the report? Share it here.

at-wat commented 1 month ago

[166] PASSED on noetic

All tests passed ``` build/test_results/mcl_3dl/gtest-test_chunked_kdtree.xml: 2 tests build/test_results/mcl_3dl/gtest-test_cloud_accum.xml: 4 tests build/test_results/mcl_3dl/gtest-test_filter.xml: 6 tests build/test_results/mcl_3dl/gtest-test_imu_measurement_model_gravity.xml: 2 tests build/test_results/mcl_3dl/gtest-test_motion_prediction_model_differential_drive.xml: 8 tests build/test_results/mcl_3dl/gtest-test_nd.xml: 4 tests build/test_results/mcl_3dl/gtest-test_noise_generator.xml: 8 tests build/test_results/mcl_3dl/gtest-test_pf.xml: 14 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler.xml: 2 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler_with_normal.xml: 2 tests build/test_results/mcl_3dl/gtest-test_point_types.xml: 2 tests build/test_results/mcl_3dl/gtest-test_quat.xml: 8 tests build/test_results/mcl_3dl/gtest-test_raycast.xml: 6 tests build/test_results/mcl_3dl/gtest-test_raycast_dda.xml: 8 tests build/test_results/mcl_3dl/gtest-test_state_6dof.xml: 8 tests build/test_results/mcl_3dl/gtest-test_vec3.xml: 10 tests build/test_results/mcl_3dl/roslint-mcl_3dl.xml: 1 tests build/test_results/mcl_3dl/rostest-localization_rostest__without_imu_true.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_beam_label_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_beam_likelihood_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_debug_output_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_expansion_resetting_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_global_localization_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_landmark_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_localization_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_mcl_3dl_compat_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_tf_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_transform_rostest.xml: 1 tests build/test_results/mcl_3dl/rosunit-compare_pose.xml: 2 tests build/test_results/mcl_3dl/rosunit-compare_tf.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_beam_label.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_beam_likelihood.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_debug_output.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_expansion_resetting.xml: 4 tests build/test_results/mcl_3dl/rosunit-test_global_localization.xml: 4 tests build/test_results/mcl_3dl/rosunit-test_landmark.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat.xml: 8 tests build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests Summary: 136 tests, 0 errors, 0 failures, 0 skipped ```
at-wat commented 1 month ago

[167] PASSED on noetic

All tests passed ``` build/test_results/mcl_3dl/gtest-test_chunked_kdtree.xml: 2 tests build/test_results/mcl_3dl/gtest-test_cloud_accum.xml: 4 tests build/test_results/mcl_3dl/gtest-test_filter.xml: 6 tests build/test_results/mcl_3dl/gtest-test_imu_measurement_model_gravity.xml: 2 tests build/test_results/mcl_3dl/gtest-test_motion_prediction_model_differential_drive.xml: 8 tests build/test_results/mcl_3dl/gtest-test_nd.xml: 4 tests build/test_results/mcl_3dl/gtest-test_noise_generator.xml: 8 tests build/test_results/mcl_3dl/gtest-test_pf.xml: 14 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler.xml: 2 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler_with_normal.xml: 2 tests build/test_results/mcl_3dl/gtest-test_point_types.xml: 2 tests build/test_results/mcl_3dl/gtest-test_quat.xml: 8 tests build/test_results/mcl_3dl/gtest-test_raycast.xml: 6 tests build/test_results/mcl_3dl/gtest-test_raycast_dda.xml: 8 tests build/test_results/mcl_3dl/gtest-test_state_6dof.xml: 8 tests build/test_results/mcl_3dl/gtest-test_vec3.xml: 10 tests build/test_results/mcl_3dl/roslint-mcl_3dl.xml: 1 tests build/test_results/mcl_3dl/rostest-localization_rostest__without_imu_true.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_beam_label_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_beam_likelihood_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_debug_output_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_expansion_resetting_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_global_localization_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_landmark_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_localization_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_mcl_3dl_compat_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_tf_rostest.xml: 1 tests build/test_results/mcl_3dl/rostest-test_tests_transform_rostest.xml: 1 tests build/test_results/mcl_3dl/rosunit-compare_pose.xml: 2 tests build/test_results/mcl_3dl/rosunit-compare_tf.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_beam_label.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_beam_likelihood.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_debug_output.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_expansion_resetting.xml: 4 tests build/test_results/mcl_3dl/rosunit-test_global_localization.xml: 4 tests build/test_results/mcl_3dl/rosunit-test_landmark.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat.xml: 8 tests build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests Summary: 136 tests, 0 errors, 0 failures, 0 skipped ```