From af17cf5a2751f7fe0bb2063e9ebe855bbc3cb61a Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 17 Nov 2017 13:03:43 +0100 Subject: [PATCH] Follow googlecartographer/cartographer#686. (#605) --- .../cartographer_ros/configuration_files_test.cc | 11 +++++------ cartographer_ros/configuration_files/backpack_2d.lua | 2 +- .../configuration_files/backpack_2d_localization.lua | 2 +- cartographer_ros/configuration_files/backpack_3d.lua | 4 ++-- .../configuration_files/backpack_3d_localization.lua | 2 +- cartographer_ros/configuration_files/revo_lds.lua | 2 +- .../configuration_files/taurob_tracker.lua | 4 ++-- docs/source/faq.rst | 4 ++-- 8 files changed, 15 insertions(+), 16 deletions(-) diff --git a/cartographer_ros/cartographer_ros/configuration_files_test.cc b/cartographer_ros/cartographer_ros/configuration_files_test.cc index e327e8a..232960a 100644 --- a/cartographer_ros/cartographer_ros/configuration_files_test.cc +++ b/cartographer_ros/cartographer_ros/configuration_files_test.cc @@ -34,12 +34,11 @@ TEST_P(ConfigurationFilesTest, ValidateNodeOptions) { }); } -INSTANTIATE_TEST_CASE_P(ValidateAllNodeOptions, ConfigurationFilesTest, - ::testing::Values("backpack_2d.lua", - "backpack_2d_localization.lua", - "backpack_3d.lua", "pr2.lua", - "revo_lds.lua", - "taurob_tracker.lua")); +INSTANTIATE_TEST_CASE_P( + ValidateAllNodeOptions, ConfigurationFilesTest, + ::testing::Values("backpack_2d.lua", "backpack_2d_localization.lua", + "backpack_3d.lua", "backpack_3d_localization.lua", + "pr2.lua", "revo_lds.lua", "taurob_tracker.lua")); } // namespace } // namespace cartographer_ros diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 997c55d..d6d89ee 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -38,6 +38,6 @@ options = { } MAP_BUILDER.use_trajectory_builder_2d = true -TRAJECTORY_BUILDER_2D.scans_per_accumulation = 10 +TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10 return options diff --git a/cartographer_ros/configuration_files/backpack_2d_localization.lua b/cartographer_ros/configuration_files/backpack_2d_localization.lua index 49ef3d7..9ecb218 100644 --- a/cartographer_ros/configuration_files/backpack_2d_localization.lua +++ b/cartographer_ros/configuration_files/backpack_2d_localization.lua @@ -15,6 +15,6 @@ include "backpack_2d.lua" TRAJECTORY_BUILDER.pure_localization = true -POSE_GRAPH.optimize_every_n_scans = 20 +POSE_GRAPH.optimize_every_n_nodes = 20 return options diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index e11fbd4..123543f 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -37,12 +37,12 @@ options = { imu_sampling_ratio = 1., } -TRAJECTORY_BUILDER_3D.scans_per_accumulation = 160 +TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 160 MAP_BUILDER.use_trajectory_builder_3d = true MAP_BUILDER.num_background_threads = 7 POSE_GRAPH.optimization_problem.huber_scale = 5e2 -POSE_GRAPH.optimize_every_n_scans = 320 +POSE_GRAPH.optimize_every_n_nodes = 320 POSE_GRAPH.constraint_builder.sampling_ratio = 0.03 POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10 POSE_GRAPH.constraint_builder.min_score = 0.62 diff --git a/cartographer_ros/configuration_files/backpack_3d_localization.lua b/cartographer_ros/configuration_files/backpack_3d_localization.lua index baea15d..e1f4e9f 100644 --- a/cartographer_ros/configuration_files/backpack_3d_localization.lua +++ b/cartographer_ros/configuration_files/backpack_3d_localization.lua @@ -15,6 +15,6 @@ include "backpack_3d.lua" TRAJECTORY_BUILDER.pure_localization = true -POSE_GRAPH.optimize_every_n_scans = 100 +POSE_GRAPH.optimize_every_n_nodes = 100 return options diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index fb328e0..3ec0c4f 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/cartographer_ros/configuration_files/revo_lds.lua @@ -50,7 +50,7 @@ TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_ TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1 POSE_GRAPH.optimization_problem.huber_scale = 1e2 -POSE_GRAPH.optimize_every_n_scans = 35 +POSE_GRAPH.optimize_every_n_nodes = 35 POSE_GRAPH.constraint_builder.min_score = 0.65 return options diff --git a/cartographer_ros/configuration_files/taurob_tracker.lua b/cartographer_ros/configuration_files/taurob_tracker.lua index c4688f8..7d555fd 100644 --- a/cartographer_ros/configuration_files/taurob_tracker.lua +++ b/cartographer_ros/configuration_files/taurob_tracker.lua @@ -37,7 +37,7 @@ options = { imu_sampling_ratio = 1., } -TRAJECTORY_BUILDER_3D.scans_per_accumulation = 180 +TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 180 TRAJECTORY_BUILDER_3D.min_range = 0.5 TRAJECTORY_BUILDER_3D.max_range = 20. TRAJECTORY_BUILDER_3D.submaps.num_range_data = 40. @@ -45,7 +45,7 @@ TRAJECTORY_BUILDER_3D.submaps.num_range_data = 40. MAP_BUILDER.use_trajectory_builder_3d = true MAP_BUILDER.num_background_threads = 7 POSE_GRAPH.optimization_problem.huber_scale = 5e2 -POSE_GRAPH.optimize_every_n_scans = 40 +POSE_GRAPH.optimize_every_n_nodes = 40 POSE_GRAPH.constraint_builder.sampling_ratio = 0.03 POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10 POSE_GRAPH.constraint_builder.min_score = 0.62 diff --git a/docs/source/faq.rst b/docs/source/faq.rst index be13c49..ce72a7a 100644 --- a/docs/source/faq.rst +++ b/docs/source/faq.rst @@ -27,8 +27,8 @@ per UDP packet, not one per revolution. __ http://www.ros.org/doc/api/sensor_msgs/html/msg/PointCloud2.html In the `corresponding Cartographer configuration file`__ you see -`TRAJECTORY_BUILDER_3D.scans_per_accumulation = 160` which means we accumulate -160 per-UDP-packet point clouds into one larger point cloud, which +`TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 160` which means we +accumulate 160 per-UDP-packet point clouds into one larger point cloud, which incorporates motion estimation by combining constant velocity and IMU measurements, for matching. Since there are two VLP-16s, 160 UDP packets is enough for roughly 2 revolutions, one per VLP-16.