cartographer基于3d地图的纯定位模式

关于纯定位模式呢，本人试过2d和3d,怎么说呢，有条件的话，建议直接上3d,因为博主当初在做定位时针对室内和室外都尝试过，2d只在室内环境下效果会很棒，但是在室外就呵呵啦！所以这里直接讲解3d室外定位（建图篇请自己查看我之前的博客：3d建图
看了我之前的博客大概知道配置文件和源文件路径了，我这里就不把路径写出来了，挺麻烦的。直接讲怎么做把，有不懂的留言，看到了我一定知无不言。

1. 首先，保存地图

   利用之前建好的3d地图，地图格式为map.pbstream的统一格式。不熟悉保存方法的看我之前的博客。3d建图，这里直接跳过。

2. 第二步，.lua文件的配置（在纯定位时用到的这个文件和建图时文件内容相同，可跳过不看）
   首先,进入配置文件夹里的configuration_files文件夹,新建my_robot_3d.lua文件(你可以复制原本的backpack_3d.lua,然后修改内容).我的文件内容如下:

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu",
  published_frame = "lidar_mid",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = false,
  use_odometry = false,
  use_nav_sat = false,#这里选择是否用GPS,博主的图建出来效果不错,定位也可以,后面也尝试了加入GPS建图定位,很nice
  use_landmarks = false,
  num_laser_scans = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 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.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1

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

保存退出到同级目录中,进入urdf文件夹,检查minibus.urdf文件,定位会用到座标系转换，所以这个文件也是需要的。只要雷达不动，这个文件就不能改动。

3. 第三步，重要文件配置，纯定位接口

文件路径（这里是配置文件路径，改完后需要在源文件路径里找到相同的文件进行修改）

{HOME}/catkin_cartographer/install_isolated/share/cartographer_ros/configuration_files/backpack_3d_localization.lua

文件内容：

include "my_robot_3d.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 6,//最大保存子图数，存定位模式通过子图进行定位，但只需要当前和上一个子图即可，我这里设置的是6
}
POSE_GRAPH.optimize_every_n_nodes = 100//每100个有效帧一个子图，子图构建完成要闭环检测一次，这个数越小，闭环检测越频繁，当然CPU爆炸

return options

4.纯定位API接口launch文件配置（配置文件和源文件要同步）

路径是：

{HOME}/catkin_cartographer/install_isolated/share/cartographer_ros/launch/demo_backpack_3d_localization.launch

内容是;

<launch>
  <param name="/use_sim_time" value="false" />

  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/minibus.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_localization.lua
          -load_state_filename $(arg load_state_filename)"
      output="screen">
    <remap from="points2" to="/minibus/front/lslidar_point_cloud" />
<!--
    <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" />

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

</launch>

5.修改文件occupancy_node_main.cc

这个文件在路径:

/home/heng/catkin_cartographer/src/cartographer_ros/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc

第171行或者直接搜索下面这行代码：

//occupancy_grid_publisher_.publish(*msg_ptr);

注释掉这行代码，不然你纯定位过程建立的子图会覆盖在之前建好的地图上。

6.检查配置文件和源文件两个文件夹里做了相同的配置之后,保存退出到工作空间路径:

cd ~
cd ~/catkin_cartographer/
catkin_make_isolated --install --use-ninja
source devel_isolated/setup.bash

7. 仿真测试:

cd ~/catkin_cartographer/
source devel_isolated/setup.bash
roslaunch cartographer_ros demo_backpack_3d_localization.launch load_state_filename:={HOME}/Downloads/mymap.pbstream

这是最终效果，博主是实车测试的，所以只有地图没有数据包，各位可以照着配置进行尝试。当时无人车并未动，3d定位大概花了20s左后就自己定好了位。博主不会插入动图，也不会插入视频，所以把定位好的视频上传到百度云里了，可以自定下载然后看效果
链接: https://pan.baidu.com/s/1PptoGorj8AvRCMtBNozsuA 提取码: yhr8

为了直观，我还是放两张截图吧！在开始定位时，绿色为当前雷达数据，黑色部分为之前建好的地图。