使用cartographer建图需要从lua文件中读取参数配置。本文记录lua文件中的一些参数
参考:官方文档
先看一些ros与cartographer的集成项,这些集成项需要在lua文件中声明:
map_frame:一般为“map”.用来发布submap的ROS帧ID.
tracking_frame:SLAM算法要跟踪的ROS 帧ID.?
published_frame:用来发布pose的帧ID.?
odom_frame:只要当有里程计信息的时候才会使用。即provide_odom_frame=true.
provide_odom_frame:如果为true,the local, non-loop-closed, continuous pose将会在map_frame里以odom_frame发布?
publish_frame_projected_to_2d:如果为true,则已经发布的pose将会被完全成2D的pose,没有roll,pitch或者z-offset?
use_odometry:如果为true,需要提供里程计信息,并话题/odom会订阅nav_msgs/Odometry类型的消息,在SLAM过程中也会使用这个消息进行建图
use_nav_sat:如果为true,会在话题/fix上订阅sensor_msgs/NavSatFix类型的消息,并且在globalSLAM中会用到
use_landmarks:如果为true,会在话题/landmarks上订阅cartographer_ros_msgs/LandmarkList类型的消息,并且在SLAM过程中会用到
num_laser_scans:SLAM可以输入的/scan话题数目的最大值
num_muti_echo_laser_scans:SLAM可以输入sensor_msgs/MultiEchoLaserScan话题数目的最大值
num_subdivisions_per_laser_scan:将每个接收到的(multi_echo)激光scan分割成的点云数。 细分scam可以在扫描仪移动时取消scanner获取的scan。 有一个相应的trajectory builder option可将细分扫描累积到将用于scan_matching的点云中。
num_point_clouds:SLAM可以输入的sensor_msgs/PointCloud2话题数目的最大值
lookup_transform_timeout_sec:使用tf2查找transform的超时时间(秒)。
submap_publish_period_sec:发布submap的时间间隔(秒)
pose_publish_period_sec:发布pose的时间间隔,值为5e-3的时候为200HZ
trajectory_publish_period_sec:发布trajectory markers(trajectory的节点)的时间间隔,值为30e-3为30ms
rangefinder_samping_ratio:测距仪的固定采样ratio
imu_sampling_ratio:IMU message的固定采样ratio
landmarks_sampling_ratio:landmarks message的固定采样ratio,(单位是啥?)
我们来看一下cattographer给的一个官方示例revo_lds.lua:
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "horizontal_laser_link",
published_frame = "horizontal_laser_link",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
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.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65
return options
如果我们要用rplidar跑cartographer,因为revo_lds用的雷达与rplidar配置相似,所以修改的值只需要将tracking_frame改为相应的laser就可以了,不用odom的话就将相应的 use_odometry 置为false就可以了:
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "laser",
published_frame = "laser",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
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.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65
return options
下面还有一些trajector_builder的配置,之后用上再瞅瞅