参考文章:https://www.cnblogs.com/Ezekiel/p/9907812.html
https://blog.csdn.net/tiancailx/article/details/90757522
cartographer 涉及到的参数需要增加删除或者修改尽量在velodyne_3d.lua文件中添加或者修改,注意添加参数时要在包含该参数的头文件,并写全参数结构,比如某个参数是一个大参数包含的小参数,需要写成:大参数.小参数=xx 。该文件原名为backpack_3d.lua,在~/{工作空间}/src/google_cartographer/cartographer_ros-master/cartographer_ros/configuration_files/文件夹下面
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map", #地图坐标系
tracking_frame = "gyro_link", #如果有imu ,就是imu坐标系
published_frame = "base_link", #机器人地盘坐标系
odom_frame = "carto_odom", #cartographer计算出的优化后里程计坐标系,并非机器人本身里程计,建议区分开
provide_odom_frame = true, #是否发布odom_frame,如果机器人本身存在odom坐标系,而你又
想让carto 使用 odom数据 ,就需要将published_frame 设置为
odom,provide_odom_frame = false 而use_odometry = true
publish_frame_projected_to_2d = false, #
use_pose_extrapolator = true,
use_odometry = false, #是否使用odom数据
use_nav_sat = false, #是否使用gps数据
use_landmarks = false, #是否使用lankmarks数据
num_laser_scans = 0, #如果使用的是单线的激光雷达,此处为激光雷达的数量
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1, #将激光雷达的数据拆分成几次发出来,对于普通的激光雷达,此处为1
num_point_clouds = 1, #多线激光雷达的数量
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1., #如果odom的数据非常不准可以设置成0.3以减弱odom对整体的优化
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1 #1帧数据被拆分成几次发送出来
TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = true
这一项配置很重要,对于 我自己使用的雷达或者在 gazebo 中仿真时,如 果不配置这项得到的建图效果将非常差。这个参数配置的是否使用 实时的闭环检测方法 来进行前端的扫描匹配。如果这项为false,则扫描匹配使用的是通过前一帧位置的先验,将当前scan与之前做对比,使用 高斯牛顿法 迭代 求解最小二乘问题 求得当前scan的坐标变换。如果这项为true,则使用闭环检测的方法,将当前scan在一定的搜索范围内搜索,范围为设定的平移距离及角度大小,然后在将scan插入到匹配的最优位置处。这种方式建图的效果非常好,即使建图有漂移也能够修正回去,但是这个方法的计算复杂度非常高,非常耗cpu。
MAP_BUILDER.use_trajectory_builder_3d = true #决定选择的是3D的激光雷达还是2D的激
光雷达
MAP_BUILDER.num_background_threads = 7
POSE_GRAPH.optimization_problem.huber_scale = 1e1
POSE_GRAPH.optimize_every_n_nodes = 20
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66
TRAJECTORY_BUILDER_3D.submaps.num_range_data = 40
return options
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------
MAX_3D_RANGE = 60.
TRAJECTORY_BUILDER_3D = {
min_range = 1.,
max_range = MAX_3D_RANGE,
num_accumulated_range_data = 1,
voxel_filter_size = 0.15,
# 在3d slam 时会有2个自适应滤波,一个生成高分辨率点云,一个生成低分辨率点云
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,
},
# 在3D中,occupied_space_weight_0和occupied_space_weight_1参数分别与高分辨率和低分辨率滤波点云相关
ceres_scan_matcher = {
occupied_space_weight_0 = 1.,
occupied_space_weight_1 = 6.,
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,
},
imu_gravity_time_constant = 10.,
rotational_histogram_size = 120, # rotational scan matcher 的 histogram buckets
# 在3D中,出于扫描匹配性能的原因,使用两个混合概率网格。 (术语“hybrid”仅指内部树状数据表示并被抽象给用户)
submaps = {
high_resolution = 0.10, # 用于近距离测量的高分辨率hybrid网格 for local SLAM and loop closure,用来与小尺寸voxel进行精匹配
high_resolution_max_range = 20., # 在插入 high_resolution map 之前过滤点云的最大范围
low_resolution = 0.45, # 用于远距离测量的低分辨率hybrid网格 for local SLAM only,用来与大尺寸voxel进行粗匹配
num_range_data = 160,
range_data_inserter = {
hit_probability = 0.55,
miss_probability = 0.49,
num_free_space_voxels = 2, # 最多可更新多少个free space voxels以进行扫描匹配,0禁用free space
},
},
}
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------
include "pose_graph.lua"
MAP_BUILDER = {
use_trajectory_builder_2d = false,
use_trajectory_builder_3d = false,
num_background_threads = 4,
pose_graph = POSE_GRAPH,
}
-------------------------------------------------------------------------------------------------------------------------------------------------------------------------
POSE_GRAPH = {
optimize_every_n_nodes = 90, # 每几个节点(节点是什么?scan?)执行一次优化,设为0即关闭后端优化
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.), # 一旦找到足够好的分数(高于最低匹配分数),它(scan)就会被送入Ceres扫描匹配器以优化姿势。
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, # Huber损失函数的尺度因子,该值越大,(potential) outliers(潜在)异常值 的影响越大。
acceleration_weight = 1e3, # IMU加速度的权重
rotation_weight = 3e5, # IMU旋转项的权重
local_slam_pose_translation_weight = 1e5, # 基于local SLAM的姿势在连续节点之间进行平移的权重
local_slam_pose_rotation_weight = 1e5, # 基于里程计的姿势在连续节点之间进行平移的权重
odometry_translation_weight = 1e5,
odometry_rotation_weight = 1e5,
fixed_frame_pose_translation_weight = 1e1, # FixedFramePose?应该是gps的
fixed_frame_pose_rotation_weight = 1e2,
log_solver_summary = false, # 可以记录Ceres全局优化的结果并用于改进您的外部校准
use_online_imu_extrinsics_in_3d = true, # 作为IMU残差的一部分
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,
-- },
assets_velodyne_3d.lua 由 assets_writer_backpack_3d.lua改写过来,此项将决定建图输出的是pcd文件还是其他格式的文件,官网还有另一种输出文件的输出格式https://google-cartographer-ros.readthedocs.io/en/latest/assets_writer.html
VOXEL_SIZE = 0.3
include "transform.lua"
options = {
tracking_frame = "base_link",
pipeline = {
{
action = "min_max_range_filter",
min_range = 1.,
max_range = 60.,
},
{
action = "dump_num_points",
},
-- Gray X-Rays. These only use geometry to color pixels.
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xy_all",
transform = XY_TRANSFORM,
},
-- {
-- action = "color_points",
-- frame_id = "laser_link",
-- color = { 255., 0., 0. },
-- },
{
action = "write_pcd",
filename = "carto_3d02.pcd",
},
-- {
-- action = "write_ply",
-- filename = "points.ply",
-- },
}
}
return options
velodyne_3d.urdf 改写自 backpack_3d.urdf,记录各个坐标系之间的转换关系