Issue Getting Map from Rtabmap

[Joel_A]

Hi again, I decided not to post this in my previous thread because its not the same robot or setup, this time its a 3D lidar but it is the same problem as before. I've been getting some pretty maps vizualized in RVIZ by setting the decay time for the pointcloud2 topic high and using Rtabmap as an odometry node using my realsense camera and a velodyne lidar.



The image above is what I get after i'm done using it, it doesn't save any map in the database nor does it give me any errors about not receiving data or making sure I'm subscribed to certain topics. It seems to function fine but as just a very nice odometry node. Here is my launch file:

<launch>

<arg name="rgbd"    default="false"/>
<arg name="pm"      default="false"/>
<arg name="nodelet" default="false"/>

<group ns="camera">
<group if="$(arg nodelet)">
      <node unless="$(arg rgbd)" pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_ros/rbgd_odometry camera_nodelet_manager" output="screen">
        <remap from="depth/image"       to="/camera/aligned_depth_to_color/image_raw"/>
        <remap from="rgb/camera_info"   to="/camera/color/camera_info"/>
        <remap from="rgb/image"         to="/camera/color/image_raw"/>
        <param name="subscribe_rgbd"  type="bool"   value="true"/>

	  
        <param name="frame_id"        type="string" value="base_link"/>   

        <param name="Odom/GuessMotion"  type="string" value="true"/>
        <param name="Odom/ResetCountdown"  type="string" value="1"/>
      </node>
</group>

<group unless="$(arg nodelet)">
      <node unless="$(arg rgbd)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
	<remap from="depth/image"       to="/camera/aligned_depth_to_color/image_raw"/>
        <remap from="rgb/camera_info"   to="/camera/color/camera_info"/>
        <remap from="rgb/image"         to="/camera/color/image_raw"/>

	  
        <param name="frame_id"        type="string" value="base_link"/>   

        <param name="Odom/GuessMotion"  type="string" value="true"/>
        <param name="Odom/ResetCountdown"  type="string" value="1"/>

	<!-- 0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) -->
	<param name="Odom/Strategy" value="0"/>
	
	<!-- Correspondences: 0=Features Matching, 1=Optical Flow -->
	<param name="Vis/CorType" value="0"/>
      </node>

      <param name="GFTT/MinDistance" type="string" value="2"/> <!-- default 5 pixels -->

      <!-- maximum features map size, default 2000 -->
      <param name="OdomF2M/MaxSize" type="string" value="5000"/> 
      <!-- maximum features extracted by image, default 3000 -->
      <param name="Vis/MaxFeatures" type="string" value="1000"/>
</group>
</group>


<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id"        type="string" value="base_link"/>
          <param name="subscribe_depth" type="bool"   value="false"/>
          <param name="subscribe_rgbd"  type="bool"   value="false"/>
          <param name="subscribe_scan"  type="bool"   value="false"/>
          <param name="subscribe_scan_cloud"  type="bool"   value="true"/>


          <remap from="odom"      	  to="/odom1"/>
          <remap from="scan_cloud"        to="/velodyne_points"/>
          <remap from="depth/image"       to="/camera/aligned_depth_to_color/image_raw"/>
          <remap from="rgb/camera_info"   to="/camera/color/camera_info"/>
          <remap from="rgb/image"         to="/camera/color/image_raw"/>

          <param name="queue_size"  type="int"  value="10"/>
          <param name="approx_sync" type="bool" value="false"/>

          <!-- RTAB-Map's parameters -->
          <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
          <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
          <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
    </node>

</launch>

Any help would be greatly appreciated!

Oh real quick edit! The reason I remapped the odom in the launch file above to /odom1 near the end there is because I am using robot_localization package to take rtabmap's visual odometry and fuse it with a nice IMU I have and it spits out that as odom1 so it doesn't conflict with rtabmap's /odom.

[matlabbe]

Hi,

For rgbd_odometry, I would choose between the nodelet version or not, to avoid confusion between both (they even have different parameters). As you are doing fusion of odometry, make sure to disable publish_tf from rgbd_odometry node to avoid conflict with tf published by robot_localization for example.

<!-- under rgbd_odometry node -->
<param name="publish_tf" value="false"/>

I don't recommend synchronizing all those topics together:

<remap from="odom"      	  to="/odom1"/>
<remap from="scan_cloud"        to="/velodyne_points"/>
<remap from="depth/image"       to="/camera/aligned_depth_to_color/image_raw"/>
<remap from="rgb/camera_info"   to="/camera/color/camera_info"/>
<remap from="rgb/image"         to="/camera/color/image_raw"/>

Like in the examples on this page, use rgbd_sync nodelet if you are synchronizing images with a lidar or external odometry (not synced with images). In your case, it would look like this:

    <!-- Use RGBD synchronization -->
    <!-- Here is a general example using a standalone nodelet, 
         but it is recommended to attach this nodelet to nodelet 
         manager of the camera to avoid topic serialization -->
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
      <remap from="rgb/image"       to="/camera/color/image_raw"/>
      <remap from="depth/image"     to="/camera/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
      <remap from="rgbd_image"      to="rgbd_image"/> <!-- output -->

      <!-- Should be true for not synchronized camera topics 
           (e.g., false for kinectv2, zed, realsense, true for xtion, kinect360)-->
      <param name="approx_sync"       value="false"/> 
    </node>

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id" type="string" value="base_link"/>

          <param name="subscribe_depth" type="bool" value="false"/>
          <param name="subscribe_rgbd" type="bool" value="true"/>
          <param name="subscribe_scan_cloud" type="bool" value="true"/>

          <remap from="odom" to="/odom1"/>
          <remap from="scan_cloud" to="/velodyne_points"/>
          <remap from="rgbd_image" to="rgbd_image"/>

          <param name="queue_size" type="int" value="10"/>
          <param name="approx_sync" type="bool" value="true"/>

          <!-- RTAB-Map's parameters -->
          <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
          <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
          <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
          <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
          <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
          <param name="Grid/FromDepth"            type="string" value="false"/> <!-- occupancy grid from lidar -->
          <param name="Reg/Force3DoF"             type="string" value="true"/> <!-- 2D mapping? -->
          <param name="Reg/Strategy"              type="string" value="1"/> <!-- 1=ICP -->
          
          <!-- ICP parameters -->
          <param name="Icp/VoxelSize"                 type="string" value="0.2"/>
          <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.2"/>
    </node>

Note that approx_sync should be true for rtabmap node, as velodyne and camera topics would not be exactly synchronized. This is probably the reason why the map was empty (though I am surprised you didn't have warnings about rtabmap not receiving topics...).

cheers,
Mathieu

[Joel_A]

Thanks for the help! It did improve the odom quality and clearly I had some confusion on how I was doing the launch file. I cleaned it up as can be seen below but I still get the same message of 0MB saved. This is also the case with approx_sync set to true. Do you know what else it could be? I used the launch file on 3 different computers.

<launch>


<group ns="camera">

      <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
	<remap from="depth/image"       to="/camera/aligned_depth_to_color/image_raw"/>
        <remap from="rgb/camera_info"   to="/camera/color/camera_info"/>
        <remap from="rgb/image"         to="/camera/color/image_raw"/>

	  
        <param name="frame_id"        type="string" value="base_link"/>   

        <param name="Odom/GuessMotion"  type="string" value="true"/>
        <param name="Odom/ResetCountdown"  type="string" value="1"/>

	<!-- 0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) -->
	<param name="Odom/Strategy" value="0"/>

	<param name="publish_tf" value="false"/>
      </node>

</group>


    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
     	 <remap from="rgb/image"       to="/camera/color/image_raw"/>
     	 <remap from="depth/image"     to="/camera/aligned_depth_to_color/image_raw"/>
     	 <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
     	 <remap from="rgbd_image"      to="rgbd_image"/>

     	 <param name="approx_sync"       value="false"/> 
    </node>

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
          <param name="frame_id" type="string" value="base_link"/>

          <param name="subscribe_depth" type="bool" value="false"/>
          <param name="subscribe_rgbd" type="bool" value="true"/>
          <param name="subscribe_scan_cloud" type="bool" value="true"/>

          <remap from="odom" to="/odom"/>
          <remap from="scan_cloud" to="/velodyne_points"/>
          <remap from="rgbd_image" to="rgbd_image"/>

          <param name="queue_size" type="int" value="10"/>
          <param name="approx_sync" type="bool" value="true"/>

          <!-- RTAB-Map's parameters -->
          <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
          <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
          <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
          <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
          <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
          <param name="Grid/FromDepth"            type="string" value="false"/> <!-- occupancy grid from lidar -->
          <param name="Reg/Force3DoF"             type="string" value="true"/> <!-- 2D mapping? -->
          <param name="Reg/Strategy"              type="string" value="1"/> <!-- 1=ICP -->
          
          <!-- ICP parameters -->
          <param name="Icp/VoxelSize"                 type="string" value="0.2"/>
          <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.2"/>
    </node>

</launch>

[matlabbe]

Hi,

If the database is 0MB, it means rtabmap is not receiving the topics. There should be warnings on the terminals, what are they? What is the rqt_graph?

[Joel_A]

Hi, so this morning I finally figured it out. I changed the remap of odom to /odometry/filtered which is what robot_localization is outputting. Found this out because I was checking out the separate data_recorder launch file and it did give me the error of some topic isn't receiving data so I did a rostopic echo on the /odom and found out it was empty. I can share the output however for my launch file above, it was not telling me that it wasn't receiving data and it was outputting really good odometry data so it is really strange. It works now though, I'm now attempting to somehow assemble the laser scans into a point cloud.

I do have a separate question also. Is the depth part of rgbd odometry necessary to work with rtabmap? I'm asking because monocular cameras are much cheaper than a realsense and when I tried making and calibrating my own stereo camera it wasn't nearly as decent as a realsense so I want to try multiple cameras orthogonal to each other and see how rgb odometry works if I fuse like 2 or 4 monocular cameras together. This is for wall features inside a concrete pipe so the depth isn't really variable as you travel down it and all I want are the lidar points in the final maps so cameras are purely for odometry.

I really appreciate you taking your time to answer these posts, it is incredibly helpful.

[matlabbe]

Hi Joel,

In rtabmap, we don't have monocular rgb-only odometry, a stereo pair or a depth image is required to triangulate accurately the 2D features (thus to estimate trajectory). If visual loop closures are not possible anyway in that kind of environment (no many repetitive similar features), you may use rtabmap with lidar and odometry inputs only (subscribe_depth=false, subscribe_rgb=false, subscribe_scan_cloud=true). To provide odometry, you may look for an external visual-inertial odometry (VIO) package like VINS-Mono or a multi-camera odometry approach like MCPTAM. It may be tedious to setup that kind of packages (in particular VIO approaches), you may try Realsense T265 camera for a solution out-of-the-box. Note that for pipes, if there are not enough visual features, visual-based odometry will fail or drift a lot. As pipe geometry is also a problem for lidar odometry, maybe you could estimate the distance moved in the pipe with encoders fused (robot_localization package) with an IMU (use only accelerometer and gyroscope to estimate quaternion as magnetometer may not work properly underground) to know the orientation.

cheers,
Mathieu

[Artem Lobantsev]

Hi Mathieu,

In this post you said, that RTAB-Map able to provide only RGB localization on the pre-built map. How does it work with unsupported RGB-only odometry? During RGB-only localization where RTAB-Map get the odometry?

Thanks for the answer!

Cheers,
Artem

[matlabbe]

Hi Artem,

in that example, there is no odometry, it does only global localization. If there is no localization, you don't have any pose. As long it can localize, you will get a refreshed pose.

cheers,
Mathieu

[Artem Lobantsev]

Thanks for clarification!