使用速腾16线激光雷达跑通cartogranpher-2d作为真值,对视觉激光融合结果使用evo评估

摘要：长走廊数据集,使用cartographer结果作为真值，评估视觉和雷达融合(VINS-Fusion-Lidar)的精度。框架：vins-fusion、vins-fusion-lidar、aloam

一、配置cartographer参数

my_robot_3d.lua

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "mynteye_imu_frame",
  published_frame = "base_link",
  odom_frame = "base_link",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_odometry = false,
  use_nav_sat = false,
  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.,
}

MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
TRAJECTORY_BUILDER_2D.min_range = 1
--TRAJECTORY_BUILDER_2D.min_z = 0

--TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points=500

--TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1e7
--TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1

TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = .1
--TRAJECTORY_BUILDER_2D.submaps.num_range_data = 200

TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
--TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1

--POSE_GRAPH.optimize_every_n_nodes = 0

return options

my_robot_3d.launch

<launch>
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/why_robot_3d.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 why_robot_3d.lua"
      output="screen">
    <remap from="points2" to="/rslidar_points" />
    <remap from="imu" to="/mynteye/imu/data_raw" />
    <!-- <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" />
</launch>

my_robot_3d.urdf

<robot name="why_sensor">
  <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="mynteye_imu_frame">
    <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="rslidar">
    <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="why_sensor" />
 
  <link name="base_link" />

  <!-- Legacy data -->
  <link name="why" />

  <joint name="why_link_joint_legacy" type="fixed">
    <parent link="base_link" />
    <child link="why" />
    <origin xyz="0.00 0. 0.03618" rpy="0. 0 0" />
  </joint>

  <joint name="sensor_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="why_sensor" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>

  <joint name="imu_link_joint" type="fixed">
    <parent link="why_sensor" />
    <child link="mynteye_imu_frame" />
    <origin xyz="0.006253 -0.011775 0.007645" rpy="0 0 0" />
  </joint>

  <joint name="why_link_joint" type="fixed">
    <parent link="why_sensor" />
    <child link="rslidar" />
    <origin xyz="0.0 0.0 0.03618" rpy="0 0 3.14159" />
  </joint>

</robot>

最终结果

二、结过分析

• 1、单目VINS-Fusion和 VINS-Fusion-Lidar都有一定的尺度飘移，与匀速状态下IMU退化有关
• 2、ALOAM在xy平面误差较小，但是z轴误差相对较大，目前原因未知
• 3、关联深度之后精度有一定提升，后面继续融合激光里程计因子到VINS-Fusion-Lidar

【注：目前采用10Hz的视觉和10Hz的激光做融合，30Hz和10Hz的激光时间戳同步还没想到容易实现的方法，有大佬知道希望教一下我】