Mapping on featureless enviroments.

[Coyotzi]

Hello I have been using RTAB-Map for my master thesis for a while now and found it works pretty well! However i tested it on a different floor with very little features on the environment, it is a plain white corridor, and the systems gets "lost" easily even if there is enough laser info to find correspondences. There is also the problem that the icp module says there are not enough correspondences and i should relax the parameters but i am not sure of all the parameters which do that. I know this is appearence based but is there a way to improve performance on low texture environments like hallways? I am currently using this launch file.

<launch>

    <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_stereo" type="bool" value="false"/>
      <param name="subscribe_rgbd" type="bool" value="false"/>
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_scan" type="bool" value="true"/>
      <param name="visual_odometry" type="bool" value="false"/>

      <remap from="odom" to="/hsrb/odom"/>
      <remap from="scan" to="/hsrb/base_scan"/>


      <remap from="rgb/image" to="/hsrb/head_rgbd_sensor/rgb/image_rect_color"/>
      <remap from="depth/image" to="/hsrb/head_rgbd_sensor/depth_registered/image_raw"/>
      <remap from="rgb/camera_info" to="/hsrb/head_rgbd_sensor/rgb/camera_info"/>
      <param name="subscribe_stereo" type="bool" value="false"/>

      <param name="queue_size" type="int" value="25"/>

      <!-- RTAB-Map's parameters -->
      <param name="Grid/FromDepth" type="string" value="false"/>
      <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/>
      <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/OptimizeMaxError" type="string" value="3.0"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
   
      <!-- Optimizer's parameters -->
      <param name="Optimizer/Strategy" type="string" value="1"/>      <!-- g2o=1, GTSAM=2 -->
      <param name="Optimizer/Robust" type="string" value="true"/>
      <!-- 1=ICP -->
      <param name="Reg/Force3DoF" type="string" value="true"/>
      <param name="Vis/MinInliers" type="string" value="5"/>
      <param name="Vis/InlierDistance" type="string" value="0.1"/>
      <param name="Rtabmap/TimeThr" type="string" value="900"/>
      <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>

      <param name="Grid/RangeMax" value="4.0" type="double"/>
      <param name="Grid/MaxGroundHeight" value="0.1" type="double"/>
      <param name="Grid/MaxObstacleHeight" value="2.0" type="double"/>
      <!-- <param name="cloud_max_depth" value="2.0" type="double"/>
      <param name="Grid/NoiseFiltering_radius" value="0.05"/>
      <param name="cloud_noise_filtering_min_neighbors" value="10"/> -->
    </node>

   <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d /home/takeshi/.rtabmap/rtabmap.ini" output="screen">
      <param name="frame_id" type="string" value="base_link"/>
      <param name="queue_size" type="int" value="30"/>
      <param name="approx_sync" type="bool" value="true"/>

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

      <remap from="rgb/image" to="/hsrb/head_rgbd_sensor/rgb/image_rect_color"/>
      <param name="rgb/image_transport" type="string" value="compressed"/>
      <remap from="scan"                to="/hsrb/base_scan"/>
      <remap from="odom" to="/hsrb/odom"/>
   </node>
</launch>

Thanks in advance

[matlabbe]

Hi,

When you say "systems gets lost easily", do you refer to odometry or localization? How is computed "/hsrb/odom"? What is the range of your lidar?

By default, Icp parameters are tuned assuming that odometry is relatively good.

$ rtabmap --params | grep Icp/

If odometry drifts very fast, you may increase Icp/MaxCorrespondenceDistance parameter. Can you show the warnings you have?

cheers,
Mathieu

[Coyotzi]

What I meant was that the robot poses drifts from the real trajectory considerably resulting in duplicated corridors on the 3D pointcloud and wall apearring where there should be nothing.
I am using a Toyota HSR the odom topic is based on wheel encoders on a holonomic base.
If I understood the warnings correctly RTAB map reports it can't find good correspondences  thus failing to find loop closures and sometimes it finds "incorrect" loop closures leading to a mapping failure.

Another thing I noticed is that I usually get:

[pcl::IterativeClosestPoint::computeTransformation] Not enough correspondences found. Relax your threshold parameters. 
Rtabmap.cpp:1755::process() Rejected loop closure 124 -> 284: Not enough inliers
Rtabmap.cpp:1755::process() Rejected loop closure 151 -> 316: Cannot compute transform covariance(....

Also:

[ERROR]OptimizerG2O.cpp:710::optimize() Computing marginals: vertex 151 has negative hessian index (-1). Cannot compute last pose covariance.

At that point the mapping usually fails.

Thanks in advance

[matlabbe]

Hi,

For the g2o error, it is now a warning in latest version. Do you have a rtabmap database to share? The pose drift would be caused by the wheel odometry. However, as you set RGBD/NeighborLinkRefining to true, the lidar would have corrected most of this drift, but for some reasons ICP fails.

cheers,
Mathieu

[Coyotzi]

I agree. Even thought the corridor wall are almost featureless I am pretty sure the Hokuyo reading has enough info to be able to correct the drift. But icp keeps failing.

Unfortunately no my rtabmap db file got corrupted for some other bug while trying to save it.
We only have one robot in the lab at the moment so i haven't been able to run another test. Hopefully In the near future i will be able to try again and reproduce the error.
In any case thanks for your help!

[Coyotzi]

Well i finally managed to get another run with the robot.
This is the database:
PDS.db
Fell free to use it anyway you see fit.
And an image:

As you can see the hallway got duplicated and the pose graph diverged horribly from the real path. Also the hallway has
very little feature or texture as it is a plain white wall.

[matlabbe]

Hi,

What is your odometry source? I see a large odometry drift (actually it seems the robot is not moving forward but it should) at some point causing the double corridor problem. Also, the lidar seems tilted toward the ground, it is not perfectly horizontal and the ground is seen as a moving wall. This would cause large scan matching failures. If your odometry is based on the lidar, this may be the cause of the large odometry drift I see.

cheers,
Mathieu

[Coyotzi]

I did mention I am using wheel encoders as an odometry source. No idea whats with the laser tilting it looked fined last time i checked

[matlabbe]

It is maybe because RGBD/NeighborRefining is true that consecutive scans are matched. Here are some examples where some parts of the scans are correctly aligned (purple and yellow), while some other parts show fake walls.
 
(I inverted the cloud view in this first one to better see the scans)