赞
踩
最近同学都在卷论文,刷力扣,自己的导师比较松也没有一个很好的学习氛围,为了敦促自己决定开始做一些以前不会做的事,养成一些新的习惯。计划在这学期完成cartographer的学习,并在CSDN做一些记录,虽是他山之石,但也是迈出的第一步,共勉。
提示:以下是本篇文章正文内容,下面案例可供参考
说明:分析所用的一些launch和lua文件是跟着学习的讲师所配置的,由官方下载的雷达2d.lua文件基础上配置成的,配合相应的雷达包可以直接运行,具有实际应用的参考价值。运行部分会另开一篇文章进行介绍,此篇主要是参数介绍。rviz中建图过程如下所示:
最终结果如下图所示:
如文件名所示,该配置文件进行的是rs16多线激光雷达录制的室外bag使用时的参数配置,建立的地图为2d形式。重要的参数提出来重写,原来所在的位置:如TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80,即在trajectory_builder_2d.lua的submaps函数中找num_range_data,相应的注释也在子lua文件中
-- 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 "map_builder.lua" --包含的cartographer里的lua文件 include "trajectory_builder.lua" options = { map_builder = MAP_BUILDER, -- map_builder.lua的配置信息 trajectory_builder = TRAJECTORY_BUILDER, -- trajectory_builder.lua的配置信息 map_frame = "map", -- 地图坐标系的名字 tracking_frame = "imu_link", -- 将所有传感器数据转换到这个坐标系下,如果有imu的,就写imu的link,如果没有,就写base_link或者footprint, --因为cartographer会将所有传感器进行坐标变换到tracking_fram坐标系下,每个传感器的频率不一样,imu频率远高于雷达的频率,这样做可以减少计算量 published_frame = "footprint", -- tf: map -> footprint 自己bag的tf最上面的坐标系的名字 odom_frame = "odom", -- 里程计的坐标系名字 provide_odom_frame = false, -- 是否提供odom的tf, 如果为true,则tf树为map->odom->footprint -- 如果为false tf树为map->footprint publish_frame_projected_to_2d = false, -- 是否将坐标系投影到平面上,没啥用 --use_pose_extrapolator = false, -- 发布tf时是使用pose_extrapolator的位姿还是前端计算出来的位姿,前端计算的位姿更准 use_odometry = false, -- 是否使用里程计,如果使用要求一定要有odom的tf * use_nav_sat = false, -- 是否使用gps *topic形式订阅,不可订阅多个里程计/gps/landmark,要注意做重映射 use_landmarks = false, -- 是否使用landmark * num_laser_scans = 0, -- 是否使用单线激光数据,可订阅多个 num_multi_echo_laser_scans = 0, -- 是否使用multi_echo_laser_scans数据 num_subdivisions_per_laser_scan = 1, -- 1帧数据被分成几次处理,一般为1 num_point_clouds = 1, -- 是否使用点云数据 lookup_transform_timeout_sec = 0.2, -- 查找tf时的超时时间 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., --设成0.1即来10帧用1帧 fixed_frame_pose_sampling_ratio = 1., --某个传感器不准,可以降低其使用频率 imu_sampling_ratio = 1., --如若不准,一般直接弃用即可 landmarks_sampling_ratio = 1., } MAP_BUILDER.use_trajectory_builder_2d = true --之前进行include已经包含在该文件中了,这里进行重写,列出的都为比较重要的参数 --在这里进行重写就会覆盖子文件中的参数 TRAJECTORY_BUILDER_2D.use_imu_data = true TRAJECTORY_BUILDER_2D.min_range = 0.3 TRAJECTORY_BUILDER_2D.max_range = 100. TRAJECTORY_BUILDER_2D.min_z = 0.2 --TRAJECTORY_BUILDER_2D.max_z = 1.4 --TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.02 --TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5 --TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200. --TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 50. --TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_length = 0.9 --TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.min_num_points = 100 --TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_range = 50. TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 1. TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1. TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1. --TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 12 --TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1 --TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = 0.004 --TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 1. TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80. TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.1 POSE_GRAPH.optimize_every_n_nodes = 160. POSE_GRAPH.constraint_builder.sampling_ratio = 0.3 POSE_GRAPH.constraint_builder.max_constraint_distance = 15. POSE_GRAPH.constraint_builder.min_score = 0.48 POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60 return options
rs16_2d_outdoor.lua中包含的子lua文件
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, -- 是否将数据放入阻塞队列中
trajectory_builder.lua中包含的子lua文件,如更改是否使用imu数据也需要在此处进行修改
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., -- 超过最大距离范围的数据点(NaN点)或反射较弱的点,即表明周围较空,用这个距离代替 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, real_time_correlative_scan_matcher = { linear_search_window = 0.1, -- 线性搜索窗口的大小 angular_search_window = math.rad(20.), -- 角度搜索窗口的大小 translation_delta_cost_weight = 1e-1, -- 用于计算各部分score的权重 rotation_delta_cost_weight = 1e-1, }, -- ceres匹配的一些配置参数,优化残差相关 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", -- 地图的种类, 还可以是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, }, }, }, }
trajectory_builder.lua中包含的子lua文件,以上都是前端一些扫描匹配的参数
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, -- 在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, }, ceres_scan_matcher = { -- 在3D中,occupied_space_weight_0和occupied_space_weight_1参数分别与高分辨率和低分辨率滤波点云相关 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 = { -- 2种分辨率的地图 high_resolution = 0.10, -- 用于近距离测量的高分辨率hybrid网格 for local SLAM and loop closure, 用来与小尺寸voxel进行精匹配 high_resolution_max_range = 20., -- 在插入 high_resolution map 之前过滤点云的最大范围 low_resolution = 0.45, num_range_data = 160, -- 用于远距离测量的低分辨率hybrid网格 for local SLAM only, 用来与大尺寸voxel进行粗匹配 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, }
rs16_2d_outdoor.lua中包含的子lua文件
include "pose_graph.lua"
MAP_BUILDER = {
use_trajectory_builder_2d = false, --2d和3d一定要有一个为true,且只能有一个,可以在1中的主lua里重写配置
use_trajectory_builder_3d = false,
num_background_threads = 4,
pose_graph = POSE_GRAPH,
collate_by_trajectory = false,
map_builder.lua中包含的子lua文件,主要为后端参数设置
-- 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, --一般设置成2倍的submaps.num_range_data -- 约束构建的相关参数 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, -- 打印约束计算的log -- 基于分支定界算法的2d粗匹配器 fast_correlative_scan_matcher = { linear_search_window = 7., angular_search_window = math.rad(30.), branch_and_bound_depth = 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, -- }, }
将常用的调整参数列写如下:
--雷达的最大最小距离 TRAJECTORY_BUILDER_2D.min_range = 0.3 TRAJECTORY_BUILDER_2D.max_range = 100. --使用多高以上的点云,单线的时候不要设置,多线防止打到地面上的点干扰建图 TRAJECTORY_BUILDER_2D.min_z = 0.2 -- / -0.8 --体素滤波参数 TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.02 --ceres地图的扫描,平移,旋转的权重,影响建图效果,其他基本上是影响计算量等 TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10.--扫描匹配点云和地图匹配程度,值越大,点云和地图匹配置信度越高 --残差平移、旋转分量,值越大,越不相信和地图匹配的效果,而是越相信先验位姿的结果 TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1. --如果imu不好,接入后地图旋转厉害,可以将这里的旋转权重减小 TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1. --一个子图插入多少个节点,optimize_every_n_nodes=80*2 TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80. TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.1 -- / 0.02 POSE_GRAPH.optimize_every_n_nodes = 160. -- 2倍的num_range_data以上 POSE_GRAPH.constraint_builder.sampling_ratio = 0.3 POSE_GRAPH.constraint_builder.max_constraint_distance = 15. --回环检测阈值,如果不稳定有错误匹配,可以提高这两个值,场景重复可以降低或者关闭回环 POSE_GRAPH.constraint_builder.min_score = 0.48 POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60
wheeltec大车出现建图效果很差的情况,可以通过调整一下参数进行调整,同时还可以通过降低调整旋转的权重改善旋转时候偏差过大;同时在走廊里进行建图时,因为走廊中场景重复度较高,回环检测导致误匹配,此时关了回环检测用纯激光建图即可。
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。