cartographer代码学习-2D仿真

昨天到今天花了两天时间研究了一下cartographer的2DSLAM仿真的launch文件以及lua配置文件。尝试自己跑了下效果，总体来说cartographer的前端匹配虽然效果一般但是回环检测确实不错。话不多说先上代码：

1、launch文件

<!--
  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.
-->
<launch>
  

  <!-- 启动cartographer -->
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename lx_rs16_2d_outdoor.lua"
      output="screen">

    <remap from="scan" to="front_scan" />
  </node>

  <!-- 生成ros格式的地图 -->
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

  <!-- 启动rviz -->
  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/lx_2d.rviz" />


</launch>

cartographer的launch文件其实挺简单的，就调用了三个东西而以，后面两个都是现成的功能包，第一个中涉及到了一个lua文件比较重要。

另外注意这里有个remap，

<remap from="scan" to="front_scan" />

这是因为为的仿真中话题名称是front_scan，如果自己的仿真中话题本身是scan的话这个可以省略。

2、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.

include "map_builder.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 = "base_link",                         -- 将所有传感器数据转换到这个坐标系下默认imu_link，在有IMU的情况下将其他数据转到IMU下，因为IMU频率高，转到其他坐标系下需要转换次数更多
  published_frame = "base_link",                        -- tf: map -> odom
  odom_frame = "odom",                                  -- 里程计的坐标系名字
  provide_odom_frame = true,                            -- 是否提供odom的tf, 如果为true,则tf树为map->odom->footprint
  publish_frame_projected_to_2d = false,                -- 是否将坐标系投影到平面上
  use_odometry = true,                                  -- 是否使用里程计,如果使用要求一定要有odom的tf
  use_nav_sat = false,                                  -- 是否使用gps
  use_landmarks = false,                                -- 是否使用landmark
  num_laser_scans = 1,                                  -- 是否使用单线激光数据
  num_multi_echo_laser_scans = 0,                       -- 是否使用multi_echo_laser_scans数据
                                                        -- 这两个还有下面的是否使用点云数据不能同时为0
  num_subdivisions_per_laser_scan = 1,                  -- 1帧数据被分成几次处理,一般为1
  num_point_clouds = 0,                                 -- 是否使用点云数据
  lookup_transform_timeout_sec = 0.2,                   -- 查找tf时的超时时间
  submap_publish_period_sec = 0.3,                      -- 发布数据的时间间隔
  pose_publish_period_sec = 5e-3,                       -- 发布pose的时间间隔，比如：5e-3频率是200Hz
  trajectory_publish_period_sec = 30e-3,                -- 发布轨迹标记的间隔，30e-3是科学记数法，表示的是30乘以10的-3次方。也就是0.030秒，就是30毫秒。
  rangefinder_sampling_ratio = 1.,                      -- 传感器数据的采样频率，多少次数据采样一次，默认都是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       -- 其含义应该应该是多少帧插入一次子图，算法中还有一个乘二操作。
--num_range_data设置的值与CPU有这样一种关系，值小(10)，CPU使用率比较稳定，整体偏高，值大时，CPU短暂爆发使用(插入子图的时候)，平时使用率低，呈现极大的波动状态。
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              --是否使用imu数据
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true     -- 选择是否先求解online scan matching，然后用correlative scan matcher为Ceres求解器产生一个好的初始解，为true时效果更好
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1  -- 线距离搜索框，在这个框的大小内，搜索最佳scan匹配.
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

这个文件开始是从网上嫖来的，嘿嘿。后面自己查了下参数信息加了个注释。这里面有些参数是必须的，例如前面的：

  map_builder = MAP_BUILDER,                            -- map_builder.lua的配置信息
  trajectory_builder = TRAJECTORY_BUILDER,              -- trajectory_builder.lua的配置信息
  map_frame = "map",                                    -- 地图坐标系的名字
  tracking_frame = "base_link",                         -- 将所有传感器数据转换到这个坐标系下默认imu_link，在有IMU的情况下将其他数据转到IMU下，因为IMU频率高，转到其他坐标系下需要转换次数更多
  published_frame = "base_link",                        -- tf: map -> odom
  odom_frame = "odom",  

也有一些参数不设置的话也没有关系，例如最后面的：

POSE_GRAPH.optimization_problem.huber_scale = 1e2       --鲁棒核函数，去噪
POSE_GRAPH.optimize_every_n_nodes = 35                  --后端优化节点
POSE_GRAPH.constraint_builder.min_score = 0.65          --当匹配分数低于此值时,忽略该分数。感觉可能是用来检测是否匹配到回环的

具体的有些还是要自己多试一下。另外这个参数我在运行的时候终端是报了一个警告的，但是没有影响到建图：

3、建图效果对比

3.1、有无里程计

cartographer的匹配方式有点类似于hector，在没有其他传感器的情况下建图结果其实也不是很好。

在无里程计的情况下建图效果：

在添加里程计的情况下建图效果：

可以看到添加了里程计的情况下效果好很多，无里程计的时候效果差的主要原因还是在于地图中存在退化环境。添加了里程计的情况下能够在一定程度上减少这种问题。

3.2、有无回环检测

cartographr的前端检测匹配效果其实挺一般的，建图过程中其实地图发生过多次的歪斜：

但是后面在回环检测之后地图又逐渐的恢复过来了很多，但是可以看到回环检测只是对于一些比较明显的边界有效果，可以看到还是有些地方并没有被很好的修复。