Cartographer系统&Config

1. 系统组成

• 它由以下两个系统组成：
• Local SLAM (Frontend) ：产生好的Submap
  创建局部一致的Submap集合，并把它们连在一起，会随着时间漂移
• Global SLAM(Backend)：把Submap以最一致的方式连接在一起
  在后端运行，寻找闭环约束（通过扫描匹配来实现scans->submap），也使用其它传感器数据以获得更好的全局一致性，在3D中，它尽力寻找重力方向

1.1 Local SLAM

• 配置文件
  • 2D (TRAJECTORY_BUILDER_2D)
    trajectory_builder_2d.lua
  • 3D (TRAJECTORY_BUILDER_3D)
    trajectory_builder_3d.lua

1.2 Global SLAM

• 配置文件
• pose_graph.lua　(POSE_GRAPH)

2. 2D Local SLAM配置参数

• 定义

syntax = "proto3";

package cartographer.mapping.proto;

import "cartographer/mapping/proto/motion_filter_options.proto";
import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
import "cartographer/mapping/proto/2d/submaps_options_2d.proto";
import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.proto";
import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto";

message LocalTrajectoryBuilderOptions2D {
  // Rangefinder points outside these ranges will be dropped.
  float min_range = 14;
  float max_range = 15;
  float min_z = 1;
  float max_z = 2;

  // Points beyond 'max_range' will be inserted with this length as empty space.
  float missing_data_ray_length = 16;

  // Number of range data to accumulate into one unwarped, combined range data
  // to use for scan matching.
  int32 num_accumulated_range_data = 19;

  // Voxel filter that gets applied to the range data immediately after
  // cropping.
  float voxel_filter_size = 3;

  // Voxel filter used to compute a sparser point cloud for matching.
  sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options = 6;

  // Voxel filter used to compute a sparser point cloud for finding loop
  // closures.
  sensor.proto.AdaptiveVoxelFilterOptions
      loop_closure_adaptive_voxel_filter_options = 20;

  // Whether to solve the online scan matching first using the correlative scan
  // matcher to generate a good starting point for Ceres.
  bool use_online_correlative_scan_matching = 5;
  cartographer.mapping.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions
      real_time_correlative_scan_matcher_options = 7;
  cartographer.mapping.scan_matching.proto.CeresScanMatcherOptions2D
      ceres_scan_matcher_options = 8;
  MotionFilterOptions motion_filter_options = 13;

  // Time constant in seconds for the orientation moving average based on
  // observed gravity via the IMU. It should be chosen so that the error
  // 1. from acceleration measurements not due to gravity (which gets worse when
  // the constant is reduced) and
  // 2. from integration of angular velocities (which gets worse when the
  // constant is increased) is balanced.
  double imu_gravity_time_constant = 17;

  SubmapsOptions2D submaps_options = 11;

  // True if IMU data should be expected and used.
  bool use_imu_data = 12;
}

• 实例

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.),
  },

  imu_gravity_time_constant = 10.,

  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,
      },
    },
  },
}

2.1 adaptive_voxel_filter (体素滤波)

• 体素滤波用于为Local SLAM匹配或为Global SLAM闭环计算Sparser Point Cloud.
• AdaptiveVoxelFilterOptions
  • 定义

syntax = "proto3";

package cartographer.sensor.proto;

message AdaptiveVoxelFilterOptions {
  // 'max_length' of a voxel edge.
  float max_length = 1;

  // If there are more points and not at least 'min_num_points' remain, the
  // voxel length is reduced trying to get this minimum number of points.
  float min_num_points = 2;

  // Points further away from the origin are removed.
  float max_range = 3;
}

• 实例

  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.,
  },

2.2 RealTimeCorrelativeScanMatcherOptions

• 定义

syntax = "proto3";

package cartographer.mapping.scan_matching.proto;

message RealTimeCorrelativeScanMatcherOptions {
  // Minimum linear search window in which the best possible scan alignment
  // will be found.
  double linear_search_window = 1;

  // Minimum angular search window in which the best possible scan alignment
  // will be found.
  double angular_search_window = 2;

  // Weights applied to each part of the score.
  double translation_delta_cost_weight = 3;
  double rotation_delta_cost_weight = 4;
}

• 实例

  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,
  },

2.3 CeresScanMatcherOptions2D

• 定义

message CeresScanMatcherOptions2D {
  // Scaling parameters for each cost functor.
  double occupied_space_weight = 1;
  double translation_weight = 2;
  double rotation_weight = 3;

  // Configure the Ceres solver. See the Ceres documentation for more
  // information: https://code.google.com/p/ceres-solver/
  common.proto.CeresSolverOptions ceres_solver_options = 9;
}

• 实例

  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,
    },
  },

2.４ MotionFilterOptions

• 定义

syntax = "proto3";

package cartographer.mapping.proto;

message MotionFilterOptions {
  // Threshold above which range data is inserted based on time.
  double max_time_seconds = 1;

  // Threshold above which range data is inserted based on linear motion.
  double max_distance_meters = 2;

  // Threshold above which range data is inserted based on rotational motion.
  double max_angle_radians = 3;
}

• 实例

  motion_filter = {
    max_time_seconds = 5.,
    max_distance_meters = 0.2,
    max_angle_radians = math.rad(1.),
  },

2.5 SubmapsOptions2D

• 定义

message SubmapsOptions2D {
  // Number of range data before adding a new submap. Each submap will get twice
  // the number of range data inserted: First for initialization without being
  // matched against, then while being matched.
  int32 num_range_data = 1;
  GridOptions2D grid_options_2d = 2;
  RangeDataInserterOptions range_data_inserter_options = 3;
}

message GridOptions2D {
  enum GridType {
    INVALID_GRID = 0;
    PROBABILITY_GRID = 1;
  }

  GridType grid_type = 1;
  float resolution = 2;
}

message RangeDataInserterOptions {
  enum RangeDataInserterType {
    INVALID_INSERTER = 0;
    PROBABILITY_GRID_INSERTER_2D = 1;
  }

  RangeDataInserterType range_data_inserter_type = 1;
  ProbabilityGridRangeDataInserterOptions2D
      probability_grid_range_data_inserter_options_2d = 2;
}

message ProbabilityGridRangeDataInserterOptions2D {
  // Probability change for a hit (this will be converted to odds and therefore
  // must be greater than 0.5).
  double hit_probability = 1;

  // Probability change for a miss (this will be converted to odds and therefore
  // must be less than 0.5).
  double miss_probability = 2;

  // If 'false', free space will not change the probabilities in the occupancy
  // grid.
  bool insert_free_space = 3;
}

• 实例

  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,
      },
    },
  },

3. 2D Global SLAM参数配置

• 定义

syntax = "proto3";

package cartographer.mapping.proto;

import "cartographer/mapping/proto/pose_graph/constraint_builder_options.proto";
import "cartographer/mapping/proto/pose_graph/optimization_problem_options.proto";

message PoseGraphOptions {
  // Online loop closure: If positive, will run the loop closure while the map
  // is built.
  int32 optimize_every_n_nodes = 1;

  // Options for the constraint builder.
  mapping.constraints.proto.ConstraintBuilderOptions
      constraint_builder_options = 3;

  // Weight used in the optimization problem for the translational component of
  // non-loop-closure scan matcher constraints.
  double matcher_translation_weight = 7;

  // Weight used in the optimization problem for the rotational component of
  // non-loop-closure scan matcher constraints.
  double matcher_rotation_weight = 8;

  // Options for the optimization problem.
  mapping.optimization.proto.OptimizationProblemOptions
      optimization_problem_options = 4;

  // Number of iterations to use in 'optimization_problem_options' for the final
  // optimization.
  int32 max_num_final_iterations = 6;

  // Rate at which we sample a single trajectory's nodes for global
  // localization.
  double global_sampling_ratio = 5;

  // Whether to output histograms for the pose residuals.
  bool log_residual_histograms = 9;

  // If for the duration specified by this option no global contraint has been
  // added between two trajectories, loop closure searches will be performed
  // globally rather than in a smaller search window.
  double global_constraint_search_after_n_seconds = 10;
}

• 实例

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,
      },
    },
  },
  // 以下两个参数用于Constraint
  matcher_translation_weight = 5e2, 
  matcher_rotation_weight = 1.6e3,
  
  optimization_problem = {
    huber_scale = 1e1,
    acceleration_weight = 1e3,
    rotation_weight = 3e5,
    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,
    log_solver_summary = 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.,
}

3.1 ConstraintBuilderOptions

• 定义

message ConstraintBuilderOptions {
  // A constraint will be added if the proportion of added constraints to
  // potential constraints drops below this number.
  double sampling_ratio = 1;

  // Threshold for poses to be considered near a submap.
  double max_constraint_distance = 2;

  // Threshold for the scan match score below which a match is not considered.
  // Low scores indicate that the scan and map do not look similar.
  double min_score = 4;

  // Threshold below which global localizations are not trusted.
  double global_localization_min_score = 5;

  // Weight used in the optimization problem for the translational component of
  // loop closure constraints.
  double loop_closure_translation_weight = 13;

  // Weight used in the optimization problem for the rotational component of
  // loop closure constraints.
  double loop_closure_rotation_weight = 14;

  // If enabled, logs information of loop-closing constraints for debugging.
  bool log_matches = 8;

  // Options for the internally used scan matchers.
  mapping.scan_matching.proto.FastCorrelativeScanMatcherOptions2D
      fast_correlative_scan_matcher_options = 9;
  mapping.scan_matching.proto.CeresScanMatcherOptions2D
      ceres_scan_matcher_options = 11;
  mapping.scan_matching.proto.FastCorrelativeScanMatcherOptions3D
      fast_correlative_scan_matcher_options_3d = 10;
  mapping.scan_matching.proto.CeresScanMatcherOptions3D
      ceres_scan_matcher_options_3d = 12;
}

• 实例

  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,
      },
    },
  },

• a
• a

4. 数据结构

4.1 Pose Graph

• NodeId

graph LR
A[NodeId]
A-->B[trajectory_id]
A-->C[node_index]

• SubmapId

graph LR
A[SubmapId] 
A --> B[trajectory_id]
A --> C[submap_index]

aa
参考：

• Cartographer ROS Tuning
• Cartographer Configuration
• Local SLAM Quality Evaluation