ROS实验笔记之——基于l515激光相机的FLVIS与MLMapping

Posted 2021-10-17 gwpscut

之前博客《ROS实验笔记之——VINS-Mono在l515上的实现》在l515上实现了vins，博客《ROS实验笔记之——SLAM无人驾驶初入门》配置flvis并跑了对应的kitti数据集

本博文在l515上先实现flvis然后再用mlmapping来建图。

camera.launch

<launch>
  <arg name="serial_no"           default=""/>
  <arg name="usb_port_id"         default=""/>
  <arg name="device_type"         default=""/>
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="camera"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>
  <arg name="external_manager"    default="false"/>
  <arg name="manager"             default="realsense2_camera_manager"/>
  <arg name="output"              default="screen"/>
  <arg name="respawn"              default="false"/>

  <arg name="fisheye_width"       default="-1"/>
  <arg name="fisheye_height"      default="-1"/>
  <arg name="enable_fisheye"      default="false"/>

  <arg name="depth_width"         default="1280"/>
  <arg name="depth_height"        default="720"/>
  <arg name="enable_depth"        default="true"/>

  <arg name="confidence_width"    default="-1"/>
  <arg name="confidence_height"   default="-1"/>
  <arg name="enable_confidence"   default="true"/>
  <arg name="confidence_fps"      default="-1"/>

  <arg name="infra_width"         default="1280"/>
  <arg name="infra_height"        default="720"/>
  <arg name="enable_infra"        default="true"/>
  <arg name="enable_infra1"       default="true"/>
  <arg name="enable_infra2"       default="false"/>
  <arg name="infra_rgb"           default="false"/>

  <arg name="color_width"         default="1280"/>
  <arg name="color_height"        default="720"/>
  <arg name="enable_color"        default="false"/>

  <arg name="fisheye_fps"         default="-1"/>
  <arg name="depth_fps"           default="-1"/>
  <arg name="infra_fps"           default="30"/>
  <arg name="color_fps"           default="-1"/>
  <arg name="gyro_fps"            default="-1"/>
  <arg name="accel_fps"           default="-1"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

  <arg name="enable_pointcloud"         default="false"/>
  <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
  <arg name="pointcloud_texture_index"  default="0"/>
  <arg name="allow_no_texture_points"   default="false"/>
  <arg name="ordered_pc"                default="false"/>

  <arg name="enable_sync"               default="true"/>
  <arg name="align_depth"               default="true"/>

  <arg name="publish_tf"                default="true"/>
  <arg name="tf_publish_rate"           default="0"/>

  <arg name="filters"                   default=""/>
  <arg name="clip_distance"             default="-2"/>
  <arg name="linear_accel_cov"          default="0.01"/>
  <arg name="initial_reset"             default="false"/>
  <arg name="unite_imu_method"          default="copy"/>
  <arg name="topic_odom_in"             default="odom_in"/>
  <arg name="calib_odom_file"           default=""/>
  <arg name="publish_odom_tf"           default="true"/>

  <arg name="stereo_module/exposure/1"  default="7500"/>
  <arg name="stereo_module/gain/1"      default="16"/>
  <arg name="stereo_module/exposure/2"  default="1"/>
  <arg name="stereo_module/gain/2"      default="16"/>
  
  

  <group ns="$(arg camera)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="external_manager"         value="$(arg external_manager)"/>
      <arg name="manager"                  value="$(arg manager)"/>
      <arg name="output"                   value="$(arg output)"/>
      <arg name="respawn"                  value="$(arg respawn)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="usb_port_id"              value="$(arg usb_port_id)"/>
      <arg name="device_type"              value="$(arg device_type)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_pointcloud"        value="$(arg enable_pointcloud)"/>
      <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
      <arg name="pointcloud_texture_index"  value="$(arg pointcloud_texture_index)"/>
      <arg name="enable_sync"              value="$(arg enable_sync)"/>
      <arg name="align_depth"              value="$(arg align_depth)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye"           value="$(arg enable_fisheye)"/>

      <arg name="depth_width"              value="$(arg depth_width)"/>
      <arg name="depth_height"             value="$(arg depth_height)"/>
      <arg name="enable_depth"             value="$(arg enable_depth)"/>

      <arg name="confidence_width"         value="$(arg confidence_width)"/>
      <arg name="confidence_height"        value="$(arg confidence_height)"/>
      <arg name="enable_confidence"        value="$(arg enable_confidence)"/>
      <arg name="confidence_fps"           value="$(arg confidence_fps)"/>

      <arg name="color_width"              value="$(arg color_width)"/>
      <arg name="color_height"             value="$(arg color_height)"/>
      <arg name="enable_color"             value="$(arg enable_color)"/>

      <arg name="infra_width"              value="$(arg infra_width)"/>
      <arg name="infra_height"             value="$(arg infra_height)"/>
      <arg name="enable_infra"             value="$(arg enable_infra)"/>
      <arg name="enable_infra1"            value="$(arg enable_infra1)"/>
      <arg name="enable_infra2"            value="$(arg enable_infra2)"/>
      <arg name="infra_rgb"                value="$(arg infra_rgb)"/>

      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
      <arg name="depth_fps"                value="$(arg depth_fps)"/>
      <arg name="infra_fps"                value="$(arg infra_fps)"/>
      <arg name="color_fps"                value="$(arg color_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>

      <arg name="publish_tf"               value="$(arg publish_tf)"/>
      <arg name="tf_publish_rate"          value="$(arg tf_publish_rate)"/>

      <arg name="filters"                  value="$(arg filters)"/>
      <arg name="clip_distance"            value="$(arg clip_distance)"/>
      <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>
      <arg name="initial_reset"            value="$(arg initial_reset)"/>
      <arg name="unite_imu_method"         value="$(arg unite_imu_method)"/>
      <arg name="topic_odom_in"            value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file"          value="$(arg calib_odom_file)"/>
      <arg name="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
      <arg name="stereo_module/exposure/1" value="$(arg stereo_module/exposure/1)"/>
      <arg name="stereo_module/gain/1"     value="$(arg stereo_module/gain/1)"/>
      <arg name="stereo_module/exposure/2" value="$(arg stereo_module/exposure/2)"/>
      <arg name="stereo_module/gain/2"     value="$(arg stereo_module/gain/2)"/>

      <arg name="allow_no_texture_points"  value="$(arg allow_no_texture_points)"/>
      <arg name="ordered_pc"               value="$(arg ordered_pc)"/>
      
    </include>
  </group>
</launch>

l515.launch

<?xml version="1.0"?>
<launch>

<!--Input######################################################################################################-->

<!--FLVIS######################################################################################################-->
    <arg name="node_start_delay"  default="1.0" />
    <param name="/yamlconfigfile" type="string" value="$(find flvis)/launch/d435i/l515.yaml"/>
    <param name="/voc"            type="string" value="$(find flvis)/voc/voc_orb.dbow3"/>


    <!-- Manager -->
    <node pkg="nodelet" type="nodelet"
          name="flvis_nodelet_manager" args="manager" output="screen"
          launch-prefix="bash -c 'sleep 0; $0 $@' ">
          <param name="num_worker_threads" value="8" />
    </node>

    <!-- TrackingNode -->
    <node pkg="nodelet" type="nodelet" args="load flvis/TrackingNodeletClass flvis_nodelet_manager"
        name="TrackingNodeletClass_loader" output="screen"
        launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@' ">
        <!-- D435i -->
        <remap from="/vo/input_image_0"  to="/camera/infra/image_raw"/>
        <remap from="/vo/input_image_1"  to="/camera/depth/image_rect_raw"/>
        <remap from="/imu"               to="/camera/imu"/>
    </node>


    
    <!-- LoopClosingNode -->
    <node pkg="nodelet" type="nodelet" args="load flvis/LoopClosingNodeletClass flvis_nodelet_manager"
        name="LoopClosingNodeletClass_loader" output="screen"
        launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@' ">
    </node>



    <node type="rviz" name="rviz" pkg="rviz" args="-d $(find flvis)/launch/rviz/euroc.rviz" />


</launch>

515的yaml

#type_of_vi:
#0---d435i_depth
#1---euroc mav dataset
#2---d435+pixhawk
#3---d435i_stereo
#4---KITTI_stereo_mode

type_of_vi: 0
#depth image is aligned to left cam0
image_width:  640
image_height: 480
cam0_intrinsics: [457.09765625, 457.234375, 304.328125, 254.6796875]#fx fy cx cy
cam0_distortion_coeffs: [0, 0, 0, 0]#k1 k2 r1 r2
depth_factor: 1000.0
T_imu_cam0:
[ 0.0,  0.0,  1.0,  0.0,
 -1.0,  0.0,  0.0,  0.0,
  0.0, -1.0,  0.0,  0.0,
  0.0,  0.0,  0.0,  1.0]


is_lite_version:   False

##vifusion parameter
#Madgwick beta
vifusion_para1: 0.1
#proportion of vision feedforware(roll and pich)
vifusion_para2: 0.01
#acc-bias feedback parameter
vifusion_para3: 0.001
#gyro-bias feedback parameter
vifusion_para4: 0.001
#acc saturation level
vifusion_para5: 0.1
#gyro saturation level
vifusion_para6: 0.1


#feature relate parameter
#max features in a grid
feature_para1: 15
#min features in a grid
feature_para2: 30
#distance of features
feature_para3: 5
#goodFeaturesToTrack detector maxCorners
feature_para4: 1000
#goodFeaturesToTrack detector qualityLevel
feature_para5: 0.01
#goodFeaturesToTrack detector minDistance
feature_para6: 5

#depth recovery
#IIR filter ratio
dr_para1: 0.98
#triangulate range
dr_para2: 40
#Dummy depth enable 1.0, enable 0.0,disable
dr_para3: 1.0

#LocalMapThread
output_sparse_map: False
window_size:       8

#LoopClosureTheread
#define lcKFStart
lcKFStart: 25
#define lcKFDist
lcKFDist: 18
#define lcKFMaxDist
lcKFMaxDist: 50
#define lcKFLast
lcKFLast: 20
#define lcNKFClosest
lcNKFClosest: 2
#define ratioMax
ratioMax: 0.5
#define ratioRansac
ratioRansac: 0.5
#define minPts
minPts: 20
#define minScore
minScore: 0.12

 roslaunch flvis camera.launch
 roslaunch flvis l515.launch

效果如下：

flvis

加上mlmapping

 roslaunch flvis camera.launch
 roslaunch flvis l515.launch
 roslaunch mlmapping mlmapping_l515.launch

但发现不能生成，改为试试glmapping（GitHub - HKPolyU-UAV/glmapping: Global-Local Mapping Kit）

好像也不行。。。

参考资料

https://github.com/HKPolyU-UAV/FLVIShttps://github.com/HKPolyU-UAV/FLVIShttps://github.com/HKPolyU-UAV/MLMappinghttps://github.com/HKPolyU-UAV/MLMapping