feat(slam): add cartographer_ros

2025-07-04 14:39:48 +08:00
parent df9cee5779
commit 5cf886315d
825 changed files with 92220 additions and 0 deletions
@@ -0,0 +1,23 @@
+-- Copyright 2016 The Cartographer Authors
+--
+-- Licensed under the Apache License, Version 2.0 (the "License");
+-- you may not use this file except in compliance with the License.
+-- You may obtain a copy of the License at
+--
+--      http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS,
+-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+-- See the License for the specific language governing permissions and
+-- limitations under the License.
+
+include "pose_graph.lua"
+
+MAP_BUILDER = {
+  use_trajectory_builder_2d = false,
+  use_trajectory_builder_3d = false,
+  num_background_threads = 4,
+  pose_graph = POSE_GRAPH,
+  collate_by_trajectory = false,
+}
@@ -0,0 +1,28 @@
+-- Copyright 2017 The Cartographer Authors
+--
+-- Licensed under the Apache License, Version 2.0 (the "License");
+-- you may not use this file except in compliance with the License.
+-- You may obtain a copy of the License at
+--
+--      http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS,
+-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+-- See the License for the specific language governing permissions and
+-- limitations under the License.
+
+include "map_builder.lua"
+
+MAP_BUILDER_SERVER = {
+  map_builder = MAP_BUILDER,
+  num_event_threads = 4,
+  num_grpc_threads = 4,
+  server_address = "0.0.0.0:50051",
+  uplink_server_address = "",
+  upload_batch_size = 100,
+  enable_ssl_encryption = false,
+  enable_google_auth = false,
+}
+
+MAP_BUILDER.collate_by_trajectory = true
@@ -0,0 +1,95 @@
+-- Copyright 2016 The Cartographer Authors
+--
+-- Licensed under the Apache License, Version 2.0 (the "License");
+-- you may not use this file except in compliance with the License.
+-- You may obtain a copy of the License at
+--
+--      http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS,
+-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+-- See the License for the specific language governing permissions and
+-- limitations under the License.
+
+POSE_GRAPH = {
+  optimize_every_n_nodes = 90,
+  constraint_builder = {
+    sampling_ratio = 0.3,
+    max_constraint_distance = 15.,
+    min_score = 0.55,
+    global_localization_min_score = 0.6,
+    loop_closure_translation_weight = 1.1e4,
+    loop_closure_rotation_weight = 1e5,
+    log_matches = true,
+    fast_correlative_scan_matcher = {
+      linear_search_window = 7.,
+      angular_search_window = math.rad(30.),
+      branch_and_bound_depth = 7,
+    },
+    ceres_scan_matcher = {
+      occupied_space_weight = 20.,
+      translation_weight = 10.,
+      rotation_weight = 1.,
+      ceres_solver_options = {
+        use_nonmonotonic_steps = true,
+        max_num_iterations = 10,
+        num_threads = 1,
+      },
+    },
+    fast_correlative_scan_matcher_3d = {
+      branch_and_bound_depth = 8,
+      full_resolution_depth = 3,
+      min_rotational_score = 0.77,
+      min_low_resolution_score = 0.55,
+      linear_xy_search_window = 5.,
+      linear_z_search_window = 1.,
+      angular_search_window = math.rad(15.),
+    },
+    ceres_scan_matcher_3d = {
+      occupied_space_weight_0 = 5.,
+      occupied_space_weight_1 = 30.,
+      translation_weight = 10.,
+      rotation_weight = 1.,
+      only_optimize_yaw = false,
+      ceres_solver_options = {
+        use_nonmonotonic_steps = false,
+        max_num_iterations = 10,
+        num_threads = 1,
+      },
+    },
+  },
+  matcher_translation_weight = 5e2,
+  matcher_rotation_weight = 1.6e3,
+  optimization_problem = {
+    huber_scale = 1e1,
+    acceleration_weight = 1.1e2,
+    rotation_weight = 1.6e4,
+    local_slam_pose_translation_weight = 1e5,
+    local_slam_pose_rotation_weight = 1e5,
+    odometry_translation_weight = 1e5,
+    odometry_rotation_weight = 1e5,
+    fixed_frame_pose_translation_weight = 1e1,
+    fixed_frame_pose_rotation_weight = 1e2,
+    fixed_frame_pose_use_tolerant_loss = false,
+    fixed_frame_pose_tolerant_loss_param_a = 1,
+    fixed_frame_pose_tolerant_loss_param_b = 1,
+    log_solver_summary = false,
+    use_online_imu_extrinsics_in_3d = true,
+    fix_z_in_3d = false,
+    ceres_solver_options = {
+      use_nonmonotonic_steps = false,
+      max_num_iterations = 50,
+      num_threads = 7,
+    },
+  },
+  max_num_final_iterations = 200,
+  global_sampling_ratio = 0.003,
+  log_residual_histograms = true,
+  global_constraint_search_after_n_seconds = 10.,
+  --  overlapping_submaps_trimmer_2d = {
+  --    fresh_submaps_count = 1,
+  --    min_covered_area = 2,
+  --    min_added_submaps_count = 5,
+  --  },
+}
@@ -0,0 +1,26 @@
+-- Copyright 2016 The Cartographer Authors
+--
+-- Licensed under the Apache License, Version 2.0 (the "License");
+-- you may not use this file except in compliance with the License.
+-- You may obtain a copy of the License at
+--
+--      http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS,
+-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+-- See the License for the specific language governing permissions and
+-- limitations under the License.
+
+include "trajectory_builder_2d.lua"
+include "trajectory_builder_3d.lua"
+
+TRAJECTORY_BUILDER = {
+  trajectory_builder_2d = TRAJECTORY_BUILDER_2D,
+  trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
+--  pure_localization_trimmer = {
+--    max_submaps_to_keep = 3,
+--  },
+  collate_fixed_frame = true,
+  collate_landmarks = false,
+}
@@ -0,0 +1,115 @@
+-- Copyright 2016 The Cartographer Authors
+--
+-- Licensed under the Apache License, Version 2.0 (the "License");
+-- you may not use this file except in compliance with the License.
+-- You may obtain a copy of the License at
+--
+--      http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS,
+-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+-- See the License for the specific language governing permissions and
+-- limitations under the License.
+
+TRAJECTORY_BUILDER_2D = {
+  use_imu_data = true,
+  min_range = 0.,
+  max_range = 30.,
+  min_z = -0.8,
+  max_z = 2.,
+  missing_data_ray_length = 5.,
+  num_accumulated_range_data = 1,
+  voxel_filter_size = 0.025,
+
+  adaptive_voxel_filter = {
+    max_length = 0.5,
+    min_num_points = 200,
+    max_range = 50.,
+  },
+
+  loop_closure_adaptive_voxel_filter = {
+    max_length = 0.9,
+    min_num_points = 100,
+    max_range = 50.,
+  },
+
+  use_online_correlative_scan_matching = false,
+  real_time_correlative_scan_matcher = {
+    linear_search_window = 0.1,
+    angular_search_window = math.rad(20.),
+    translation_delta_cost_weight = 1e-1,
+    rotation_delta_cost_weight = 1e-1,
+  },
+
+  ceres_scan_matcher = {
+    occupied_space_weight = 1.,
+    translation_weight = 10.,
+    rotation_weight = 40.,
+    ceres_solver_options = {
+      use_nonmonotonic_steps = false,
+      max_num_iterations = 20,
+      num_threads = 1,
+    },
+  },
+
+  motion_filter = {
+    max_time_seconds = 5.,
+    max_distance_meters = 0.2,
+    max_angle_radians = math.rad(1.),
+  },
+
+  -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
+  imu_gravity_time_constant = 10.,
+  pose_extrapolator = {
+    use_imu_based = false,
+    constant_velocity = {
+      imu_gravity_time_constant = 10.,
+      pose_queue_duration = 0.001,
+    },
+    imu_based = {
+      pose_queue_duration = 5.,
+      gravity_constant = 9.806,
+      pose_translation_weight = 1.,
+      pose_rotation_weight = 1.,
+      imu_acceleration_weight = 1.,
+      imu_rotation_weight = 1.,
+      odometry_translation_weight = 1.,
+      odometry_rotation_weight = 1.,
+      solver_options = {
+        use_nonmonotonic_steps = false;
+        max_num_iterations = 10;
+        num_threads = 1;
+      },
+    },
+  },
+
+  submaps = {
+    num_range_data = 90,
+    grid_options_2d = {
+      grid_type = "PROBABILITY_GRID",
+      resolution = 0.05,
+    },
+    range_data_inserter = {
+      range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
+      probability_grid_range_data_inserter = {
+        insert_free_space = true,
+        hit_probability = 0.55,
+        miss_probability = 0.49,
+      },
+      tsdf_range_data_inserter = {
+        truncation_distance = 0.3,
+        maximum_weight = 10.,
+        update_free_space = false,
+        normal_estimation_options = {
+          num_normal_samples = 4,
+          sample_radius = 0.5,
+        },
+        project_sdf_distance_to_scan_normal = true,
+        update_weight_range_exponent = 0,
+        update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5,
+        update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5,
+      },
+    },
+  },
+}
@@ -0,0 +1,113 @@
+-- Copyright 2016 The Cartographer Authors
+--
+-- Licensed under the Apache License, Version 2.0 (the "License");
+-- you may not use this file except in compliance with the License.
+-- You may obtain a copy of the License at
+--
+--      http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS,
+-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+-- See the License for the specific language governing permissions and
+-- limitations under the License.
+
+MAX_3D_RANGE = 60.
+INTENSITY_THRESHOLD = 40
+
+TRAJECTORY_BUILDER_3D = {
+  min_range = 1.,
+  max_range = MAX_3D_RANGE,
+  num_accumulated_range_data = 1,
+  voxel_filter_size = 0.15,
+
+  high_resolution_adaptive_voxel_filter = {
+    max_length = 2.,
+    min_num_points = 150,
+    max_range = 15.,
+  },
+
+  low_resolution_adaptive_voxel_filter = {
+    max_length = 4.,
+    min_num_points = 200,
+    max_range = MAX_3D_RANGE,
+  },
+
+  use_online_correlative_scan_matching = false,
+  real_time_correlative_scan_matcher = {
+    linear_search_window = 0.15,
+    angular_search_window = math.rad(1.),
+    translation_delta_cost_weight = 1e-1,
+    rotation_delta_cost_weight = 1e-1,
+  },
+
+  ceres_scan_matcher = {
+    occupied_space_weight_0 = 1.,
+    occupied_space_weight_1 = 6.,
+    intensity_cost_function_options_0 = {
+        weight = 0.5,
+        huber_scale = 0.3,
+        intensity_threshold = INTENSITY_THRESHOLD,
+    },
+    translation_weight = 5.,
+    rotation_weight = 4e2,
+    only_optimize_yaw = false,
+    ceres_solver_options = {
+      use_nonmonotonic_steps = false,
+      max_num_iterations = 12,
+      num_threads = 1,
+    },
+  },
+
+  motion_filter = {
+    max_time_seconds = 0.5,
+    max_distance_meters = 0.1,
+    max_angle_radians = 0.004,
+  },
+
+  rotational_histogram_size = 120,
+
+  -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
+  imu_gravity_time_constant = 10.,
+  pose_extrapolator = {
+    use_imu_based = false,
+    constant_velocity = {
+      imu_gravity_time_constant = 10.,
+      pose_queue_duration = 0.001,
+    },
+    -- TODO(wohe): Tune these parameters on the example datasets.
+    imu_based = {
+      pose_queue_duration = 5.,
+      gravity_constant = 9.806,
+      pose_translation_weight = 1.,
+      pose_rotation_weight = 1.,
+      imu_acceleration_weight = 1.,
+      imu_rotation_weight = 1.,
+      odometry_translation_weight = 1.,
+      odometry_rotation_weight = 1.,
+      solver_options = {
+        use_nonmonotonic_steps = false;
+        max_num_iterations = 10;
+        num_threads = 1;
+      },
+    },
+  },
+
+  submaps = {
+    high_resolution = 0.10,
+    high_resolution_max_range = 20.,
+    low_resolution = 0.45,
+    num_range_data = 160,
+    range_data_inserter = {
+      hit_probability = 0.55,
+      miss_probability = 0.49,
+      num_free_space_voxels = 2,
+      intensity_threshold = INTENSITY_THRESHOLD,
+    },
+  },
+
+  -- When setting use_intensites to true, the intensity_cost_function_options_0
+  -- parameter in ceres_scan_matcher has to be set up as well or otherwise
+  -- CeresScanMatcher will CHECK-fail.
+  use_intensities = false,
+}