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
- 2D (TRAJECTORY_BUILDER_2D)
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,
},
},
},
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
- SubmapId
aa
参考：