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

Speed-up global localization #248

Closed at-wat closed 5 years ago

at-wat commented 5 years ago

for #247 Roughly 15% of time in the example https://github.com/at-wat/mcl_3dl/issues/247#issue-475476599 is reduced.

at-wat commented 5 years ago

[#646] FAILED on kinetic

catkin_make failed


[#646] PASSED on melodic

All tests passed ``` build/test_results/mcl_3dl/gtest-test_chunked_kdtree.xml: 1 tests build/test_results/mcl_3dl/gtest-test_filter.xml: 3 tests build/test_results/mcl_3dl/gtest-test_imu_measurement_model_gravity.xml: 1 tests build/test_results/mcl_3dl/gtest-test_motion_prediction_model_differential_drive.xml: 4 tests build/test_results/mcl_3dl/gtest-test_nd.xml: 2 tests build/test_results/mcl_3dl/gtest-test_pf.xml: 6 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler.xml: 1 tests build/test_results/mcl_3dl/gtest-test_point_types.xml: 1 tests build/test_results/mcl_3dl/gtest-test_quat.xml: 4 tests build/test_results/mcl_3dl/gtest-test_raycast.xml: 3 tests build/test_results/mcl_3dl/gtest-test_state_6dof.xml: 3 tests build/test_results/mcl_3dl/gtest-test_vec3.xml: 5 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: 1 tests build/test_results/mcl_3dl/rosunit-compare_tf.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_debug_output.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_expansion_resetting.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_global_localization.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_landmark.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 1 tests Summary: 57 tests, 0 errors, 0 failures, 0 skipped ```
codecov-io commented 5 years ago

Codecov Report

Merging #248 into master will increase coverage by 0.13%. The diff coverage is 100%.

Impacted file tree graph

@@            Coverage Diff             @@
##           master     #248      +/-   ##
==========================================
+ Coverage   94.22%   94.36%   +0.13%     
==========================================
  Files          25       21       -4     
  Lines        1559     1491      -68     
==========================================
- Hits         1469     1407      -62     
+ Misses         90       84       -6
Impacted Files Coverage Δ
include/mcl_3dl/chunked_kdtree.h 94.93% <100%> (ø) :arrow_up:
include/mcl_3dl/pf.h 97.67% <100%> (+0.11%) :arrow_up:
src/mcl_3dl.cpp 92.46% <100%> (+0.04%) :arrow_up:
include/mcl_3dl/raycast.h 100% <0%> (ø) :arrow_up:
include/mcl_3dl/lidar_measurement_model_base.h
include/mcl_3dl/motion_prediction_model_base.h
include/mcl_3dl/imu_measurement_model_base.h
include/pcl18_backports/voxel_grid.h

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 18a8828...007ad29. Read the comment docs.

at-wat commented 5 years ago

[#647] FAILED on kinetic

catkin_make failed


[#647] PASSED on melodic

All tests passed ``` build/test_results/mcl_3dl/gtest-test_chunked_kdtree.xml: 1 tests build/test_results/mcl_3dl/gtest-test_filter.xml: 3 tests build/test_results/mcl_3dl/gtest-test_imu_measurement_model_gravity.xml: 1 tests build/test_results/mcl_3dl/gtest-test_motion_prediction_model_differential_drive.xml: 4 tests build/test_results/mcl_3dl/gtest-test_nd.xml: 2 tests build/test_results/mcl_3dl/gtest-test_pf.xml: 6 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler.xml: 1 tests build/test_results/mcl_3dl/gtest-test_point_types.xml: 1 tests build/test_results/mcl_3dl/gtest-test_quat.xml: 4 tests build/test_results/mcl_3dl/gtest-test_raycast.xml: 3 tests build/test_results/mcl_3dl/gtest-test_state_6dof.xml: 3 tests build/test_results/mcl_3dl/gtest-test_vec3.xml: 5 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: 1 tests build/test_results/mcl_3dl/rosunit-compare_tf.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_debug_output.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_expansion_resetting.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_global_localization.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_landmark.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 1 tests Summary: 57 tests, 0 errors, 0 failures, 0 skipped ```
at-wat commented 5 years ago

[#648] PASSED on kinetic

All tests passed ``` build/test_results/mcl_3dl/gtest-test_chunked_kdtree.xml: 1 tests build/test_results/mcl_3dl/gtest-test_filter.xml: 3 tests build/test_results/mcl_3dl/gtest-test_imu_measurement_model_gravity.xml: 1 tests build/test_results/mcl_3dl/gtest-test_motion_prediction_model_differential_drive.xml: 4 tests build/test_results/mcl_3dl/gtest-test_nd.xml: 2 tests build/test_results/mcl_3dl/gtest-test_pf.xml: 6 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler.xml: 1 tests build/test_results/mcl_3dl/gtest-test_point_types.xml: 1 tests build/test_results/mcl_3dl/gtest-test_quat.xml: 4 tests build/test_results/mcl_3dl/gtest-test_raycast.xml: 3 tests build/test_results/mcl_3dl/gtest-test_state_6dof.xml: 3 tests build/test_results/mcl_3dl/gtest-test_vec3.xml: 5 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: 1 tests build/test_results/mcl_3dl/rosunit-compare_tf.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_debug_output.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_expansion_resetting.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_global_localization.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_landmark.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 1 tests Summary: 57 tests, 0 errors, 0 failures, 0 skipped ```

[#648] PASSED on melodic

All tests passed ``` build/test_results/mcl_3dl/gtest-test_chunked_kdtree.xml: 1 tests build/test_results/mcl_3dl/gtest-test_filter.xml: 3 tests build/test_results/mcl_3dl/gtest-test_imu_measurement_model_gravity.xml: 1 tests build/test_results/mcl_3dl/gtest-test_motion_prediction_model_differential_drive.xml: 4 tests build/test_results/mcl_3dl/gtest-test_nd.xml: 2 tests build/test_results/mcl_3dl/gtest-test_pf.xml: 6 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler.xml: 1 tests build/test_results/mcl_3dl/gtest-test_point_types.xml: 1 tests build/test_results/mcl_3dl/gtest-test_quat.xml: 4 tests build/test_results/mcl_3dl/gtest-test_raycast.xml: 3 tests build/test_results/mcl_3dl/gtest-test_state_6dof.xml: 3 tests build/test_results/mcl_3dl/gtest-test_vec3.xml: 5 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: 1 tests build/test_results/mcl_3dl/rosunit-compare_tf.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_debug_output.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_expansion_resetting.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_global_localization.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_landmark.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 1 tests Summary: 57 tests, 0 errors, 0 failures, 0 skipped ```
at-wat commented 5 years ago

@nhatao please take a look

at-wat commented 5 years ago

[#649] PASSED on melodic

All tests passed ``` build/test_results/mcl_3dl/gtest-test_chunked_kdtree.xml: 1 tests build/test_results/mcl_3dl/gtest-test_filter.xml: 3 tests build/test_results/mcl_3dl/gtest-test_imu_measurement_model_gravity.xml: 1 tests build/test_results/mcl_3dl/gtest-test_motion_prediction_model_differential_drive.xml: 4 tests build/test_results/mcl_3dl/gtest-test_nd.xml: 2 tests build/test_results/mcl_3dl/gtest-test_pf.xml: 6 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler.xml: 1 tests build/test_results/mcl_3dl/gtest-test_point_types.xml: 1 tests build/test_results/mcl_3dl/gtest-test_quat.xml: 4 tests build/test_results/mcl_3dl/gtest-test_raycast.xml: 3 tests build/test_results/mcl_3dl/gtest-test_state_6dof.xml: 3 tests build/test_results/mcl_3dl/gtest-test_vec3.xml: 5 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: 1 tests build/test_results/mcl_3dl/rosunit-compare_tf.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_debug_output.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_expansion_resetting.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_global_localization.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_landmark.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 1 tests Summary: 57 tests, 0 errors, 0 failures, 0 skipped ```

[#649] PASSED on kinetic

All tests passed ``` build/test_results/mcl_3dl/gtest-test_chunked_kdtree.xml: 1 tests build/test_results/mcl_3dl/gtest-test_filter.xml: 3 tests build/test_results/mcl_3dl/gtest-test_imu_measurement_model_gravity.xml: 1 tests build/test_results/mcl_3dl/gtest-test_motion_prediction_model_differential_drive.xml: 4 tests build/test_results/mcl_3dl/gtest-test_nd.xml: 2 tests build/test_results/mcl_3dl/gtest-test_pf.xml: 6 tests build/test_results/mcl_3dl/gtest-test_point_cloud_random_sampler.xml: 1 tests build/test_results/mcl_3dl/gtest-test_point_types.xml: 1 tests build/test_results/mcl_3dl/gtest-test_quat.xml: 4 tests build/test_results/mcl_3dl/gtest-test_raycast.xml: 3 tests build/test_results/mcl_3dl/gtest-test_state_6dof.xml: 3 tests build/test_results/mcl_3dl/gtest-test_vec3.xml: 5 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: 1 tests build/test_results/mcl_3dl/rosunit-compare_tf.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_debug_output.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_expansion_resetting.xml: 2 tests build/test_results/mcl_3dl/rosunit-test_global_localization.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_landmark.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_mcl_3dl_compat_incompatible2.xml: 1 tests build/test_results/mcl_3dl/rosunit-test_transform_failure.xml: 1 tests Summary: 57 tests, 0 errors, 0 failures, 0 skipped ```