1. What is cartographer, what is slam
Cartographer is a laser slam framework based on graph optimization, laser-based, and can be fused with other sensors; slam real-time positioning and map construction 2. The cartographer
framework
code consists of two parts: cartopher_ros and cartographer
cartographer_ros:
It mainly includes three parts
1> configuration file parameter reading
2> start of trajectory, sensor data collection and transfer, topic processing
3> 3D rigid body transformation
cartographer: including front-end and back-end two
front-end:
radar data processing: time synchronization, data fusion ; Radar data distortion correction; point cloud coordinate transformation and z-axis filtering; voxel filtering and postprocessing Pose prediction
Probabilistic
grid map
scan matching: ceres scan matching; correlation scan matching RealTimeCorrelativeScanMatcher2D
writes radar into grid map
Backend:
Thread pool task and scheduling
pose graph: add nodes to the pose graph, calculate the intra-subgraph constraints and inter-subgraph constraints of nodes (loop detection); multi-resolution maps; rough matching based on branch and bound algorithm, construction of optimization problems and solve
3. Code module understanding
1> Configuration file reading
DEFINE_bool(collect_metrics, false,
"Activates the collection of runtime metrics. If activated, the "
"metrics can be accessed via a ROS service.");
//文件夹
DEFINE_string(configuration_directory, "",
"First directory in which configuration files are searched, "
"second is always the Cartographer installation to allow "
"including files from there.");
//lua文件
DEFINE_string(configuration_basename, "",
"Basename, i.e. not containing any directory prefix, of the "
"configuration file.");
//stream读取，地图文件
DEFINE_string(load_state_filename, "",
"If non-empty, filename of a .pbstream file to load, containing "
"a saved SLAM state.");
DEFINE_bool(load_frozen_state, true,
"Load the saved state as frozen (non-optimized) trajectories.");
DEFINE_bool(
start_trajectory_with_default_topics, true,
"Enable to immediately start the first trajectory with default topics.");
DEFINE_string(
save_state_filename, "",
"If non-empty, serialize state and write it to disk before shutting down.");
2> Initialize the glog and gflags library glog to use for logging, and gflags to read configuration file parameters
// note: 初始化glog库
google::InitGoogleLogging(argv[0]);
// 使用gflags进行参数的初始化. 其中第三个参数为remove_flag
// 如果为true, gflags会移除parse过的参数, 否则gflags就会保留这些参数, 但可能会对参数顺序进行调整.
google::ParseCommandLineFlags(&argc, &argv, true);
3> Check whether the configuration file directory and file exist
//这是运行cartographer_ros时带的参数
args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d_localization.lua
-load_state_filename $(arg load_state_filename)" >
//对文件目录和文件进行检查，没有文件就推出程序
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
4>Initialization of ros node
::ros::init(argc, argv, "cartographer_node");
5>tf monitoring, used to monitor coordinate transformation
constexpr double kTfBufferCacheTimeInSeconds = 10.;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
// 开启监听tf的独立线程
tf2_ros::TransformListener tf(tf_buffer);
NodeOptions node_options;
6> The purpose of the initialization of these two classes is to create a structure that interfaces with the interface from the configuration file
NodeOptions node_options;
TrajectoryOptions trajectory_options
7> Read from the lua configuration file and assign values to node_options, trajectory_options
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
8>slam algorithm class includes front-end and back-end
auto map_builder =
cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
9> Pass the topic to slam
// Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
10>The use of pbstream
// 如果加载了pbstream文件, 就执行这个函数
if (!FLAGS_load_state_filename.empty()) {
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
11> Add track
// 使用默认topic 开始轨迹
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
12>Program end operation
//代码执行这一句就不往下执行了,下边的是程序结束后执行的代码
::ros::spin();
// 结束所有处于活动状态的轨迹
node.FinishAllTrajectories();
// 当所有的轨迹结束时, 再执行一次全局优化
node.RunFinalOptimization();
// 如果save_state_filename非空, 就保存pbstream文件
if (!FLAGS_save_state_filename.empty()) {
node.SerializeState(FLAGS_save_state_filename,
true /* include_unfinished_submaps */);
}
4. node_main.cc file
/*
* 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 "absl/memory/memory.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/ros_log_sink.h"
#include "gflags/gflags.h"
#include "tf2_ros/transform_listener.h"
/**
* note: gflags是一套命令行参数解析工具
* DEFINE_bool在gflags.h中定义
* gflags主要支持的参数类型包括bool, int32, int64, uint64, double, string等
* 定义参数通过DEFINE_type宏实现, 该宏的三个参数含义分别为命令行参数名, 参数默认值, 以及参数的帮助信息
* 当参数被定义后, 通过FLAGS_name就可访问到对应的参数
*/
// collect_metrics ：激活运行时度量的集合.如果激活, 可以通过ROS服务访问度量
DEFINE_bool(collect_metrics, false,
"Activates the collection of runtime metrics. If activated, the "
"metrics can be accessed via a ROS service.");
DEFINE_string(configuration_directory, "",
"First directory in which configuration files are searched, "
"second is always the Cartographer installation to allow "
"including files from there.");
DEFINE_string(configuration_basename, "",
"Basename, i.e. not containing any directory prefix, of the "
"configuration file.");
DEFINE_string(load_state_filename, "",
"If non-empty, filename of a .pbstream file to load, containing "
"a saved SLAM state.");
DEFINE_bool(load_frozen_state, true,
"Load the saved state as frozen (non-optimized) trajectories.");
DEFINE_bool(
start_trajectory_with_default_topics, true,
"Enable to immediately start the first trajectory with default topics.");
DEFINE_string(
save_state_filename, "",
"If non-empty, serialize state and write it to disk before shutting down.");
namespace cartographer_ros {
namespace {
void Run() {
constexpr double kTfBufferCacheTimeInSeconds = 10.;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
// 开启监听tf的独立线程
tf2_ros::TransformListener tf(tf_buffer);
NodeOptions node_options;
TrajectoryOptions trajectory_options;
// c++11: std::tie()函数可以将变量连接到一个给定的tuple上,生成一个元素类型全是引用的tuple
// 根据Lua配置文件中的内容, 为node_options, trajectory_options 赋值
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
// MapBuilder类是完整的SLAM算法类
// 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph)
auto map_builder =
cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
// c++11: std::move 是将对象的状态或者所有权从一个对象转移到另一个对象,
// 只是转移, 没有内存的搬迁或者内存拷贝所以可以提高利用效率,改善性能..
// 右值引用是用来支持转移语义的.转移语义可以将资源 ( 堆, 系统对象等 ) 从一个对象转移到另一个对象,
// 这样能够减少不必要的临时对象的创建、拷贝以及销毁, 能够大幅度提高 C++ 应用程序的性能.
// 临时对象的维护 ( 创建和销毁 ) 对性能有严重影响.
// Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
// 如果加载了pbstream文件, 就执行这个函数
if (!FLAGS_load_state_filename.empty()) {
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
// 使用默认topic 开始轨迹
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
::ros::spin();
// 结束所有处于活动状态的轨迹
node.FinishAllTrajectories();
// 当所有的轨迹结束时, 再执行一次全局优化
node.RunFinalOptimization();
// 如果save_state_filename非空, 就保存pbstream文件
if (!FLAGS_save_state_filename.empty()) {
node.SerializeState(FLAGS_save_state_filename,
true /* include_unfinished_submaps */);
}
}
} // namespace
} // namespace cartographer_ros
int main(int argc, char** argv) {
// note: 初始化glog库
google::InitGoogleLogging(argv[0]);
// 使用gflags进行参数的初始化. 其中第三个参数为remove_flag
// 如果为true, gflags会移除parse过的参数, 否则gflags就会保留这些参数, 但可能会对参数顺序进行调整.
google::ParseCommandLineFlags(&argc, &argv, true);
/**
* @brief glog里提供的CHECK系列的宏, 检测某个表达式是否为真
* 检测expression如果不为真, 则打印后面的description和栈上的信息
* 然后退出程序, 出错后的处理过程和FATAL比较像.
*/
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
// ros节点的初始化
::ros::init(argc, argv, "cartographer_node");
// 一般不需要在自己的代码中显式调用
// 但是若想在创建任何NodeHandle实例之前启动ROS相关的线程, 网络等, 可以显式调用该函数.
::ros::start();
// 使用ROS_INFO进行glog消息的输出
cartographer_ros::ScopedRosLogSink ros_log_sink;
// 开始运行cartographer_ros
cartographer_ros::Run();
// 结束ROS相关的线程, 网络等
::ros::shutdown();
}
5. Lua file parameter tuning
1> There are often 5 files, here we mainly analyze the 2d configuration file, there are 4
front ends
trajectory_builder.lua
trajectory_builder_2d.lua
rear end
map_builder.lua
pose_graph.lua
2>trajectory_builder.lua
TRAJECTORY_BUILDER = {
trajectory_builder_2d = TRAJECTORY_BUILDER_2D,--2d轨迹赋值
trajectory_builder_3d = TRAJECTORY_BUILDER_3D,--
-- pure_localization_trimmer = {
-- max_submaps_to_keep = 3,
-- },
collate_fixed_frame = true, --是否通过固定帧修正
collate_landmarks = false, --是否通过反光板修正
}
3>trajectory_builder_2d.lua
TRAJECTORY_BUILDER_2D = {
use_imu_data = true, -- 是否使用imu数据
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, -- 体素滤波的立方体的边长
-- 使用固定的voxel滤波之后, 再使用自适应体素滤波器
-- 体素滤波器 用于生成稀疏点云 以进行 扫描匹配
adaptive_voxel_filter = {
max_length = 0.5, -- 尝试确定最佳的立方体边长, 边长最大为0.5
min_num_points = 200, -- 如果存在 较多点 并且大于'min_num_points', 则减小体素长度以尝试获得该最小点数
max_range = 50., -- 距远离原点超过max_range 的点被移除
},
-- 闭环检测的自适应体素滤波器, 用于生成稀疏点云 以进行 闭环检测
loop_closure_adaptive_voxel_filter = {
max_length = 0.9,
min_num_points = 100,
max_range = 50.,
},
-- 是否使用 real_time_correlative_scan_matcher 为ceres提供先验信息
-- 计算复杂度高 , 但是很鲁棒 , 在odom或者imu不准时依然能达到很好的效果
use_online_correlative_scan_matching = false,--使用CSM激光匹配
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匹配的一些配置参数
ceres_scan_matcher = {--使用ceres优化的方式进行激光匹配
occupied_space_weight = 1.,--占据空间权重
translation_weight = 10.,--平移权重
rotation_weight = 40.,--旋转权重
ceres_solver_options = {--ceres优化参数
use_nonmonotonic_steps = false,
max_num_iterations = 20, --最大迭代次数
num_threads = 1,--使用几个线程优化
},
},
-- 为了防止子图里插入太多数据, 在插入子图之前之前对数据进行过滤
motion_filter = {--移动滤波，就是判断小车有没有动，就是检测2帧激光数据是否相似，要满足一下全部条件
max_time_seconds = 5.,--2帧激光时间戳小间隔
max_distance_meters = 0.2, --2帧激光最小距离
max_angle_radians = math.rad(1.), --2帧激光最小角度
},
-- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
imu_gravity_time_constant = 10.,--imu的重力常数
-- 位姿预测器
pose_extrapolator = {--位姿推断
use_imu_based = false,, --3d用来初始化位姿推断器的方式
constant_velocity = {
imu_gravity_time_constant = 10.,--imu的重力常数
pose_queue_duration = 0.001,
},
imu_based = {--3d使用
pose_queue_duration = 5.,
gravity_constant = 9.806,--重力常数
pose_translation_weight = 1., --位姿偏移权重，偏移越远要求得分越高才被信任
pose_rotation_weight = 1., --位姿旋转权重，解释同上
imu_acceleration_weight = 1.,--IMU加速度权重，解释同上
imu_rotation_weight = 1.,--IMU旋转权重，解释同上
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", -- 地图的种类, 还可以是tsdf格式
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地图的一些配置
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,
},
},
},
}
4>map_builder.lua
MAP_BUILDER = {
use_trajectory_builder_2d = false,--使用2d轨迹
use_trajectory_builder_3d = false,--使用3d轨迹
num_background_threads = 4,--核心线程数
pose_graph = POSE_GRAPH,--位置图赋值
collate_by_trajectory = false,--是否根据轨迹构建修正器
}
5>pose_graph.lua
POSE_GRAPH = {
-- 每隔多少个节点执行一次后端优化
optimize_every_n_nodes = 90,
-- 约束构建的相关参数
constraint_builder = {
sampling_ratio = 0.3, -- 对局部子图进行回环检测时的计算频率, 数值越大, 计算次数越多，全局约束采样比率(nodes)
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, -- 打印约束计算的log
-- 基于分支定界算法的2d粗匹配器
fast_correlative_scan_matcher = {
linear_search_window = 7.,-- 线性搜索窗7mi
angular_search_window = math.rad(30.),
branch_and_bound_depth = 7,-- -- 树的深度是5层 标准7层
},
-- 基于ceres的2d精匹配器
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,
},
},
-- 基于分支定界算法的3d粗匹配器
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的3d精匹配器
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, -- 3d里imu的线加速度的权重
rotation_weight = 1.6e4, -- 3d里imu的旋转的权重
-- 前端结果残差的权重
local_slam_pose_translation_weight = 1e5,
local_slam_pose_rotation_weight = 1e5,
-- 里程计残差的权重
odometry_translation_weight = 1e5,
odometry_rotation_weight = 1e5,
-- gps残差的权重
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,
-- },
}
6> How to adjust the parameters, first close the back end, and only use the laser, first debug the front end, the front end is ok, and then test other parameters
optimize_every_n_nodes = 0, --设置为0，关闭后端优化。
7>Pure positioning realizes video
cartographer pure positioning video