recording a bag file for offline processing

[elgarbe]

Hi, I'm working with a kinect v2 and a hokuyo lidar. I would like to record a bag file for off line processing of the data.
For online maping and testing the system I use this launchfile:

<launch>

  <!-- HECTOR MAPPING VERSION: use this with ROS bag demo_mapping_no_odom.bag generated  -->
  <!--                         from demo_mapping.bag with:                               -->
  <!-- rosbag filter demo_mapping.bag demo_mapping_no_odom.bag 'topic != "/tf" or topic == "/tf" and m.transforms[0].header.frame_id != "/odom"' -->
  <!-- WARNING : Database is automatically deleted on each startup                       -->
  <!--           See "delete_db_on_start" option below...                                -->
  
  <!-- Choose visualization -->
  <arg name="rviz" default="true" />
  <arg name="rtabmapviz" default="false" />
  
  <!-- Choose hector_slam or icp_odometry for odometry -->
  <arg name="hector" default="true" />

  <param name="use_sim_time" type="bool" value="false"/>
  
   <arg name="pi/2" value="1.5707963267948966"/>
   <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
   <node pkg="tf" type="static_transform_publisher" name="kinect2_base_link"
        args="$(arg optical_rotate) base_footprint kinect2_link 100" /> 

  <node if="$(arg hector)" pkg="tf" type="static_transform_publisher" name="scanmatcher_to_base_footprint" 
    args="0.0 0.0 0.0 0.0 0.0 0.0 /scanmatcher_frame /base_footprint 100" />

  <!-- Odometry from laser scans -->
  <!-- If argument "hector" is true, we use Hector mapping to generate odometry for us -->
  <node if="$(arg hector)" pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
    
    <!-- Frame names -->
    <param name="map_frame" value="hector_map" />
    <param name="base_frame" value="base_footprint" />
    <param name="odom_frame" value="odom" />
    
    <!-- Tf use -->
    <param name="pub_map_odom_transform" value="false"/>
    <param name="pub_map_scanmatch_transform" value="true"/>
    <param name="pub_odometry" value="true"/>
    
    <!-- Map size / start point -->
    <param name="map_resolution" value="0.050"/>
    <param name="map_size" value="2048"/>
    <param name="map_multi_res_levels" value="2" />
    
    <!-- Map update parameters -->
    <param name="map_update_angle_thresh" value="0.06" />
    
    <!-- Advertising config --> 
    <param name="scan_topic" value="/scan"/>
  </node>
  
  <!-- If argument "hector" is false, we use rtabmap's icp odometry to generate odometry for us -->
  <node unless="$(arg hector)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen" >
     <remap from="scan"      to="/scan"/>
     <remap from="odom"      to="/scanmatch_odom"/>
     <remap from="odom_info"      to="/rtabmap/odom_info"/>
	  
     <param name="frame_id"        type="string" value="base_footprint"/>   
     
     <param name="Icp/PointToPlane"  type="string" value="true"/>
     <param name="Icp/VoxelSize"     type="string" value="0.05"/>
     <param name="Icp/Epsilon"       type="string" value="0.001"/>
     <param name="Icp/PointToPlaneK"  type="string" value="5"/>
     <param name="Icp/PointToPlaneRadius"  type="string" value="0.3"/>
     <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
     <param name="Icp/PM"             type="string" value="true"/> <!-- use libpointmatcher to handle PointToPlane with 2d scans-->
     <param name="Icp/PMOutlierRatio" type="string" value="0.95"/>
     <param name="Odom/Strategy"        type="string" value="0"/>
     <param name="Odom/GuessMotion"     type="string" value="true"/>
     <param name="Odom/ResetCountdown"  type="string" value="0"/>
     <param name="Odom/ScanKeyFrameThr"  type="string" value="0.9"/>
  </node>

  <group ns="rtabmap">
    <!-- SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="frame_id" type="string" value="base_footprint"/>
	
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_scan"  type="bool" value="true"/>
      <param name="Grid/FromDepth"  type="bool" value="false"/>
	
      <remap from="scan" to="/scan"/>

      <remap from="rgb/image"       to="/kinect2/hd/image_color_rect"/>
      <remap from="depth/image"     to="/kinect2/hd/image_depth_rect"/>
      <remap from="rgb/camera_info" to="/kinect2/hd/camera_info"/>
  	
      <!--<param name="rgb/image_transport" type="string" value="compressed"/>-->
      <!-- <param name="depth/image_transport" type="string" value="compressedDepth"/> -->

      <!-- As hector doesn't provide compatible covariance in the odometry topic, don't use the topic and fix the covariance -->
      <param if="$(arg hector)" name="odom_frame_id"            type="string" value="hector_map"/>
      <param if="$(arg hector)" name="odom_tf_linear_variance"  type="double" value="0.0005"/>
      <param if="$(arg hector)" name="odom_tf_angular_variance" type="double" value="0.0005"/>

      <remap unless="$(arg hector)" from="odom" to="/scanmatch_odom"/>
      <param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>
	
      <!-- RTAB-Map's parameters -->
      <param name="Reg/Strategy"       type="string" value="1"/>    <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
      <param name="Reg/Force3DoF"      type="string" value="true"/>
      <param name="RGBD/ProximityBySpace"      type="string" value="false"/>
    </node>
    
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_depth"     type="bool" value="true"/>
      <param name="subscribe_laserScan" type="bool" value="true"/>
      <param name="frame_id"            type="string" value="base_footprint"/>
    
      <remap from="rgb/image"       to="/kinect2/hd/image_color_rect"/>
      <remap from="depth/image"     to="/kinect2/hd/image_depth_rect"/>
      <remap from="rgb/camera_info" to="/kinect2/hd/camera_info"/>
      <remap from="scan"            to="/scan"/>
      <!--<param name="rgb/image_transport"   type="string" value="compressed"/>-->
      <!--<param name="depth/image_transport" type="string" value="compressedDepth"/>-->

      <!-- As hector doesn't provide compatible covariance in the odometry topic -->
      <param if="$(arg hector)" name="odom_frame_id" type="string" value="hector_map"/>

      <remap unless="$(arg hector)" from="odom" to="/scanmatch_odom"/>
      <param unless="$(arg hector)" name="subscribe_odom_info" type="bool" value="true"/>
    </node>
  
  </group>
  
  <!-- Visualisation RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
   <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
      <remap from="rgb/image"       to="/kinect2/hd/image_color_rect"/>
      <remap from="depth/image"     to="/kinect2/hd/image_depth_rect"/>
      <remap from="rgb/camera_info" to="/kinect2/hd/camera_info"/>
      <remap from="cloud"           to="voxel_cloud" />
    
      <!--<param name="rgb/image_transport"   type="string" value="compressed"/>-->
      <!--<param name="depth/image_transport" type="string" value="compressedDepth"/>-->

    <param name="voxel_size" type="double" value="0.01"/>
  </node>

</launch>

I want to use it for offline data processing (making minor changes)

After launching kinect v2 ros package and urg_node I have this topics:

elgarbe@notebook:~$ rostopic list
/diagnostics
/kinect2/bond
/kinect2/hd/camera_info
/kinect2/hd/image_color
/kinect2/hd/image_color/compressed
/kinect2/hd/image_color_rect
/kinect2/hd/image_color_rect/compressed
/kinect2/hd/image_depth_rect
/kinect2/hd/image_depth_rect/compressed
/kinect2/hd/image_mono
/kinect2/hd/image_mono/compressed
/kinect2/hd/image_mono_rect
/kinect2/hd/image_mono_rect/compressed
/kinect2/hd/points
/kinect2/qhd/camera_info
/kinect2/qhd/image_color
/kinect2/qhd/image_color/compressed
/kinect2/qhd/image_color_rect
/kinect2/qhd/image_color_rect/compressed
/kinect2/qhd/image_depth_rect
/kinect2/qhd/image_depth_rect/compressed
/kinect2/qhd/image_mono
/kinect2/qhd/image_mono/compressed
/kinect2/qhd/image_mono_rect
/kinect2/qhd/image_mono_rect/compressed
/kinect2/qhd/points
/kinect2/sd/camera_info
/kinect2/sd/image_color_rect
/kinect2/sd/image_color_rect/compressed
/kinect2/sd/image_depth
/kinect2/sd/image_depth/compressed
/kinect2/sd/image_depth_rect
/kinect2/sd/image_depth_rect/compressed
/kinect2/sd/image_ir
/kinect2/sd/image_ir/compressed
/kinect2/sd/image_ir_rect
/kinect2/sd/image_ir_rect/compressed
/kinect2/sd/points
/laser_status
/rosout
/rosout_agg
/scan
/tf
/urg_node/parameter_descriptions
/urg_node/parameter_updates

which one should I record in order to have all information needed for offline processing?
as you can see I use hector package in order to get odometry from lidar data.


Thank!

[matlabbe]

$ rosbag record /tf /scan /kinect2/hd/image_color_rect /kinect2/hd/image_depth_rect /kinect2/hd/camera_info

To use less space, you can use the qhd topics instead (and remap in the mapping launch file):

$ rosbag record /tf /scan /kinect2/qhd/image_color_rect /kinect2/qhd/image_depth_rect /kinect2/qhd/camera_info

When playing the bag, make sure to set use_sim_time to true and launch the bag with --clock as tf has been recorded (--pause can be convenient to start the bag as paused):

$ roscore
$ rosparam set use_sim_time true
$ rosbag play --clock --pause my_bag.bag
... here launch the file for mapping...
.. resume the bag....

cheers,
Mathieu

[elgarbe]

Thank!

So, this way I could try different rtabmap configuration to get a good 3D map, right?
In an online test I don't get good result. I'm trying to map a floor on my university.

Do I need to move tf from current launchfile:

   <arg name="pi/2" value="1.5707963267948966"/>
   <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
   <node pkg="tf" type="static_transform_publisher" name="kinect2_base_link"
        args="$(arg optical_rotate) base_footprint kinect2_link 100" /> 

  <node if="$(arg hector)" pkg="tf" type="static_transform_publisher" name="scanmatcher_to_base_footprint" 
    args="0.0 0.0 0.0 0.0 0.0 0.0 /scanmatcher_frame /base_footprint 100" />

 and run them in the record session?

thank

[matlabbe]

Hi,

Yes you could change parameters of rtabmap and replay the bag.

Yes you can record them, but they can be also published when replaying the bag too. If you don't need to modify them in the future, recording directly in the bag may be better. The only thing is the "hector" argument, if you want to try without hector, you would not want the frame "/scanmatcher_frame -> /base_footprint" saved in the bag.

cheers,
Mathieu

[elgarbe]

In your experience, what would be beter, hector or rtabmap icp for odometry estimation using a hokuyo urg-04lx lidar?
I can mount kinect and lidar at 10 cm from the floor or at 1 meter, what will be beter?

thank

[matlabbe]

For the height, it depends more if you want to have obstacles at 10 cm or 1 meter over the ground in the map.

With short-range lidar, I've found that lidar odometry approaches able to use input odometry from the wheel encoders are more robust to corridor-like environments (e.g., low geometry complexity in the range of the lidar). See for example figure 13 and table 9 of this paper: RTAB-Map as an Open-Source Lidar and Visual SLAM Library for Large-Scale and Long-Term Online Operation where we compare rtabmap's icp_odometry to Cartographer, Hector and gmapping. If I cleanup how rtabmap was launched for the short-range icp mapping of the corresponding experiment launch file of the paper, I would get something like this for icp_odometry node only:

// icp_odometry with guess from odom_combined frame
$ rosrun rtabmap_ros icp_odometry 
  --Reg/Force3DoF true \
  --Icp/PM true \
  --Icp/Epsilon 0.001 \
  --Icp/MaxTranslation 0.5 \
  --Icp/PointToPlane true \
  --Icp/CorrespondenceRatio 0.10 \
  --Icp/PMOutlierRatio 0.95 \
  --Icp/VoxelSize 0.05 \
  --Icp/PointToPlaneRadius 1 \
  --Odom/GuessMotion true"  \
  _frame_id:=base_footprint \
  _guess_frame_id:=odom_combined \
  _guess_min_translation:=0.1 \
  _guess_min_rotation:=0.1 \
  scan_topic:=/scan

Note however that for best results, rtabmap should be built with libpointmatcher support, the results in the paper are based on that.

If you don't have to a relatively accurate wheel odometry available, rtabmap's icp_odometry would have the same issues than hector in low geometric environments (e.g., causing large drift in corridor environments).

cheers,
Mathieu

[elgarbe]

I don't expect obstacles in the mapping session. The thing is at 10cm kinect will see more floor than at 1m. The corridors have 3m width and 15m long with some doors at booth side. Hector maping works just fine in this enviroment, now I want to get a good 3D and 2D map with the kinect and the lidar. I don't have wheel odometry at all.

I will make the experiment and share the result with you. Thank a lot!