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
485 stars 119 forks source link

Fix reported entropy #408

Closed f-fl0 closed 10 months ago

f-fl0 commented 10 months ago

With the current implementation on the master branch, the entropy of the particle filter seems to be always calculated after resampling or resizing the number of particles which reset each particle probability to 1 / num_particles. Thus the reported entropy is always the same. In this PR, this quantity is calculated in the measure function to represent the entropy of the particle filter right after the particles probabilities are updated based on the measurement likelihood.

at-wat commented 10 months ago

[147] 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: 6 tests build/test_results/mcl_3dl/gtest-test_pf.xml: 12 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: 6 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: 130 tests, 0 errors, 0 failures, 0 skipped ```
codecov-commenter commented 10 months ago

Codecov Report

All modified and coverable lines are covered by tests :white_check_mark:

Comparison is base (8d8835f) 91.72% compared to head (14acbab) 91.71%.

Additional details and impacted files ```diff @@ Coverage Diff @@ ## master #408 +/- ## ========================================== - Coverage 91.72% 91.71% -0.02% ========================================== Files 30 30 Lines 1886 1883 -3 ========================================== - Hits 1730 1727 -3 Misses 156 156 ```

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

f-fl0 commented 10 months ago

@at-wat I added some simple tests in 14acbab7216b56363b698515e7abd22d91f673c7 but let me know if you have better suggestions.

at-wat commented 10 months ago

[148] 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: 6 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: 6 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: 132 tests, 0 errors, 0 failures, 0 skipped ```