Loop closure rejected

[Paco]

Hi,
I'm working on Pepper Robot (IR, RGB, laser, odom)
in addition to my trouble with depth calibration I'm facing some problem with loop closure detection.
I still didn't manage to run depth calibration, but I'm testing RTABMap with ROS Wiki tutorial. Depth + RGB (except depth calibration), 2D laser scan and odometry work and create a map (with some distortion due to depth error from Pepper's Asus Xtion) but all loop closure are rejected, feature are matched but always rejected. When I launch RTABMap with RGBD/Enabled -> False, loops are detected but I need Depth cloud to create a map. I'm missing something in the configuration. I've only neighbor link between image (on the constraints view with rtabmap-databaseViewer).


RTABMAP fail to compute transform every time a loop closure is detected :
 [WARN] (2017-02-23 11:27:53.195) Rtabmap.cpp:1799::process() Rejected loop closure 38 -> 233: Cannot compute transform (converged=false var=1.000000)

And a lots of warning of this type :
[ WARN] (2017-02-23 11:29:28.121) Rtabmap.cpp:2061::process() Local scan matching rejected: Cannot compute transform (converged=false var=1.000000)
[ WARN] (2017-02-23 11:29:28.121) Rtabmap.cpp:2061::process() Local scan matching rejected: Cannot compute transform (cor=5 corrRatio=0.081967/0.200000)
[ WARN] (2017-02-23 11:29:28.122) Rtabmap.cpp:2061::process() Local scan matching rejected: Cannot compute transform (cor=6 corrRatio=0.098361/0.200000)


I've identified thanks to this post that the problem comes from ICP, so I've switch to Vis only with :
Reg/Strategy = 0

Loop are now accepted (some times) but I'm loosing precision in the cloud generated. And frequently if a loop is accepted then I'm facing this error :
[ INFO] [1487861237.501954347]: rtabmap (247): Rate=0.00s, Limit=0.700s, RTAB-Map=0.0301s, Maps update=0.0022s pub=0.0001s (local map=122, WM=122)
[pcl::IterativeClosestPoint::computeTransformation] Not enough correspondences found. Relax your threshold parameters.
[ WARN] (2017-02-23 15:47:18.030) Rtabmap.cpp:2061::process() Local scan matching rejected: Cannot compute transform (converged=false var=1.000000)
[ERROR] (2017-02-23 15:47:18.032) OptimizerGTSAM.cpp:265::optimize() GTSAM exception caught:
Indeterminant linear system detected while working near variable
126 (Symbol: 126).

Thrown when a linear system is ill-posed.  The most common cause for this
error is having underconstrained variables.  Mathematically, the system is
underdetermined.  See the GTSAM Doxygen documentation at
http://borg.cc.gatech.edu/ on gtsam::IndeterminantLinearSystemException for
more information.
[ERROR] (2017-02-23 15:47:18.032) Rtabmap.cpp:3020::optimizeCurrentMap() Failed to optimize the graph! returning empty optimized poses...
[ WARN] (2017-02-23 15:47:18.032) Rtabmap.cpp:2179::process() Graph optimization failed! Rejecting last loop closures added.
[ WARN] (2017-02-23 15:47:18.032) Rtabmap.cpp:2183::process() Loop closure 248->160 rejected!
[ WARN] (2017-02-23 15:47:18.032) Rtabmap.cpp:2183::process() Loop closure 248->165 rejected!
[ INFO] [1487861238.036882990]: rtabmap (248): Rate=0.00s, Limit=0.700s, RTAB-Map=0.0383s, Maps update=0.0023s pub=0.0001s (local map=122, WM=122)
[pcl::IterativeClosestPoint::computeTransformation] Not enough correspondences found. Relax your threshold parameters.
[ WARN] (2017-02-23 15:47:19.682) Rtabmap.cpp:2061::process() Local scan matching rejected: Cannot compute transform (converged=false var=1.000000)


EDIT: Thanks to this post I changed from GTSAM to TORO and turn back on RGBD/Enabled=1 . Now I've some loop closure but the result still messy.

I'll upload soon a db that I've collected with Pepper robot. EDIT2: this is the db : https://drive.google.com/open?id=0B8ZKa1H_np_EUmdyc0ROYmtYc2s

This is my launch file :

<launch>
 

 

 

 
  <arg name="stereo"          default="false"/>

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

 
  <arg name="localization"            default="false"/>

 
  <arg name="cfg"                     default="" /> 
  <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"/>     
  <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="rtabmap_args"            default="delete_db_on_start"/>             
  <arg name="launch_prefix"           default=""/>             

 
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="true"/>

 
  <arg name="rgb_topic"               default="/pepper_robot/camera/front/image_rect_color" />
  <arg name="depth_topic"             default="/pepper_robot/camera/depth_registered/sw_registered/image_rect" />
  <arg name="camera_info_topic"       default="/pepper_robot/camera/front/camera_info" />

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

  <arg name="compressed"              default="false"/>         
  <arg name="rgb_image_transport"     default="compressed"/>   
  <arg name="depth_image_transport"   default="compressedDepth"/> 

  <arg name="subscribe_scan"          default="true"/>
  <arg name="scan_topic"              default="/pepper_robot/laser"/>

  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>

  <arg name="visual_odometry"         default="false"/>         
  <arg name="odom_topic"              default="/pepper_robot/odom"/>       
  <arg name="odom_frame_id"           default="odom"/>         
  <arg name="odom_args"               default="$(arg rtabmap_args)"/>

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

 
  <group ns="$(arg namespace)">

   
    <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="screen" 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)"/>

       

       
       
       
       
       
      </node>
    </group>

   
    <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="screen" 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)"/>

       
       
       
       
       
      </node>
    </group>

   
   
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_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="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>

   
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="screen" 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="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>

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

   
   
   
  </node>

</launch>

[Paco]

Welll after many try, I've found that if I turn down video quality to QQVGA this allow me to increase the fps from 10 -> 20. This seems to give me better result (10fps seems really too low).

High FPS is better than quality for RTABMAP ?
I've run some test with QVGA 10fps and QQVGA 20fps, QQVGA seems to have generate better result :


this is the result with QVGA at 10fps (this result rejected all loop closure):

[matlabbe]

Hi,

Higher frame rate would help to better synchronize laser scan, odometry and RGB-D image. What is the frame rate of your sensors?

$ rostopic hz /pepper_robot/laser
$ rostopic hz /pepper_robot/odom
$ rostopic hz /pepper_robot/camera/front/image_rect_color
$ rostopic hz /pepper_robot/camera/depth_registered/sw_registered/image_rect
$ rostopic hz /pepper_robot/camera/front/camera_info

"RGBD/Enabled" should be true for metric mapping. When it is set to false, only appearance-based loop closure detection is done (without any metric information) like in this paper.

Looking at the database, the laser scan has a very low number of points (e.g., 10-15 points!). So yes, no loop closures would be accepted if "Reg/Strategy" is 1 (Icp) or 2 (Vis+Icp). What is the LiDAR used?

What also is the RGB-D camera used? It seems to have high wavy distortions even below 2 meters:

Without a precise depth image, loop closure transformations would be bad (or could not be accepted). Also in this case, CLAMS depth calibration may not help, as it requires at least that points under ~4 meters are relatively precise. CLAMS helps to undistort points over ~4-5 meters up to ~10 meters with a Xtion Live Pro or a Kinect XBOX 360.

cheers,
Mathieu

[Paco]

Hi mathieu,

Sorry for the delay, I was trying to improve the frame rate coming from the Pepper robot (ROS Aldebaran related post) and finally succeed to get 24fps from the camera which give me 20fps limited by the depth image. I didn't retry to run rtabmap yet.

matlabbe wrote

Hi,

Higher frame rate would help to better synchronize laser scan, odometry and RGB-D image. What is the frame rate of your sensors?

The frame rate of the sensor was really low (10~15fps from the camera).

matlabbe wrote

Looking at the database, the laser scan has a very low number of points (e.g., 10-15 points!). So yes, no loop closures would be accepted if "Reg/Strategy" is 1 (Icp) or 2 (Vis+Icp). What is the LiDAR used?

What also is the RGB-D camera used? It seems to have high wavy distortions even below 2 meters:

As I just said I'm working with the Pepper robot : Technical description which is equipped with the ASUS Xtion, and you are right the point cloud have high distortions, walls seems always wavy. I don't understand why the result from the sensor is so poor. We tried with 2 different Pepper and the results are pretty much the same from rviz view. I'll post some screen of the depth view tomorrow.
About lasers that's pretty much all we have from the robot which is equipped with 1 frontal laser and 2 side laser on the base, and one "shovel" laser Technical doc (I don't think those points are passed to rtabmap, I'll take a look).

[matlabbe]

Hi,

The single point lasers cannot be used for mapping, but they could be used for the local costmap of move_base (if navigation is done). RTAB-Map assumes that laser scans are from 2D LiDAR like Hokuyo or SICK. For the camera, it is indeed strange that the ASUS Xtion depth is so wavy. See this video of my Xtion Live PRO with default calibration: https://www.youtube.com/watch?v=cjDFDPpLnRc&feature=youtu.be.

Note that the frame rate of the rgb and depth images is not so important (at least over 1 Hz) if you use don't do visual odometry and they have exact timestamps (so that rtabmap can subscribe to them with exact time synchronization: approx_sync=false).

cheers,
Mathieu

[flier]

Maybe it's a little late concerning this reply... We are also struggling with the Robot's navigation caps, ... but that's another story.

Basically, the depth image is so poor because Aldebaran/Softbank decided to put "lenses" in front of the IR-emitter:

[Load]

Hi Paco, I have tried the RTABSLAM in turtlebot and I want to use this in pepper too. However , I meet some problems:
How do you use ros in pepper , like the wiki http://wiki.ros.org/nao/Tutorials/Installation 
I try two ways :
 a. I install ROS on a computer and running some ROS nodes to connect to pepper but I meet the problem that too much delay between pepper and my computer.
b.I compiled and install ROS on pepper with virtual-nao but I cannot compiled the navigation because of pcl.
So what's way do you use?
Look forward to your kind advice!

[flier]

You can tune a few things... Try 5 GHZ-based wifi this reduces the delay. Try to stream data at lower frequencies.

[Load]

Thanks for your replay.But the rate of image between pepper and my conputer is really slow about three seconds each frame. It's terrible..

[flier]

Okay, so we also tried this and we can get up to 15 FPS streaming only RGB images. We are using 5 GHZ channel A/C only. If you stream the RGB and Depth image we can get up to 7 FPS each. Resolutions are 640x480 color and 320x240 (thats also the maximum) for depth. Maybe try to setup your system like that. If that doesn't help try to debug your Wifi connection using network tools like ...

https://www.cyberciti.biz/tips/linux-find-out-wireless-network-speed-signal-strength.html

try to 'iperf' the connection between your computer and pepper

https://openmaniak.com/iperf.php

[Load]

Thanks !I will try the this.