cartographer建图参数注释及配置修改

建图命令：
录完包之后，运行以下命令，会播放包内容。

roslaunch cartographer_ros demo_backpack_2d.launch bag_filenames:=XXX.bag

播放结束后，再运行以下命令，生成pbstream文件。

rosservice call /finish_trajectory 0

rosservice call /write_state "filename: '${HOME}/Downloads/XXXX.pbtream'"

再将pbtream文件转化成pgm和yaml文件。

rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=./XXX.pgm  -pbstream_filename=./XXXX.pbstream -resolution=0.05

完成建图，生成一张地图作为导航的底图---END

---

一、demo_backpack_2d.launch

demo_backpack_2d.launch为启动文件，主要包括：
（1）backpack_2d.launch文件——>主要修改对象。
（2）demo_2d.rviz文件——>主要用于显示，暂不用修改。

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

  <include file="$(find cartographer_ros)/launch/backpack_2d.launch" />

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
  <node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)" />
</launch>

二、backpack_2d.launch

backpack_2d.launch文件主要包括：
（1）backpack_2d.urdf文件——机器人描述文件
（2）backpack_2d.lua文件——配置文件

<launch>
<!--机器人描述文件-->
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />

<!--机器人状态发布，，，里程计信息？？-->
  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />

<!--cartographer建图节点，调用配置参数-->
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename backpack_2d.lua"
      output="screen">
     <!--remap  "scan"根据自己的激光雷达类型配置-->
    <remap from="scan" to="scan" />
  </node>

  <!--栅格地图节点-->
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>

三、backpack_2d.urdf

  <robot name="cartographer_backpack_2d">
  <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>

  <!--name "laser"修改成自己的-->
  <link name="laser">
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <cylinder length="0.05" radius="0.03" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

  <link name="base_link" />

  <!--name="laser_link_joint"修改成自己的-->
  <!--link="laser"修改成自己的-->  
  <joint name="laser_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="laser" />
    <!--ypy：row,pich,yaw；x,y,z为雷达安装的位置。以base_link为基点，base_link为小车的中心点-->  
    <origin rpy="0 0 0" xyz="0.05 0 0.2" />
  </joint>
</robot>

四、backpack_2d.lua

backpack_2d.lua主要包括:
（1）map_builder.lua文件
（2）trajectory_builder.lua文件

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",  --如果有imu填imu,没有imu则用base_link
  published_frame = "odom",   --有odom一般用odom，没有odom一般用base_link
  odom_frame = "odom",
  provide_odom_frame = false,  --底盘提供了里程计，这里不使用算法提供的里程计；如果没有底盘提供，则可以用cartographer提供的里程计，这里摄制成true
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,   --使能位姿
  use_odometry = true,  --是否使用里程计（底盘提供）
  use_nav_sat = false, 
  use_landmarks = false,
  num_laser_scans = 1,  -- 激光雷达数量
  num_multi_echo_laser_scans = 0,  --google提供的激光雷达，这里用不到，为0
  num_subdivisions_per_laser_scan = 1,  --每扫描一帧作为一帧，原始值为10帧为一帧
  num_point_clouds = 0,
  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.,
}

MAP_BUILDER.use_trajectory_builder_2d = true   --对应map_builder.lua文件，这里设置成true.
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 4  --每4帧取一帧，防止雷达帧率太高耗费计算资源，可以减少计算量

return options

五、map_builder.lua

-- MAP_BUILDER.use_trajectory_builder_2d = true所以这里的参数都为true
include "pose_graph.lua"

MAP_BUILDER = {
  use_trajectory_builder_2d = false,
  use_trajectory_builder_3d = false,
  num_background_threads = 4,
  pose_graph = POSE_GRAPH,
  collate_by_trajectory = false,
}

六、trajectory_builder.lua

trajectory_builder.lua文件包括：
（1）trajectory_builder_2d.lua
（2）trajectory_builder_3d.lua

include "trajectory_builder_2d.lua"
include "trajectory_builder_3d.lua"

TRAJECTORY_BUILDER = {
  trajectory_builder_2d = TRAJECTORY_BUILDER_2D,
  trajectory_builder_3d = TRAJECTORY_BUILDER_3D,
--  pure_localization_trimmer = {
--    max_submaps_to_keep = 3,
--  },
  collate_fixed_frame = true,
  collate_landmarks = false,
}

七、trajectory_builder_2d.lua（生成较好的子图）

TRAJECTORY_BUILDER_2D = {
  --没有接入imu，设置false。2D SLAM 可选 IMU；3D SLAM 必选 IMU。
  --[[2D 订阅IMU话题后，将由imu话题来计算运动的角速度值，同时激光雷达俯仰角度变化后，
  也可以将一定高度范围内的点云投影到平面上。]]
  use_imu_data = false,
  -- 激光雷达数据的有效范围，需根据传感器参数和机器人工作场景确定。
  min_range = 0.,  --激光雷达探测的深度最小距离（最近距离）
  max_range = 30., --激光雷达探测的深度最大距离（最远距离）
  min_z = -0.8,  --激光雷达探测的高度最小距离（最低距离）
  max_z = 2.,  --激光雷达探测的高度最大距离（最高距离）
  missing_data_ray_length = 5.,  --当传感器数据超出有效范围最大值时，按此值来处理。默认配置为5m，小于最大距离30m。
  num_accumulated_range_data = 1,  --积累到这个数量的点云信息后，执行一次匹配scan_match，将有效点云插入局部地图
  
  --[[由于激光雷达的基于原点旋转扫描采样，距离近的障碍物采集数据密集，
  距离远的障碍物采集点稀疏，为保证远近障碍物权重一致，需做处理。采用体素滤波器进行降采样机制，
  使得局部稠密的点变得稀疏。此值越小，得到的数据越稠密，但是计算量越高；此值越大，丢失数据越多，但计算更快。]]
  voxel_filter_size = 0.025,  

  --自适应体素滤波器adaptive voxel filter参数配置，自适应得出的参数在配置参数范围内。
  adaptive_voxel_filter = {
    max_length = 0.5,
    min_num_points = 200,
    max_range = 50.,
  },

  loop_closure_adaptive_voxel_filter = {
    max_length = 0.9,
    min_num_points = 100,
    max_range = 50.,
  },

  --[[局部local SLAM部分，两种匹配策略：CeresScanMatcher和RealTimeCorrelativeScanMatcher]]

     -- RealTimeCorrelativeScanMatcher启用和参数配置
     -- 设置成true，启用
  use_online_correlative_scan_matching = true,
  --[[RealTimeCorrelativeScanMatcher：用于传感器质量较差场合，该算法拥有类似于回环检测的特点，
  具有环境鲁棒性，但耗费计算资源。]]
  real_time_correlative_scan_matcher = {
  	-- 搜索窗口
    linear_search_window = 0.1,
    angular_search_window = math.rad(20.),
    -- 权重
    translation_delta_cost_weight = 1e-1,
    rotation_delta_cost_weight = 1e-1,
  },

  -- CeresScanMatcher：用于高质量的传感器场合；
  ceres_scan_matcher = {
    occupied_space_weight = 1.,
    --下面两个权重是指：PoseExtrapolator或者RealTimeCorrelativeScanMatcher的权重；
    translation_weight = 10.,
    rotation_weight = 40.,
    -- Ceres匹配算法收敛速度配置
    ceres_solver_options = {
      -- 含义：bool量，启动非单调置信区域；
      use_nonmonotonic_steps = false,
      -- 含义：最大迭代步数；
      max_num_iterations = 20,
      -- 含义：线程数；
      num_threads = 1,
    },
  },


  --[[motion filter配置：为了防止一个submap中插入太多scan数据，加入motion filter，
  在移动不明显时丢弃点云数据。满足以下条件，则丢弃。有效帧的判断阈值参数入下：]]
  motion_filter = {
    -- 时间间隔
    max_time_seconds = 5.,
    -- 平移距离
    max_distance_meters = 0.2,
    -- 旋转角度1rad=180/2pi=28.7
    max_angle_radians = math.rad(1.),
  },

  -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
  imu_gravity_time_constant = 10.,
  
  pose_extrapolator = {
    use_imu_based = false,
    constant_velocity = {
      imu_gravity_time_constant = 10.,
      pose_queue_duration = 0.001,
    },
    imu_based = {
      pose_queue_duration = 5.,
      gravity_constant = 9.806,
      pose_translation_weight = 1.,
      pose_rotation_weight = 1.,
      imu_acceleration_weight = 1.,
      imu_rotation_weight = 1.,
      odometry_translation_weight = 1.,
      odometry_rotation_weight = 1.,
      solver_options = {
        use_nonmonotonic_steps = false;
        max_num_iterations = 10;
        num_threads = 1;
      },
    },
  },

  -- Submaps大小规格配置
  submaps = {
    --Submaps由一定数量的range data构成。Submaps越小，内部漂移越小；Submaps越大，越有利于全局定位的回环检测。
    num_range_data = 90,
    --栅格地图格式：通常为PROBABILITY_GRID格式，也可选Truncated Signed Distance Fields (TSDF)格式。
    grid_options_2d = {
      grid_type = "PROBABILITY_GRID",
      --地图分辨率配置：2D地图仅有一个分辨率配置项
      resolution = 0.05,
    },
    --概率栅格地图占有概率计算权重
    range_data_inserter = {
      range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
      probability_grid_range_data_inserter = {
        insert_free_space = true,
        -- 栅格地图的最小单元odds是依据hits、misses进行更新栅格概率，权重可配置
        hit_probability = 0.55,
        miss_probability = 0.49,
      },
      tsdf_range_data_inserter = {
        truncation_distance = 0.3,
        maximum_weight = 10.,
        update_free_space = false,
        normal_estimation_options = {
          num_normal_samples = 4,
          sample_radius = 0.5,
        },
        project_sdf_distance_to_scan_normal = true,
        update_weight_range_exponent = 0,
        update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0.5,
        update_weight_distance_cell_to_hit_kernel_bandwidth = 0.5,
      },
    },
  },
}

八、pose_graph.lua（进行全局优化）

--[[全局Global SLAM全局定位优化在后台运行，执行“the optimization problem” 和
 “sparse pose adjustment”两项任务，以达到修整submaps之间相对位姿关系的目的。]]
POSE_GRAPH = {
  --执行频率。当一个数量的trajectory nodes被插入地图后，就执行一次全局优化。
  --当该参数设置为0时，禁用全局优化功能，仅在调试local slam时才使用。
  optimize_every_n_nodes = 90,
  
  constraint_builder = {
    --采样率。通过将采样实现限制约束条件的数量和计算量，此值过低会使得约束失效，无法正确回环检测，
    --此值过高或增加计算负荷，影响实时性。
    sampling_ratio = 0.3,
    max_constraint_distance = 15.,  -- 不是回环检测的匹配范围
    --匹配最小分数，当FastCorrelativeScanMatcher初步匹配的结果分数高于min_score时，
    --才进入下一步的Ceres Scan Matcher处理。
    min_score = 0.55,
    global_localization_min_score = 0.6,
    loop_closure_translation_weight = 1.1e4,
    loop_closure_rotation_weight = 1e5,
    log_matches = true,
    --使用FastCorrelativeScanMatcher最初步的帧匹配，依赖参数branch_and_bound_depth。
    fast_correlative_scan_matcher = {
      linear_search_window = 7.,   -- 不是回环检测的匹配范围
      angular_search_window = math.rad(30.),   -- 不是回环检测的匹配范围
      branch_and_bound_depth = 7,
    },
    ceres_scan_matcher = {
      occupied_space_weight = 20.,
      translation_weight = 10.,
      rotation_weight = 1.,
      ceres_solver_options = {
        use_nonmonotonic_steps = true,
        max_num_iterations = 10,
        num_threads = 1,
      },
    },
    fast_correlative_scan_matcher_3d = {
      branch_and_bound_depth = 8,
      full_resolution_depth = 3,
      min_rotational_score = 0.77,
      min_low_resolution_score = 0.55,
      linear_xy_search_window = 5.,
      linear_z_search_window = 1.,
      angular_search_window = math.rad(15.),
    },
    ceres_scan_matcher_3d = {
      occupied_space_weight_0 = 5.,
      occupied_space_weight_1 = 30.,
      translation_weight = 10.,
      rotation_weight = 1.,
      only_optimize_yaw = false,
      ceres_solver_options = {
        use_nonmonotonic_steps = false,
        max_num_iterations = 10,
        num_threads = 1,
      },
    },
  },
  matcher_translation_weight = 5e2,
  matcher_rotation_weight = 1.6e3,
  optimization_problem = {
    huber_scale = 1e1,
    acceleration_weight = 1.1e2,
    rotation_weight = 1.6e4,
    local_slam_pose_translation_weight = 1e5,
    local_slam_pose_rotation_weight = 1e5,
    odometry_translation_weight = 1e5,
    odometry_rotation_weight = 1e5,
    fixed_frame_pose_translation_weight = 1e1,
    fixed_frame_pose_rotation_weight = 1e2,
    fixed_frame_pose_use_tolerant_loss = false,
    fixed_frame_pose_tolerant_loss_param_a = 1,
    fixed_frame_pose_tolerant_loss_param_b = 1,
    log_solver_summary = false,
    use_online_imu_extrinsics_in_3d = true,
    fix_z_in_3d = false,
    ceres_solver_options = {
      use_nonmonotonic_steps = false,
      max_num_iterations = 50,
      num_threads = 7,
    },
  },
  max_num_final_iterations = 200,
  global_sampling_ratio = 0.003,
  log_residual_histograms = true,
  global_constraint_search_after_n_seconds = 10.,
  --  overlapping_submaps_trimmer_2d = {
  --    fresh_submaps_count = 1,
  --    min_covered_area = 2,
  --    min_added_submaps_count = 5,
  --  },
}

九、低延时配置

在机器人定位场景中，低延时非常重要。
Local SLAM,在前台实时运行，对延迟影响很大；
Global SLAM，运行在后台，处理一系列的子地图，若不能保证实时性，则各子地图间的关系会发生漂移。

（1）降低 global SLAM延迟, 可采取如下措施：

• 降低 optimize_every_n_nodes；
  每结束一个node都会添加对应的约束，等待n个node完成后，执行一致回环优化；
• 增加 MAP_BUILDER.num_background_threads 至内核数量；
  这是线程池的数量；
• 降低 global_sampling_ratio；
  回环检测采用submaps的采样率；
• 降低 constraint_builder.sampling_ratio；
  约束条件添加时的采样率；
• 增加 constraint_builder.min_score；
  约束条件的最小分数；
• 关于 adaptive voxel filter(s), 降低 .min_num_points, .max_range, 增加 .max_length；
• 增加 voxel_filter_size, submaps.resolution, 降低 submaps.num_range_data；
• 降低搜索窗口的尺：.linear_xy_search_window, .linear_z_search_window, .angular_search_window；
• 增加 global_constraint_search_after_n_seconds；
• 降低 max_num_iterations；

（2）降低Local SLAM延时，可采取如下措施：

• 增加 voxel_filter_size
• 降低 submaps.resolution
• 关于 voxel filter(s), 降低 .min_num_points, .max_range, 增加 .max_length
• 降低 max_range (尤其是信号噪声大时)
• 降低 submaps.num_range_data

 

十、纯定位配置

• 在配置文件中使能纯定位功能
  TRAJECTORY_BUILDER.pure_localization = true -----???
• 降低POSE_GRAPH.optimize_every_n_nodes，可增加定位频率；
• 降低global_sampling_ratio，constraint_builder.sampling_ratio，可降低回环检测频率；
• 可参考 lower latency（低延时设置）；

注意事项：
启动纯定位功能后，submaps.resolution的值需要与加载的地图文件.pbstream分辨率一致。

十一、里程计配置

若系统中有里程计存在，则可通过配置 use_odometry = true将里程计作为输入。
一共有4个参数配置local slam与里程计的权重关系：
POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight
POSE_GRAPH.optimization_problem.odometry_translation_weight
POSE_GRAPH.optimization_problem.odometry_rotation_weight

十二、配置文件中提供四个坐标系含义

• map_frame: 地图坐标系，单一机器人的全局坐标系，“map”；
• tracking_frame：跟踪和校准对象的坐标系(激光雷达坐标系)，“base_link”；
• published_frame：发布对象，在TF中发布一个坐标，达到校准tracking_frame的目的；
• odom_frame：里程坐标系名称，“odom”；

只使用雷达 tracking_frame="base_link" ; published_frame="base_link"
使用雷达+里程计tracking_frame="base_link" ; published_frame="odom"
使用雷达+里程计+IMU tracking_frame="imu_link" ; published_frame="odom"

是否需要在TF中发布里程坐标系：
如果外部模块提供了里程信息，则cartographer无需提供里程，则设置：
provide_odom_frame = false;

机器人里程计单元会发布里程信息： odom -> base_foot_print。但是由于长时间的运行后，里程信息会发生漂移，需要修整map -> odom 的关系。

cartographer 会实时跟踪估算base_link相对于map的关系；因为base_foot_print与base_link为固定关系，所以可修正odom相对于map的关系，达到里程计校准的目的。

use_odometry = false, --是否订阅/odom话题，订阅后将会采用里程计的位置话题来计算线速度值。