Problem fusing LiDAR and depth (hector_slam)

[utrecht]

Hey! I've followed your tutorials and I've managed to get a good clear image with odometry with depth alone (using an R200). However, I added a LIDAR now (RPLiDAR A2) and there are errors when trying to match the scans with the pointclouds. When RTABMap first initializes the map actually looks quite nice with plenty details, as seen below:



However, when the drone moves around slightly, the scan and 3D cloud start jumping around and become mismatched. The map then gets broken, as seen below:



I think this has something to do with the static transform publishers in my code, but I just don't see where they mess it up (note, the RPLiDAR node initializes with a static transform publisher from base link to laser with 0 0 0 3.14159 0 0). Here is my TF tree:



Notice the transform publisher types. I chose to not use robot_state_publisher as I felt that it was unnecessary. Do correct me if I am wrong. Below is my custom launch file for RTABMap, with code borrowed from the demo_hector_mapping launch file(apologies for the length):

<?xml version="1.0"?>

<launch>

  <arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>

  <arg name="x" default="0.0"/>
  <arg name="y" default="0.0"/>
  <arg name="z" default="0.0"/>

  <arg name="use_ground_truth_for_tf" default="true" />
  <arg name="use_ground_truth_for_control" default="true" />

  <arg name="world_frame" default="/world"/>
  <arg name="base_link_frame" default="$(arg tf_prefix)/base_link"/>

  <param name="tf_prefix" type="string" value="$(arg tf_prefix)"/> <!-- required for hector_pose_estimation and message_to_tf -->
  <param name="base_link_frame" type="string" value="$(arg base_link_frame)" />
  <param name="base_stabilized_frame" type="string" value="$(arg tf_prefix)/base_stabilized" />
  <param name="base_footprint_frame" type="string" value="$(arg tf_prefix)/base_footprint" />
  <param name="world_frame" type="string" value="$(arg world_frame)" />

  <!-- publish state and tf -->
  <node name="ground_truth_to_tf" pkg="message_to_tf" type="message_to_tf" output="screen">
    <param name="odometry_topic" value="ground_truth/state" />
    <param name="frame_id" value="$(arg world_frame)" />
    <param name="tf_prefix" value="$(arg tf_prefix)" if="$(arg use_ground_truth_for_tf)" />
    <param name="tf_prefix" value="$(arg tf_prefix)/ground_truth" unless="$(arg use_ground_truth_for_tf)" />
  </node>

  <node pkg="tf" type="static_transform_publisher" name="world_to_map" 
    args="0.0 0.0 0.0 0.0 0.0 0.0 world map 100" />

  <node 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" />

  <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="laser_filter">
        <rosparam>
            scan_filter_chain:
            - name: range
              type: LaserScanRangeFilter
              params:
                lower_threshold: 0.25
                upper_threshold: .inf
        </rosparam>
        <remap from="scan" to="/scan" />
        <remap from="scan_filtered" to="/scan_filtered" />
  </node>

  <!-- Odometry from laser scans -->
  <!-- We use Hector mapping to generate odometry for us -->
  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping">
    
    <!-- Frame names -->
    <param name="map_frame" value="/map" />
    <param name="base_frame" value="/base_footprint" />
    <param name="odom_frame" value="/scanmatcher_frame" />
    
    <!-- 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_filtered"/>
  </node>

  <arg name="stereo"                  default="false" />
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="true" />
  
  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
  
  <!-- sim time for convenience, if playing a rosbag -->
  <arg name="use_sim_time"            default="false"/>
  <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
  
  <!-- Corresponding config files -->
  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="base_footprint"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="odom_frame_id"           default="/scanmatcher_frame"/>                <!-- If set, TF is used to get odometry instead of the topic -->
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     <!-- e.g., "world" -->
  <arg name="ground_truth_base_frame_id" default=""/>  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="rtabmap_args"            default="--delete_db_on_start"/>   <!-- deprecated, use "args" argument -->
  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
  <arg name="output"                  default="screen"/>        <!--Control node output (screen or log)-->
  
  <!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="true"/>         
   
  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/sw_registered/image_rect_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="image_raw_topic"         default="/camera/depth/points/image_raw" />
  <arg name="cloud_topic"             default="/camera/depth/points" />
  <arg name="image_topic"             default="/camera/depth/image" />
  <arg name="_approx"                 default="false" />
  <arg name="_fill_holes_size"        default="2" />
  
  <!-- stereo related topics -->
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
  
  <!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
  <arg name="subscribe_rgbd"          default="false"/>
  <arg name="rgbd_topic"              default="/camera/rgbd_image" />
  
  <arg name="compressed"              default="true"/>         <!-- If you want to subscribe to compressed image topics -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
  <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
   
  <arg name="subscribe_scan"          default="true"/>
  <arg name="scan_topic"              default="/scan_filtered"/>
  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  <arg name="scan_normal_k"           default="0"/>
   
  <arg name="visual_odometry"          default="false"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="odom_topic"               default="/scanmatcher_frame"/>         <!-- Odometry topic used if visual_odometry is false -->
  <arg name="vo_frame_id"              default="odom"/>
  <arg name="odom_tf_angular_variance" default="1"/>             <!-- If TF is used to get odometry, this is the default angular variance -->
  <arg name="odom_tf_linear_variance"  default="1"/>             <!-- If TF is used to get odometry, this is the default linear variance -->
  <arg name="odom_args"                default="$(arg rtabmap_args)"/>
  <arg name="odom_sensor_sync"         default="false"/>
  
  
  <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
  
  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>

  <node pkg="rtabmap_ros" type="pointcloud_to_depthimage" name="pointcloud_to_depthimage" args="$(arg cloud_topic) $(arg camera_info_topic) $(arg image_raw_topic) $(arg image_topic) $(arg _approx) $(arg _fill_holes_size)" />

  <!-- Nodes -->
  <group ns="$(arg namespace)">
  
    <!-- RGB-D Odometry -->
    <group unless="$(arg stereo)">
      <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
      <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
  
      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg odom_args)" launch-prefix="$(arg launch_prefix)">
        <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
        <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
        <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
        
        <remap from="rgbd_image"      to="$(arg rgbd_topic)"/>
	<param name="Vis/MinInliers"		  value="10"/>
	<param name="Odom/ResetCountdown"         value="1" />
	<!-- 0: 3D - 3D, 1: 3D-2D -->
	<param name="Odom/EstimationType" 	  value="1"/>
	<!-- Minimum Eucliedean distance in Mode 0 (3D-3D) -->
	<param name="Odom/InlinerDistance"	  value="0.2" />
        <!-- Reprojection error in Mode 1 (3D->2D) --> 
	<param name="Odom/PnPReprojError"	  value="5.0" />
	<!-- Maximum depth --> 
        <param name="Odom/MaxDepth"		  value="10.0" />
	<param name="GFFT/QualityLevel"		  value="0.001" />
	<param name="GFFT/MinDistance"		  value="3" />
	<param name="Kp/DetectorStrategy"	  value="6" />
        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
        <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
        <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
        <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
        <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
        <param name="config_path"                 type="string" value="$(arg cfg)"/>
        <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
        <param name="subscribe_rgbd"              type="bool"   value="$(arg subscribe_rgbd)"/>
      </node>
    </group>
    
    <!-- Stereo Odometry -->
    <group if="$(arg stereo)">
      <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="compressed in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
      <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
  
      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" args="$(arg odom_args)" launch-prefix="$(arg launch_prefix)">
        <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
        <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
        <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
        <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
	  
        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
        <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
        <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
        <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
        <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
        <param name="config_path"                 type="string" value="$(arg cfg)"/>
        <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
      </node>
    </group>
  
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="true"/>
      <param unless="$(arg stereo)" name="subscribe_rgbd"  type="bool" value="$(arg subscribe_rgbd)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="map_frame_id"         type="string" value="$(arg map_frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
      <param name="odom_tf_linear_variance"  type="double" value="$(arg odom_tf_linear_variance)"/>
      <param name="odom_sensor_sync"         type="bool"   value="$(arg odom_sensor_sync)"/>
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
      <param name="database_path"        type="string" value="$(arg database_path)"/>
      <param name="approx_sync"          type="bool"   value="$(arg approx_sync)"/>
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      <param name="scan_normal_k"        type="int" value="$(arg scan_normal_k)"/>

      <param name="Grid/FromDepth"       type="string"   value="true"/>
      <param name="RGBD/OptimizeMaxError" value="0.2" />
      <param name="Reg/Strategy"                  type="string" value="2"/> 
      <param name="RGBD/ProximityBySpace" type="string" value="true"/>
      <param name="Icp/CorrespondenceRatio"  type="string" value="0.25"/>
      <param name="Vis/MaxDepth"              type="string" value="10.0"/>      
      <param name="Vis/InlierDistance"        type="string" value="0.1"/>
      <param name="RGBD/AngularUpdate"           type="string" value="0.1"/>
      <param name="RGBD/LinearUpdate"            type="string" value="0.1"/>

      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap from="rgbd_image"      to="$(arg rgbd_topic)"/>
	
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap from="user_data"              to="$(arg user_data_topic)"/>
      <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
      
      <!-- localization mode -->
	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
	  <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="true"/>
      <param unless="$(arg stereo)" name="subscribe_rgbd"  type="bool" value="$(arg subscribe_rgbd)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_odom_info"  type="bool"   value="$(arg visual_odometry)"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
      <param name="queue_size"           type="int"    value="$(arg queue_size)"/>
      <param name="approx_sync"          type="bool"   value="$(arg approx_sync)"/>
    
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap from="rgbd_image"      to="$(arg rgbd_topic)"/>
      
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
    </node>
  
  </group>
  
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap from="right/image"       to="$(arg right_image_topic_relay)"/>
    <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
    <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
    <remap from="rgb/image"         to="$(arg rgb_topic_relay)"/>
    <remap from="depth/image"       to="$(arg depth_topic_relay)"/>
    <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    <remap from="cloud"             to="voxel_cloud" />

    <param name="decimation"  type="double" value="2"/>
    <param name="voxel_size"  type="double" value="0.02"/>
    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>

  <node pkg="tf" type="static_transform_publisher" name="footprint_to_base_link" 
    args="0.0 0.0 0.0 0.0 0.0 0.0 base_footprint base_link 100" />

  <node pkg="tf" type="static_transform_publisher" name="base_link_to_camera" 
    args="0.0 0.0 0.0 0.0 0.0 0.0 base_link camera_link 100" />

</launch>

Do you have an idea what I can change in my code? Also, I need the Z values as well. Using RTABMap I can easily extract them, but since odometry is provided by hector_slam, RTABMap ceases to output odometry and I can only rely on the topic /poseupdate from hector_slam to provide me with odometry values in the XY plane. I've removed the Force3DoF parameter from the launch file but I think it does not matter. Yet If I put the visual odometry argument to be true I still don't get anything :(. Any ideas? Mathieu please save me!

[matlabbe]

Hi,

I don't think you can use hector_mapping to estimate a 3D trajectory out-of-the-box, see this thread: https://answers.ros.org/question/226823/can-hector_slam-be-used-to-create-3d-models/

The hector_mapping+rtabmap example you are using is for a ground 2D robot. To do it on a drone in 3D, the odometry node should provide 6DoF instead of 3DoF (Reg/Force3DoF parameter should be removed from the example as you did for rtabmap node).

Another approach would be to estimate the 3D trajectory using visual odometry, while assembling the laser scans, like in this post: http://official-rtab-map-forum.206.s1.nabble.com/Visual-Odometry-tp2856p2894.html

cheers,
Mathieu

[utrecht]

Thank you for bringing this to my attention, I will try integrating the laser scans and an IMU and let you know how it went, thanks!

[utrecht]

Hey Mathieu,

I've succeeded in integrating the LiDAR and IMU. Well, sort of... couple of questions:

1. When running without the IMU I'm running into a bit of trouble with saving the map. It keeps disappearing after losing odometry (the R200 really sucks sometimes...) as seen below:

Before:



After:



Is this normal? It's very annoying, and I just wanted to know if I am getting it right. Even if it's not possible to keep the combined clouds from the R200, is it possible to not erase the assembled clouds from the LiDAR? (note, I used the hector_laser_to_pointcloud for convenience, check it out if you have the time https://github.com/team-vigir/vigir_lidar_proc/blob/master/vigir_laserscan_to_pointcloud/src/laserscan_to_pointcloud_node.cpp)

2. After combining with the LiDAR, I've come across a duplication problem once more, as seen below:



I've managed to correct it, thing is the pointcloud topic reverses in its yaw. If I remove it everything looks fine:



In the picture the pointcloud topic is the weird out of place one. If I uncheck the box in RVIZ everything looks ok.

Why do you think this is happening? Instead of using the robot_localization package I've decided to write my own node to publish the odometry information from the odom topic. It's unclean at the moment, but take a look:

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <sensor_msgs/Imu.h> 
#include <geometry_msgs/PoseStamped.h> 
#include <nav_msgs/Odometry.h> 

class State {
	private:
	ros::NodeHandle nh_;
	ros::Subscriber imu_sub_; 
	ros::Subscriber realsense_pose_sub_;
	ros::Subscriber lidar_pose_sub_;

	geometry_msgs::PoseStamped lidar_msg_;
	nav_msgs::Odometry realsense_msg_;

	tf::TransformBroadcaster br_;	   
	tf::Transform transform_;
 
	public:
	State() {
		imu_sub_ = nh_.subscribe("imu", 1, &State::imuCallback, this); 
		realsense_pose_sub_ = nh_.subscribe("rtabmap/odom", 1, &State::RealsenseCallback, this);
		transform_.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
		transform_.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0)); 
	} 
  
	void imuCallback(const sensor_msgs::Imu::ConstPtr &imu){
		tf::Quaternion quat = tf::Quaternion(imu->orientation.x, 
						     imu->orientation.y, 
					 	     imu->orientation.z,
					 	     imu->orientation.w);
		transform_.setRotation(quat);
		br_.sendTransform(tf::StampedTransform(transform_, ros::Time::now(), "map", "base_footprint")); 
	}

	void RealsenseCallback(const nav_msgs::Odometry::ConstPtr &msg){
		transform_.setOrigin(tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z));
		br_.sendTransform(tf::StampedTransform(transform_, ros::Time::now(), "map", "base_footprint"));
	}
};

int main(int argc, char** argv){
	ros::init(argc, argv, "state_tf"); 
	State s;
	ros::spin();

	return 0;
}

In addition, here is the TF tree and the launch file:

<?xml version="1.0"?>

<launch>
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="true" />
  
  <arg name="localization"            default="false"/>

  <node pkg="maple" type="ScanToPointcloud2" name="ScanToPointcloud2" respawn="true">
    <param name="min_range" value="0.25" />
    <param name="use_high_fidelity_projection" value="true" />
    <param name="target_frame" value="base_link" />
  </node>

  <node pkg="maple" type="IMUPositionEstimate" name="IMUPositionEstimate" />
  
  <!-- sim time for convenience, if playing a rosbag -->
  <arg name="use_sim_time"            default="false"/>
  <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
  
  <!-- Corresponding config files -->
  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="base_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="odom_frame_id"           default=""/>                <!-- If set, TF is used to get odometry instead of the topic -->
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     <!-- e.g., "world" -->
  <arg name="ground_truth_base_frame_id" default=""/>  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="rtabmap_args"            default="--delete_db_on_start"/>   <!-- deprecated, use "args" argument -->
  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
  <arg name="output"                  default="screen"/>        <!--Control node output (screen or log)-->
  
  <arg name="approx_sync"  default="true"/>         
   
  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/sw_registered/image_rect_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="image_raw_topic"         default="/camera/depth/points/image_raw" />
  <arg name="cloud_topic"             default="/camera/depth/points" />
  <arg name="image_topic"             default="/camera/depth/image" />
  <arg name="_approx"                 default="false" />
  <arg name="_fill_holes_size"        default="2" />
  
  <!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
  <arg name="subscribe_rgbd"          default="false"/>
  <arg name="rgbd_topic"              default="/camera/rgbd_image" />
  
  <arg name="compressed"              default="true"/>         <!-- If you want to subscribe to compressed image topics -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
  <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
   
  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="true"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  <arg name="scan_normal_k"           default="0"/>
   
  <arg name="visual_odometry"          default="true"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="odom_topic"               default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
  <arg name="vo_frame_id"              default="odom"/>
  <arg name="odom_tf_angular_variance" default="1"/>             <!-- If TF is used to get odometry, this is the default angular variance -->
  <arg name="odom_tf_linear_variance"  default="1"/>             <!-- If TF is used to get odometry, this is the default linear variance -->
  <arg name="odom_args"                default="$(arg rtabmap_args)"/>
  <arg name="odom_sensor_sync"         default="false"/>
  
  
  <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
  
  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>

  <!-- Nodes -->
  <group ns="$(arg namespace)">
  
    <!-- RGB-D Odometry -->
      <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
      <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
  
      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg odom_args)" launch-prefix="$(arg launch_prefix)">
        <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
        <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
        <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
        <remap from="rgbd_image"      to="$(arg rgbd_topic)"/>

	<param name="publish_tf"             type="bool"   value="true"/>
	<param name="guess_from_tf"		  type="bool"	value="false"/>
	<param name="guess_frame_id"		  type="string"	value="base_footprint"/>
	<param name="Odom/FillInfoData"      type="string" value="true"/>
	<param name="Vis/FeatureType"        type="string" value="6"/>
	<param name="OdomF2M/MaxSize"        type="string" value="1000"/>
	<param name="Vis/MinInliers"		  value="8"/>
	<param name="Odom/ResetCountdown"         value="1" />
	<!-- 0: 3D - 3D, 1: 3D-2D -->
	<param name="Odom/EstimationType" 	  value="1"/>
	<!-- Minimum Eucliedean distance in Mode 0 (3D-3D) -->
	<param name="Odom/InlinerDistance"	  value="0.2" />
        <!-- Reprojection error in Mode 1 (3D->2D) --> 
	<param name="Odom/PnPReprojError"	  value="5.0" />
	<!-- Maximum depth --> 
        <param name="Odom/MaxDepth"		  value="10.0" />
	<param name="GFFT/QualityLevel"		  value="0.001" />
	<param name="GFFT/MinDistance"		  value="3" />
	<param name="Kp/DetectorStrategy"	  value="6" />
        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
        <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
        <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
        <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
        <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
        <param name="config_path"                 type="string" value="$(arg cfg)"/>
        <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
        <param name="subscribe_rgbd"              type="bool"   value="$(arg subscribe_rgbd)"/>
      </node>
  
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param name="subscribe_depth"  type="bool" value="true"/>
      <param name="subscribe_rgbd"  type="bool" value="$(arg subscribe_rgbd)"/>
      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="map_frame_id"         type="string" value="$(arg map_frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
      <param name="odom_tf_linear_variance"  type="double" value="$(arg odom_tf_linear_variance)"/>
      <param name="odom_sensor_sync"         type="bool"   value="$(arg odom_sensor_sync)"/>
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
      <param name="database_path"        type="string" value="$(arg database_path)"/>
      <param name="approx_sync"          type="bool"   value="$(arg approx_sync)"/>
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      <param name="scan_normal_k"        type="int" value="$(arg scan_normal_k)"/>

      <param name="Rtabmap/DetectionRate"          type="string" value="0"/>
      <param name="RGBD/LinearUpdate"              type="string" value="0"/>
      <param name="RGBD/AngularUpdate"             type="string" value="0"/>
      <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
      <param name="Grid/FromDepth"                 type="string" value="true"/>
      <param name="RGBD/OptimizeMaxError"          value="0.2" />

      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap from="rgbd_image"      to="$(arg rgbd_topic)"/>
	
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap from="user_data"              to="$(arg user_data_topic)"/>
      <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
      
      <!-- localization mode -->
	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
	  <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
      <param name="subscribe_depth"  type="bool" value="true"/>
      <param name="subscribe_rgbd"  type="bool" value="$(arg subscribe_rgbd)"/>
      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_odom_info"  type="bool"   value="$(arg visual_odometry)"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
      <param name="queue_size"           type="int"    value="$(arg queue_size)"/>
      <param name="approx_sync"          type="bool"   value="$(arg approx_sync)"/>
    
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap from="rgbd_image"      to="$(arg rgbd_topic)"/>
            
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
    </node>
  
  </group>
  
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="rgb/image"         to="$(arg rgb_topic_relay)"/>
    <remap from="depth/image"       to="$(arg depth_topic_relay)"/>
    <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    <remap from="cloud"             to="voxel_cloud" />

    <param name="decimation"  type="double" value="2"/>
    <param name="voxel_size"  type="double" value="0.05"/>
    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>

  <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0 0 0 0 base_footprint base_link 100"/>
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_camera" args="0 0 0 0 0 0 base_link camera_link 100"/>

</launch>

BTW, maple is the name of my package. The nodes are pretty self-explanatory I assume. Does anything jump at you? Something obviously wrong?

Many gratitudes :)

[matlabbe]

Hi,

so you are not using hector_mapping anymore, right? The State class should publish TF between /odom and /base_footprint, not /map to /base_footprint. You would want to send only one time the TF, not one time just orientation, then another time just position... merge orientation of IMU and pose from rgbd_odometry in one callback (like an approximate synchronizer). When another node publishes odometry TF, you should set "publish_tf" to false for rgbd_odometry node. Global "frame_id" should be base_footprint (the root of the robot).

For the map disappearing, it is because rtabmap would have received an odometry with covariance over 9999, which is published only when lost and if Odom/ResetCountdown is set (which is set to 1 here) or "publish_null_when_lost" is set. Currently rtabmap seems connected to /rtabmap/odom topic coming from rgbd_odometry (which is default), but to use TF for odometry from your IMUEstimate node, set "odom_frame_id" to odom and /rtabmap/odom topic will be ignored.

Note that with robot_localization package, you can also do fusion of only position from an odometry topic and only orientation from IMU topic, similar to sensor_fusion.launch example.

In your current setup, only one scan per node will be saved. You may want to use laser_assembler like in the link of my previous post. Unless you use the laser_assembler approach publishing assembled scans at 1 or 2 Hz, you may not want to set Rtabmap/DetectionRate to 0 (to avoid large memory and computation usage).

cheers,
Mathieu

[utrecht]

Thanks for the advice Mathieu, but given the weakness of the R200, I think I might change my approach as a whole so that I could consistently assemble the pointclouds from the LIDAR. I appreciate your help though!

[utrecht]

Actually, could you refer me to the segment in RTABMap's code where you concatenate the PointCloud2 messages from the scan_cloud topic? Would be much appreciated!

[matlabbe]

Hi,

It depends on what you want as output. Is there a particular output cloud topic that you want to know how it is assembled form the actual ones published by rtabmap node? Output map topics are assembled in MapsManager class. They don't use raw laser scans directly though, but local occupancy grids (which can be created from laser scans when "Grid/FromDepth" is false) for efficiency reasons.

If you are referring to cyan laser scans shown in rtabmapviz, they are not concatenated but added separately (each node of the map's graph) to OpenGL renderer from MainWindow::createAndAddScanToMap().

If you want to generate the raw concatenated laser scans after mapping, this can be done with "File->Export 3D clouds..." from rtabmapviz or standalone RTAB-Map application (by opening the generated database), then check top laser scan checkbox to use laser scans as point cloud instead of depth images. Clouds are actually assembled here.

If you want to get the assembled raw laser scans (like the File->Export above), you should do it yourself by subscribing to /rtabmap/mapData topic. Convert node data from the ROS msg to Signature class (Signature is a node in the graph) like here, look for nodes with laser scan set (normally the last node should always have data), then instead of using MapsManager, you can convert the laser scan field of Signature.sensorData() (make sure to do Signature.sensorData().uncompress() before to uncompress laser scans) to a point cloud by using rtabmap::util3d::laserScanToPointCloud() with transform set to Signature.sensorData().laserScanInfo().localTransform(). You will have clouds in "frame_id" frame, now you should transform the point clouds according to corresponding node's pose (poses from MapData ROS msg can be converted like this).

cheers,
Mathieu

[utrecht]

Cheers Mathieu, much appreciated!