Cartographer 3d配置

     谷歌的Cartographer在设计时，留下了很多的接口，但是它的文档中，对于如何使用并没有特别多的解释，这也导致我们在做实验时遇到了很多坑。其实主要还是对ROS的消息机制理解不够，我们通过对Cartographer进行文件配置，也对ROS的新消息机制有了更深的理解。

     根据ROS的消息机制，一个系统的搭建需要有topic的发送与接收，对于多个传感器，需要通过urdf文件定义其位置关系。在结合Cartographer的一个3d的demo，以及他的图片资料，我们知道谷歌在设计时使用了两个Velodyne vlp-16雷达和一个IMU传感器对环境进行感知，并为Cartographer提供数据。

     对于cartographer的配置，主要是launch文件中参数配置和lua文件对其参数配置。其中lua文件的主要参数为

1.scan_per_accumulation

   该值设置过小，rviz实时显示频率过快；若设置太大，则rviz实时显示频率过慢，会出现卡顿。（1-4比较合适）

2.voxel_filter_size

体素滤波格网大小。参数设置太大,建图精度低且实时显示点云少,参数设置太大,运算量大且显示速度慢。Carto中为0.15，可以参考。

3.cears_scan_match相关设置

包括线性搜索窗口大小、角度搜索窗口大小和两者对应权重四个参数。参数大小来源于实验,窗口小影响精度,窗口过大拖慢算法速度。Carto 默认设置线性 0.15m、角度为 1°、权重均为 0.1,可参考,根据不同场景调整可提高匹配精度。

4.submap相关参数

1、submaps 按序前四个参数:submap 网格的分辨率及网格范围;2、num_range_data:表示每个 submap 包含点云的帧数;3、hit_probability 和miss_probability:hit 点和 miss 点的概率值。设置原则:1和2来源于实验,其中2可根据传感器采样率设置,原则是

submap 能够缓存一段距离的点云,hit 点概率高于 0.5,miss 点低于 0.5 即可。

5.全局参数配置

含义:1、optimize_every_n_scans:累积多少帧点云后执行一次全局图优化 ; 2 max_num_final_iterations : 最 后 一 次 图 优 化 的 迭 代 次 数 上 限 ; 3、global_sampling_ratio:全局采样率,即参与图优化节点的采样率。

设置原则:设置1值小,图优化执行频率高,计算机压力较大;设置1值较大,图优化执行频率低,计算机压力相对较小,实验中适当调大1值,实时显示效果较差,但对后处理影响不明显;2和3含义较为明显,可参考 Carto默认值

6.约束构建参数

1、min_score 和 global_localization_min_score:回环检测分数阈值,若大于该分数,回环检测成功,若小于该值,回环检测失败。区别在于搜素窗口大小不同,前者根据设置窗口内检测,后者在整个点云窗口内搜索。设置原则:来自于实验,可适当减小参数值增加形成回环几率;

2、fast_correlative_scan_matcher_3d 模块:基于多分辨率地图的 CSM 算法,用于回环检测。设置原则:搜索窗口设置与 CSM 算法一致,分支定界深度与全分辨率深度的差值为分辨率地图层数,Carto 分别设置为 8和 3,即包含 5 层分辨率地图;fast_correlative_scan_matcher 模块:针对 2D问题设置,仅包含搜索窗口和分支定界深度;

3、ceres_scan_matcher_3d 和 ceres_scan_matcher 模块:分别针对 3D 和2D 问题设置,用于修正 POS,设置原则与 Carto 一致。

4、max_constraint_distance:约束距离上限,控制边的距离不能太远,避免明显漂移。

5、adaptive_voxel_filter 模块:包括与目标点距离上限、点数上限和体素滤波网格大小,设置原则:设置来自于实验,参考 Carto 设置。

6、lower_covariance_eigenvalue_bound:信息矩阵对角元默认初值,通常该值设置较小,Carto 设置为 1e-11;

7、log_matches=true:是否在屏幕上输出当前的匹配信息,如节点 ID,匹配分数等。

 

Cartographer在自己机器人上进行配置；在使用3d激光雷达时，如果配置cartograher需要那些配置呢？如下

1、下载和编译，这个不用多说，照着来就行；

2、配置文件：首先应该先看Launch文件，了解Cartographer_ros启动了哪些节点：

<launch>
	<param name="robot_description"
	textfile="$(find cartographer_ros)/urdf/backpack_3d.urdf" />
	<node name="robot_state_publisher" pkg="robot_state_publisher"
	type="robot_state_publisher" />
	<node name="cartographer_node" pkg="cartographer_ros"
	type="cartographer_node" args="
	-configuration_directory $(find cartographer_ros)/configuration_files
	-configuration_basename backpack_3d.lua"
	output="screen">
	<remap from="points2_1" to="horizontal_laser_3d" />
	<remap from="points2_2" to="vertical_laser_3d" />
	</node>
	<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
	type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
	</launch>

实时处理3d点云的话，我们参考一下backpack.launch。这个是cartographer中的backpack.launch，从这个launch中，我们得知，谷歌的背包launch需要配置一下几个文件：

1、urdf；2、lua；3、remap 到激光的topic；

分别来说 1、urdf （backpack_3d.urdf）

<robot name="cartographer_backpack_3d">
	<material name="orange">
	<color rgba="1.0 0.5 0.2 1" />
	</material>
	<material name="gray">
	<color rgba="0.2 0.2 0.2 1" />
	</material>
	<link name="imu_link">
	<visual>
	<origin xyz="0.0 0.0 0.0" />
	<geometry>
	<box size="0.06 0.04 0.02" />
	</geometry>
	<material name="orange" />
	</visual>
	</link>
	<link name="horizontal_vlp16_link">
	<visual>
	<origin xyz="0.0 0.0 0.0" />
	<geometry>
	<cylinder length="0.07" radius="0.05" />
	</geometry>
	<material name="gray" />
	</visual>
	</link>
	<link name="vertical_vlp16_link">
	<visual>
	<origin xyz="0.0 0.0 0.0" />
	<geometry>
	<cylinder length="0.07" radius="0.05" />
	</geometry>
	<material name="gray" />
	</visual>
	</link>
	<link name="base_link" />
	<joint name="imu_link_joint" type="fixed">
	<parent link="base_link" />
	<child link="imu_link" />
	<origin xyz="0 0 0" rpy="3.1416 0 0" />
	</joint>
	<joint name="horizontal_vlp16_link_joint" type="fixed">
	<parent link="base_link" />
	<child link="horizontal_vlp16_link" />
	<origin xyz="0.01 0. 0.19" rpy="0. -0.1745 3.1416" />
	</joint>
	<joint name="vertical_vlp16_link_joint" type="fixed">
	<parent link="base_link" />
	<child link="vertical_vlp16_link" />
	<origin xyz="0.19 0. 0.04" rpy="0. 1.3963 0." />
	</joint>
	</robot>

   这一部分主要是讲各个传感器之间的位姿关系，joint name 是传感器节点，他们的父节点都设成了IMU，也就是说他们视机器人的中心为IMU。也就是各个传感器和IMU之间的位姿差，xyz是位置，rpy是横滚、俯仰、偏航三个角，大家去测一下填上就好（都是各个传感器相对于IMU的位姿差，多余的部分需要删去（如果只用一个雷达，那需要删去另一个雷达的参数））。

2 、lua（backpack_3d.lua）lua就相当于Cartographer的配置文件，需要改的我会在后面标记，其他的参数，请对照Cartographer的官方文档，我就不多说了。

include "map_builder.lua"
	include "trajectory_builder.lua"
	options = {
	map_builder = MAP_BUILDER,
	trajectory_builder = TRAJECTORY_BUILDER,
	map_frame = "map",
	tracking_frame = "base_link",
	published_frame = "base_link",
	odom_frame = "odom",
	provide_odom_frame = true,
	publish_frame_projected_to_2d = false,
	use_pose_extrapolator = true,
	use_odometry = false,  //如果有里程计的话，就是true，反之false
	use_nav_sat = false,  //如果用经纬度的话，就是true，反之false
	use_landmarks = false,
	num_laser_scans = 0,
	num_multi_echo_laser_scans = 0,
	num_subdivisions_per_laser_scan = 1,
	num_point_clouds = 2,  //点云数，其实就是激光雷达的数量
	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.,
	}
	TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 160  //建议适当降低
	MAP_BUILDER.use_trajectory_builder_3d = true
	MAP_BUILDER.num_background_threads = 7
	POSE_GRAPH.optimization_problem.huber_scale = 5e2
	POSE_GRAPH.optimize_every_n_nodes = 320
	POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
	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
	return options

3、

<remap from="points2_1" to="horizontal_laser_3d" />  改为<remap from = "自己激光的Topic名字"to ="horizontal_laser_3d"/>

<remap from="points2_2" to="vertical_laser_3d" />  如果没有的话，删除

然后就是IMU <remap from ="自己imu的名字"to = "imu/data"/> 加上即可