Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix resampling failure of last particle #313

Merged
merged 2 commits into from May 8, 2020
Merged

Conversation

nhatao
Copy link
Collaborator

@nhatao nhatao commented May 8, 2020

In the master branch, last particle is not removed even when its probability is small enough.
The algorithm described in Probabilistic Robotics is implemented.

@at-wat
Copy link
Owner

at-wat commented May 8, 2020

[#790] PASSED on noetic

Tested on Alpine ROS


[#790] PASSED on kinetic

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_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_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_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_incompatible_rostest.xml: 2 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_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: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests
Summary: 114 tests, 0 errors, 0 failures, 0 skipped

[#790] PASSED on melodic

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_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_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_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_incompatible_rostest.xml: 2 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_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: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests
Summary: 114 tests, 0 errors, 0 failures, 0 skipped

@nhatao nhatao requested a review from at-wat May 8, 2020 04:06
@nhatao
Copy link
Collaborator Author

nhatao commented May 8, 2020

@at-wat PTAL.

Copy link
Owner

@at-wat at-wat left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Added a comment, otherwise looks good.

{
1.0f, 2.0f, 3.0f, 4.0f, 4.0f
};
testResample(probs, states, expected_resampled_states);
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done

@nhatao nhatao requested a review from at-wat May 8, 2020 05:41
@at-wat
Copy link
Owner

at-wat commented May 8, 2020

[#791] PASSED on noetic

Tested on Alpine ROS


[#791] PASSED on melodic

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_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_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_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_incompatible_rostest.xml: 2 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_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: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests
Summary: 114 tests, 0 errors, 0 failures, 0 skipped

[#791] PASSED on kinetic

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_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_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_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_incompatible_rostest.xml: 2 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_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: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 2 tests
build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 2 tests
Summary: 114 tests, 0 errors, 0 failures, 0 skipped

@codecov-io
Copy link

Codecov Report

Merging #313 into master will decrease coverage by 0.05%.
The diff coverage is 100.00%.

Impacted file tree graph

@@            Coverage Diff             @@
##           master     #313      +/-   ##
==========================================
- Coverage   94.51%   94.46%   -0.06%     
==========================================
  Files          32       32              
  Lines        1642     1643       +1     
==========================================
  Hits         1552     1552              
- Misses         90       91       +1     
Impacted Files Coverage Δ
include/mcl_3dl/pf.h 97.81% <100.00%> (+0.01%) ⬆️
src/mcl_3dl.cpp 91.25% <0.00%> (-0.15%) ⬇️

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 94eeb00...622d3e2. Read the comment docs.

@at-wat at-wat merged commit e71df9e into master May 8, 2020
@at-wat at-wat deleted the fix-particle-resampling branch May 8, 2020 05:54
@at-wat at-wat mentioned this pull request May 8, 2020
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

3 participants