cartographer 参数理解

参考文章：cartographer参数调整 - xjEzekiel - 博客园

cartographer探秘第一章之安装编译与参数配置_李太白lx的博客-CSDN博客

cartographer 涉及到的参数需要增加删除或者修改尽量在velodyne_3d.lua文件中添加或者修改，注意添加参数时要在包含该参数的头文件，并写全参数结构，比如某个参数是一个大参数包含的小参数，需要写成：大参数.小参数=xx 。该文件原名为backpack_3d.lua，在~/{工作空间}/src/google_cartographer/cartographer_ros-master/cartographer_ros/configuration_files/文件夹下面

velodyne_3d.lua

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map", #地图坐标系
  tracking_frame = "gyro_link",  #如果有imu ，就是imu坐标系
  published_frame = "base_link",   #机器人地盘坐标系
  odom_frame = "carto_odom",       #cartographer计算出的优化后里程计坐标系，并非机器人本身里程计，建议区分开
  provide_odom_frame = true,       #是否发布odom_frame，如果机器人本身存在odom坐标系，而你又 
                                   想让carto 使用 odom数据 ，就需要将published_frame 设置为 
                                   odom，provide_odom_frame = false 而use_odometry = true 

  publish_frame_projected_to_2d = false,  #    
  use_pose_extrapolator = true,
  use_odometry = false,         #是否使用odom数据
  use_nav_sat = false,          #是否使用gps数据
  use_landmarks = false,        #是否使用lankmarks数据
  num_laser_scans = 0,           #如果使用的是单线的激光雷达，此处为激光雷达的数量
  num_multi_echo_laser_scans = 0,     
  num_subdivisions_per_laser_scan = 1,       #将激光雷达的数据拆分成几次发出来，对于普通的激光雷达，此处为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.,      #如果odom的数据非常不准可以设置成0.3以减弱odom对整体的优化
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1     #1帧数据被拆分成几次发送出来
TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = true    
这一项配置很重要，对于 我自己使用的雷达或者在 gazebo 中仿真时，如 果不配置这项得到的建图效果将非常差。这个参数配置的是否使用 实时的闭环检测方法 来进行前端的扫描匹配。如果这项为false，则扫描匹配使用的是通过前一帧位置的先验，将当前scan与之前做对比，使用 高斯牛顿法 迭代 求解最小二乘问题 求得当前scan的坐标变换。如果这项为true，则使用闭环检测的方法，将当前scan在一定的搜索范围内搜索，范围为设定的平移距离及角度大小，然后在将scan插入到匹配的最优位置处。这种方式建图的效果非常好，即使建图有漂移也能够修正回去，但是这个方法的计算复杂度非常高，非常耗cpu。

MAP_BUILDER.use_trajectory_builder_3d = true            #决定选择的是3D的激光雷达还是2D的激 
                                                         光雷达
MAP_BUILDER.num_background_threads = 7
POSE_GRAPH.optimization_problem.huber_scale = 1e1
POSE_GRAPH.optimize_every_n_nodes = 20
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
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
TRAJECTORY_BUILDER_3D.submaps.num_range_data = 40

return options

-------------------------------------------------------------------------------------------------------------------------------------------------------------------------

trajectory_builder_3d.lua

MAX_3D_RANGE = 60.
 
TRAJECTORY_BUILDER_3D = {
  min_range = 1.,
  max_range = MAX_3D_RANGE,
  num_accumulated_range_data = 1,
  voxel_filter_size = 0.15,
 
 
# 在3d slam 时会有2个自适应滤波，一个生成高分辨率点云，一个生成低分辨率点云
 
  high_resolution_adaptive_voxel_filter = {
    max_length = 2.,
    min_num_points = 150,
    max_range = 15.,
  },
 
  low_resolution_adaptive_voxel_filter = {
    max_length = 4.,
    min_num_points = 200,
    max_range = MAX_3D_RANGE,
  },
 
  use_online_correlative_scan_matching = false,
  real_time_correlative_scan_matcher = {
    linear_search_window = 0.15,
    angular_search_window = math.rad(1.),
    translation_delta_cost_weight = 1e-1,
    rotation_delta_cost_weight = 1e-1,
  },
 
# 在3D中，occupied_space_weight_0和occupied_space_weight_1参数分别与高分辨率和低分辨率滤波点云相关
  ceres_scan_matcher = {
    occupied_space_weight_0 = 1.,
    occupied_space_weight_1 = 6.,
    translation_weight = 5.,
    rotation_weight = 4e2,
    only_optimize_yaw = false,
    ceres_solver_options = {
      use_nonmonotonic_steps = false,
      max_num_iterations = 12,
      num_threads = 1,
    },
  },
 
  motion_filter = {
    max_time_seconds = 0.5,
    max_distance_meters = 0.1,
    max_angle_radians = 0.004,
  },
 
  imu_gravity_time_constant = 10.,
  rotational_histogram_size = 120, # rotational scan matcher 的 histogram buckets
 
 
# 在3D中，出于扫描匹配性能的原因，使用两个混合概率网格。 （术语“hybrid”仅指内部树状数据表示并被抽象给用户）
  submaps = {
    high_resolution = 0.10,  # 用于近距离测量的高分辨率hybrid网格 for local SLAM and loop closure，用来与小尺寸voxel进行精匹配
    high_resolution_max_range = 20.,  # 在插入 high_resolution map 之前过滤点云的最大范围
    low_resolution = 0.45,   # 用于远距离测量的低分辨率hybrid网格 for local SLAM only，用来与大尺寸voxel进行粗匹配
    num_range_data = 160,
    range_data_inserter = {
      hit_probability = 0.55,
      miss_probability = 0.49,
      num_free_space_voxels = 2, # 最多可更新多少个free space voxels以进行扫描匹配，0禁用free space
    },
  },
}

-------------------------------------------------------------------------------------------------------------------------------------------------------------------------

map_builder.lua

include "pose_graph.lua"

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

-------------------------------------------------------------------------------------------------------------------------------------------------------------------------

pose_graph.lua

POSE_GRAPH = {
  optimize_every_n_nodes = 90, # 每几个节点（节点是什么？scan?）执行一次优化，设为0即关闭后端优化
  constraint_builder = {
    sampling_ratio = 0.3,    # 如果添加的约束与潜在约束的比例低于 此比例，则将添加约束。
    max_constraint_distance = 15., # 在子图附近考虑姿势的阈值
    min_score = 0.55,        # 扫描匹配分数的阈值，低于该阈值时不考虑匹配。 低分表示扫描和地图看起来不相似
    global_localization_min_score = 0.6,    # 阈值，低于该阈值的全局定位不受信任
    loop_closure_translation_weight = 1.1e4, # 优化问题中的 闭环约束 的 平移 的权重
    loop_closure_rotation_weight = 1e5,      # 优化问题中的 闭环约束 的 旋转 的权重
    log_matches = true,     # 闭环约束的直方图统计报告
 
    fast_correlative_scan_matcher = { # 闭环检测的第一步
      linear_search_window = 7.,      # 依靠“分支定界”机制在不同的网格分辨率下工作，并有效地消除不正确的匹配
      angular_search_window = math.rad(30.), # 一旦找到足够好的分数（高于最低匹配分数），它（scan）就会被送入Ceres扫描匹配器以优化姿势。
      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, # Huber损失函数的尺度因子，该值越大，(potential) outliers（潜在）异常值 的影响越大。
    acceleration_weight = 1e3, # IMU加速度的权重
    rotation_weight = 3e5,     # IMU旋转项的权重
    local_slam_pose_translation_weight = 1e5, # 基于local SLAM的姿势在连续节点之间进行平移的权重
    local_slam_pose_rotation_weight = 1e5,    # 基于里程计的姿势在连续节点之间进行平移的权重
    odometry_translation_weight = 1e5,
    odometry_rotation_weight = 1e5,
    fixed_frame_pose_translation_weight = 1e1, # FixedFramePose？应该是gps的
    fixed_frame_pose_rotation_weight = 1e2,
    log_solver_summary = false,             # 可以记录Ceres全局优化的结果并用于改进您的外部校准
    use_online_imu_extrinsics_in_3d = true, # 作为IMU残差的一部分
    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,
  --  },
    

assets_velodyne_3d.lua 由   assets_writer_backpack_3d.lua改写过来，此项将决定建图输出的是pcd文件还是其他格式的文件，官网还有另一种输出文件的输出格式Exploiting the map generated by Cartographer ROS — Cartographer ROS documentation

VOXEL_SIZE = 0.3

include "transform.lua"

options = {
  tracking_frame = "base_link",
  pipeline = {
    {
      action = "min_max_range_filter",
      min_range = 1.,
      max_range = 60.,
    },
    {
      action = "dump_num_points",
    },
-- Gray X-Rays. These only use geometry to color pixels.
    
    {
      action = "write_xray_image",
      voxel_size = VOXEL_SIZE,
      filename = "xray_xy_all",
      transform = XY_TRANSFORM,
    },   
 
   -- {
   --   action = "color_points",
   --   frame_id = "laser_link",
   --   color = { 255., 0., 0. },
   -- },

    {
      action = "write_pcd",
     filename = "carto_3d02.pcd",
    },
   --   {
   --      action = "write_ply",
   --      filename = "points.ply",
   --   },    
  }
}

return options

 velodyne_3d.urdf    改写自  backpack_3d.urdf，记录各个坐标系之间的转换关系

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

  <link name="gyro_link">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <box size="0.06 0.04 0.02" />
      </geometry>
      <material name="orange" />
    </visual>
  </link>

  <link name="velodyne">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <cylinder length="0.07" radius="0.05" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>


  <link name="base_link" />

  <joint name="imu_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="gyro_link" />
    <origin xyz="0 0 0.5" rpy="0 0 0" />
  </joint>

  <joint name="laser_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="velodyne" />
    <origin xyz="0.3 0 1.5" rpy="0 0 0" />
  </joint>

  
</robot>