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

爲了直觀，我還是放兩張截圖吧！在開始定位時，綠色爲當前雷達數據，黑色部分爲之前建好的地圖。