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

  1. 仿真测试:
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

为了直观,我还是放两张截图吧!在开始定位时,绿色为当前雷达数据,黑色部分为之前建好的地图。
cartographer基于3d地图的纯定位模式_第1张图片
cartographer基于3d地图的纯定位模式_第2张图片

你可能感兴趣的:(激光SLAM)