search

cartographer-project / cartographer

Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.
Apache License 2.0
7.04k stars 2.24k forks source link

Huge remaining work in queue when use imu data #1962

Closed C-002 closed 1 week ago

C-002 commented 2 weeks ago

Hi, I am working on 2D cartographer pure localization mode with 2 lidars, imu and wheel odometry, The localization works with a 0.025 m resolution map. A huge number of "remaining work in queue" is reported by the pose_graph_2d logger. But when setting the use_imu_data=false, the number of "remaining work in queue" can be contained around zero. I am not sure what I am doing wrong. Any advice? Thank you.

output info

sensor rate:

[INFO] [1720429049.037858296] [cartographer logger]: I0708 16:57:29.000000 104893 collated_trajectory_builder.cc:81] odom rate: 20.00 Hz 5.00e-02 s +/- 8.02e-04 s (pulsed at 100.00% real time)
[INFO] [1720429049.037902965] [cartographer logger]: I0708 16:57:29.000000 104893 collated_trajectory_builder.cc:81] scan rate: 34.05 Hz 2.94e-02 s +/- 6.54e-04 s (pulsed at 100.00% real time)
[INFO] [1720408160.936455319] [cartographer logger]: I0708 11:09:20.000000  2700 collated_trajectory_builder.cc:81] imu rate: 20.04 Hz 4.99e-02 s +/- 1.84e-04 s (pulsed at 99.93% real time)
[INFO] [1720408160.936529662] [cartographer logger]: I0708 11:09:20.000000  2700 collated_trajectory_builder.cc:81] odom rate: 20.01 Hz 5.00e-02 s +/- 4.79e-04 s (pulsed at 100.13% real time)
[INFO] [1720408160.936550716] [cartographer logger]: I0708 11:09:20.000000  2700 collated_trajectory_builder.cc:81] scan rate: 34.06 Hz 2.94e-02 s +/- 7.14e-04 s (pulsed at 100.04% real time)

cartographer output:

[INFO] [1720408381.601759610] [cartographer logger]: I0708 11:13:01.000000  2736 pose_graph_2d.cc:538] Remaining work items in queue: 4719
[INFO] [1720408384.028980917] [cartographer logger]: I0708 11:13:04.000000  2700 motion_filter.cc:42] Motion filter reduced the number of nodes to 0.728302%.
[INFO] [1720408386.063300600] [cartographer logger]: I0708 11:13:06.000000  2700 collated_trajectory_builder.cc:81] imu rate: 20.02 Hz 4.99e-02 s +/- 3.23e-04 s (pulsed at 99.92% real time)
[INFO] [1720408386.063340585] [cartographer logger]: I0708 11:13:06.000000  2700 collated_trajectory_builder.cc:81] odom rate: 19.99 Hz 5.00e-02 s +/- 3.87e-04 s (pulsed at 100.03% real time)
[INFO] [1720408386.063361030] [cartographer logger]: I0708 11:13:06.000000  2700 collated_trajectory_builder.cc:81] scan rate: 34.04 Hz 2.94e-02 s +/- 6.78e-04 s (pulsed at 100.20% real time)
[INFO] [1720408398.718274695] [cartographer logger]: I0708 11:13:18.000000  2700 motion_filter.cc:42] Motion filter reduced the number of nodes to 0.718519%.
[INFO] [1720408399.157242435] [cartographer logger]: I0708 11:13:19.000000  2731 constraint_builder_2d.cc:290] 1 computations resulted in 0 additional constraints.
[INFO] [1720408399.157317194] [cartographer logger]: I0708 11:13:19.000000  2731 constraint_builder_2d.cc:292] Score histogram:
Count: 5  Min: 0.459629  Max: 0.516057  Mean: 0.485932
[0.459629, 0.465272)                    ####    Count: 1 (20%)  Total: 1 (20%)
[0.465272, 0.470915)                    ####    Count: 1 (20%)  Total: 2 (40%)
[0.470915, 0.476558)                            Count: 0 (0%)   Total: 2 (40%)
[0.476558, 0.482200)                    ####    Count: 1 (20%)  Total: 3 (60%)
[0.482200, 0.487843)                            Count: 0 (0%)   Total: 3 (60%)
[0.487843, 0.493486)                            Count: 0 (0%)   Total: 3 (60%)
[0.493486, 0.499129)                            Count: 0 (0%)   Total: 3 (60%)
[0.499129, 0.504772)                            Count: 0 (0%)   Total: 3 (60%)
[0.504772, 0.510414)                    ####    Count: 1 (20%)  Total: 4 (80%)
[0.510414, 0.516057]                    ####    Count: 1 (20%)  Total: 5 (100%)
[INFO] [1720408399.177323907] [cartographer logger]: I0708 11:13:19.000000  2731 pose_graph_2d.cc:538] Remaining work items in queue: 5320
[INFO] [1720408401.067137035] [cartographer logger]: I0708 11:13:21.000000  2700 collated_trajectory_builder.cc:81] imu rate: 20.03 Hz 4.99e-02 s +/- 2.96e-04 s (pulsed at 100.10% real time)
[INFO] [1720408401.067175863] [cartographer logger]: I0708 11:13:21.000000  2700 collated_trajectory_builder.cc:81] odom rate: 20.00 Hz 5.00e-02 s +/- 3.55e-04 s (pulsed at 99.81% real time)
[INFO] [1720408401.067194903] [cartographer logger]: I0708 11:13:21.000000  2700 collated_trajectory_builder.cc:81] scan rate: 34.04 Hz 2.94e-02 s +/- 6.77e-04 s (pulsed at 99.93% real time)
[INFO] [1720408405.299193946] [cartographer logger]: I0708 11:13:25.000000  2735 constraint_builder_2d.cc:275] Node (1, 162) with 405 points on submap (0, 13) matches with score 50.6%.
[INFO] [1720408405.299227648] [cartographer logger]: I0708 11:13:25.000000  2735 constraint_builder_2d.cc:290] 1 computations resulted in 1 additional constraints.
[INFO] [1720408405.299246262] [cartographer logger]: I0708 11:13:25.000000  2735 constraint_builder_2d.cc:292] Score histogram:
Count: 6  Min: 0.459629  Max: 0.516057  Mean: 0.489354
[0.459629, 0.465272)                     ###    Count: 1 (16.6667%) Total: 1 (16.6667%)
[0.465272, 0.470915)                     ###    Count: 1 (16.6667%) Total: 2 (33.3333%)
[0.470915, 0.476558)                            Count: 0 (0%)   Total: 2 (33.3333%)
[0.476558, 0.482200)                     ###    Count: 1 (16.6667%) Total: 3 (50%)
[0.482200, 0.487843)                            Count: 0 (0%)   Total: 3 (50%)
[0.487843, 0.493486)                            Count: 0 (0%)   Total: 3 (50%)
[0.493486, 0.499129)                            Count: 0 (0%)   Total: 3 (50%)
[0.499129, 0.504772)                            Count: 0 (0%)   Total: 3 (50%)
[0.504772, 0.510414)                 #######    Count: 2 (33.3333%) Total: 5 (83.3333%)
[0.510414, 0.516057]                     ###    Count: 1 (16.6667%) Total: 6 (100%)
[INFO] [1720408405.337255731] [cartographer logger]: I0708 11:13:25.000000  2735 pose_graph_2d.cc:538] Remaining work items in queue: 5464
[INFO] [1720408405.340009849] [cartographer logger]: I0708 11:13:25.000000  2732 constraint_builder_2d.cc:290] 2 computations resulted in 0 additional constraints.
[INFO] [1720408405.340043701] [cartographer logger]: I0708 11:13:25.000000  2732 constraint_builder_2d.cc:292] Score histogram:
Count: 6  Min: 0.459629  Max: 0.516057  Mean: 0.489354
[0.459629, 0.465272)                     ###    Count: 1 (16.6667%) Total: 1 (16.6667%)
[0.465272, 0.470915)                     ###    Count: 1 (16.6667%) Total: 2 (33.3333%)
[0.470915, 0.476558)                            Count: 0 (0%)   Total: 2 (33.3333%)
[0.476558, 0.482200)                     ###    Count: 1 (16.6667%) Total: 3 (50%)
[0.482200, 0.487843)                            Count: 0 (0%)   Total: 3 (50%)
[0.487843, 0.493486)                            Count: 0 (0%)   Total: 3 (50%)
[0.493486, 0.499129)                            Count: 0 (0%)   Total: 3 (50%)
[0.499129, 0.504772)                            Count: 0 (0%)   Total: 3 (50%)
[0.504772, 0.510414)                 #######    Count: 2 (33.3333%) Total: 5 (83.3333%)
[0.510414, 0.516057]                     ###    Count: 1 (16.6667%) Total: 6 (100%)
[INFO] [1720408405.360257634] [cartographer logger]: I0708 11:13:25.000000  2732 pose_graph_2d.cc:538] Remaining work items in queue: 5363
[INFO] [1720408405.362460679] [cartographer logger]: I0708 11:13:25.000000  2730 constraint_builder_2d.cc:290] 1 computations resulted in 0 additional constraints.
[INFO] [1720408405.362493988] [cartographer logger]: I0708 11:13:25.000000  2730 constraint_builder_2d.cc:292] Score histogram:
Count: 6  Min: 0.459629  Max: 0.516057  Mean: 0.489354
[0.459629, 0.465272)                     ###    Count: 1 (16.6667%) Total: 1 (16.6667%)
[0.465272, 0.470915)                     ###    Count: 1 (16.6667%) Total: 2 (33.3333%)
[0.470915, 0.476558)                            Count: 0 (0%)   Total: 2 (33.3333%)
[0.476558, 0.482200)                     ###    Count: 1 (16.6667%) Total: 3 (50%)
[0.482200, 0.487843)                            Count: 0 (0%)   Total: 3 (50%)
[0.487843, 0.493486)                            Count: 0 (0%)   Total: 3 (50%)
[0.493486, 0.499129)                            Count: 0 (0%)   Total: 3 (50%)
[0.499129, 0.504772)                            Count: 0 (0%)   Total: 3 (50%)
[0.504772, 0.510414)                 #######    Count: 2 (33.3333%) Total: 5 (83.3333%)
[0.510414, 0.516057]                     ###    Count: 1 (16.6667%) Total: 6 (100%)
[INFO] [1720408405.384545825] [cartographer logger]: I0708 11:13:25.000000  2730 pose_graph_2d.cc:538] Remaining work items in queue: 5260
[INFO] [1720408405.387312717] [cartographer logger]: I0708 11:13:25.000000  2736 constraint_builder_2d.cc:290] 2 computations resulted in 0 additional constraints.
[INFO] [1720408405.387343711] [cartographer logger]: I0708 11:13:25.000000  2736 constraint_builder_2d.cc:292] Score histogram:
Count: 6  Min: 0.459629  Max: 0.516057  Mean: 0.489354
[0.459629, 0.465272)                     ###    Count: 1 (16.6667%) Total: 1 (16.6667%)
[0.465272, 0.470915)                     ###    Count: 1 (16.6667%) Total: 2 (33.3333%)
[0.470915, 0.476558)                            Count: 0 (0%)   Total: 2 (33.3333%)
[0.476558, 0.482200)                     ###    Count: 1 (16.6667%) Total: 3 (50%)
[0.482200, 0.487843)                            Count: 0 (0%)   Total: 3 (50%)
[0.487843, 0.493486)                            Count: 0 (0%)   Total: 3 (50%)
[0.493486, 0.499129)                            Count: 0 (0%)   Total: 3 (50%)
[0.499129, 0.504772)                            Count: 0 (0%)   Total: 3 (50%)
[0.504772, 0.510414)                 #######    Count: 2 (33.3333%) Total: 5 (83.3333%)
[0.510414, 0.516057]                     ###    Count: 1 (16.6667%) Total: 6 (100%)
[INFO] [1720408405.407074810] [cartographer logger]: I0708 11:13:25.000000  2736 pose_graph_2d.cc:538] Remaining work items in queue: 5122

cartographer localization configure

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 180
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.025
TRAJECTORY_BUILDER_2D.min_range = 0.0
TRAJECTORY_BUILDER_2D.max_range = 13.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.

TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 1. 
TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 30.
TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 400
TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.025

TRAJECTORY_BUILDER_2D.use_imu_data = false --true

TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 15. 
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.3
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(15.)

TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3,
}

POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 3
POSE_GRAPH.constraint_builder.max_constraint_distance = 5
POSE_GRAPH.constraint_builder.sampling_ratio = 0.2
POSE_GRAPH.constraint_builder.min_score = 0.55
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.45

POSE_GRAPH.constraint_builder.ceres_scan_matcher.ceres_solver_options.num_threads = 5
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.linear_search_window = 5.0
POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher.angular_search_window = math.rad(15.0)
POSE_GRAPH.optimization_problem.ceres_solver_options.num_threads = 7

POSE_GRAPH.global_sampling_ratio = 0.01
POSE_GRAPH.global_constraint_search_after_n_seconds = 15.
kscottz commented 1 week ago

Just a heads up, this project really isn't actively maintained. If you want send a PR to address the issue we might be able to merge it.