Incomplete depth cloud and no loop closures with Realsense R200

[danmou]

Hi Mathieu,

First of all, thanks for your awesome work with RTAB-Map! It is nice to see a piece of open-source software that is both well-developed and well-supported.

I'm using RTAB-Map with the Intel Realsense R200 on the lowest depth resolution (320x240) because I need the minimum depth of 31cm which that gives (and I don't have much processing power available either). However, I'm having two issues, which I think could be related:

1. In rtabmapviz the depth cloud seems to be missing points, giving sort of a checkerboard pattern:

The pattern becomes less apparent as the robot drives around, because it seems to be fixed relative to the camera.
I think it might be related to the way the depth image is upsampled in the registration process:

As can be seen above, 3/4 of the pixels are missing because the rgb image is 640x480, but I don't know why this would lead to the larger checkerboard pattern seen in rtabmapviz (is it just a rendering artifact?).

2. The other problem, which is more critical, is that I have not been able to get a single loop closure. The loop closure view in rtabmapviz stays blank even though I drive the robot back to the same places, and statistics shows both zero loop closures detected and zero rejected. I haven't been able to find the correct parameter to make it detect loop closures more easily. Could the problem be caused by the lacking depth data?

The robot is quite small and it also navigates in a fairly small area (it's supposed to stay within a 4x5m area) but the odometry is not great so I need some loop closures to get a more accurate map. I am not using visual odometry because there was too much delay and it would loose tracking too easily. Also, if I try to fuse the visual odometry with wheel odometry and IMU using robot_localization, the location jumps around a fair bit, even when the robot is standing still - which seems to indicate that the VO is underreporting its covariance?

I still run the odometry node, because it is needed to be able to get loop closures right? Is there a way to optimize it so it uses less CPU when I am not using it for odometry?

Thanks,
Daniel

[matlabbe]

Hi Daniel,

1. The realsense camera is a stereo camera in IR. On some surfaces, it can give various results. You can open rqt_image_view to see the raw depth image from the sensor. If you can see the checkboard pattern in the raw images, the problem is not from rtabmap. On the second image, it looks to me that the bottom of the image is the table, which cannot be detected by the depth sensor. The right black border is an effect of the registration of Ir to RGB

2. Loop closures can only been found on images with a lot of discriminative visual features. A white wall like this will give nothing. Revisiting locations should be also seen with the same view point. If the camera is seeing an empty space and has limited range, this would give empty depth images, which will affect loop closure performance (depth values are required). All these cases are similar to events that would cause lost odometry.

Visual odometry is not required for loop closure detection, rtabmap and rgbd_odometry nodes are independent. If you have already wheel odometry + IMU fusion, you can remove rgbd_odometry node to save a lot of computing resources.

To save computing power for rtabmap node, we can reduce rgb image size so that feature extraction is faster. To do so, similar to Tango ROS Streamer example, set:

<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap">
   ...
   <param name="Mem/ImagePreDecimation" value="2"/>  <!-- use 2 times smaller RGB image to extract features -->
   <param name="Mem/ImagePostDecimation" value="2"/> <!-- save in database 2 times smaller raw images -->
</node>
</node>

cheers,
Mathieu

[danmou]

Thanks, I will disable the visual odometry and add decimation to the rgb image, that should help with the processing.

The white wall was just to illustrate the checkerboard pattern, the area I have tried to map has a fair amount of visual features. I can try to generate a bag so you can see. The second picture in my previous post was the registered depth image viewing the same white wall as the first picture, and as can be seen the depth image has pretty much complete coverage on the wall (depending on your screen, you might need to zoom in to avoid aliasing), whereas the point cloud in rtabmapviz has large holes. Any idea why this is occurring? As you say, depth values are required for loop closure so I think this could be what's causing my loop closure problem.

Thanks,
Daniel

[danmou]

Hey again,

I have bagged the data from a short test where the robot drives in a circle, returning to the same place: link

I still get no loop closure. I suspect that it must be the aforementioned checkerboard problem, which i would assume will cause 3/4 of matches to be discarded because the depth is missing? The pattern is also visible when activating the depth image overlay in the loop closure panel in rtabmapviz, but not when looking at the depth image in rviz or rqt_image_view...

Any ideas?

Daniel

[matlabbe]

Hi,

The depth image has a lot of null lines:

 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0,
 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0,
 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 
192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 
192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 
192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 
192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0, 192, 127, 0, 0,
...

Like in your rqt_image_view picture above, the depth image is not full. All the holes are causing the checkboard aliasing.
Example just by resizing the window, the rendered depth is not rendered correctly, however this is just a visual gui bug.
Ok:

Window resized:


The best would be to have the depth image more dense (without all these null lines). I think thesse lines were added during registration, because the original depth image is a lot smaller than RGB image. If I remember, with realsense r200, you may use the original depth directly (this would fix the empty lines). Can you show a comparison of the original depth and the registered one?

cheers,
Mathieu

[danmou]

Hey,
Yes, the empty pixels in the depth image are added in the registration process. I could not use the depth directly because it is in the reference frame of the left IR camera, which is around 5cm from the rgb camera. However, I finally managed to solve the problem by inserting a node that resizes the depth image to 640x480 before the registration. I now get a much denser map and I am finally getting some loop closures  

Now I think I just need to tune the parameters. I had to decrease Rtabmap/LoopThr to 0.03 to get some loop closures. What exactly does this parameter represent? It doesn't seem to correspond with the values shown in the likelihood plot. And do you have any pointers for tuning the other parameters related to loop closure?

Thanks,
Daniel

[matlabbe]

Hi,

How did you interpolate the depth image? Be aware that on edge we may not want to interpolate depth values. I updated the Hand-held mapping tutorial  for RealSense R200 camera with another approach to get dense depth image at the RGB size.

Setting Rtabmap/LoopThr as low (<0.1) makes rtabmap ignores the loop closure hypotheses and just tried to match with the image with highest likelihood. So a lot of wrong hypotheses will be tested. Hopefully with the geometric test, a least Vis/MinInliers (default 20) are required to accept the loop. You may try with lower Vis/MinInliers too, though transformations accepted below 20 inliers would be less accurate.

cheers,
Mathieu

[danmou]

Hi Mathieu,

Wish I had known that! Well my method works, so I think I will just stick with that. I use nearest-neighbor interpolation (aka no interpolation) to avoid the issue with the edges (I modified this package to work with depth images and correctly update the camera_info).

I have done some more testing on the environment that the robot is intended to operate on, and I still feel that it is too difficult for loop closures to be accepted. The environment is rather sparse visually.

These are my current settings:

      <param name="Mem/ImagePreDecimation"       type="string" value="2"/>  <!-- use 2 times smaller RGB image to extract features -->
      <param name="Mem/ImagePostDecimation"      type="string" value="2"/> <!-- save in database 2 times smaller raw images -->

      <param name="Rtabmap/DetectionRate"        type="string" value="1"/>
      <param name="Rtabmap/TimeThr"              type="string" value="800"/>
      <param name="RGBD/LocalLoopDetectionSpace" type="string" value="false"/> <!-- Local loop closure detection (using estimated position) with locations in WM -->
      <param name="RGBD/LocalLoopDetectionTime"  type="string" value="true"/>  <!-- Local loop closure detection with locations in STM -->
      <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="false"/> <!-- Optimize graph from the latest node -->
      <param name="Reg/Strategy"                 type="string" value="0"/>     <!-- 0=Vis, 1=Icp, 2=VisIcp -->
      <param name="Vis/FeatureType"              type="string" value="0"/>     <!-- 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK -->
      <param name="Vis/EstimationType"           type="string" value="0"/>     <!-- Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry) -->
      <param name="Vis/Iterations"               type="string" value="100"/>
      <param name="Vis/MinInliers"               type="string" value="5"/>     <!-- 3D visual words minimum inliers to accept loop closure -->
      <param name="Vis/MaxDepth"                 type="string" value="4.0"/>   <!-- 3D visual words maximum depth 0=infinity -->
      <param name="Vis/MinDepth"                 type="string" value="0.3"/>   <!-- 3D visual words minimum depth -->
      <param name="Vis/MaxFeatures"              type="string" value="0"/>     <!-- 0 no limits -->
      <param name="Vis/InlierDistance"           type="string" value="0.1"/>   <!-- 3D visual words correspondence distance -->
      <param name="Vis/CorNNDR"                  type="string" value="0.7"/>   <!-- NNDR: nearest neighbor distance ratio. Used for features matching approach (default 0.6) -->
      <param name="SURF/HessianThreshold"        type="string" value="400"/>   <!-- Threshold for hessian keypoint detector used in SURF. Lower=more features but less repeatable -->
      <param name="RGBD/AngularUpdate"           type="string" value="0.02"/>  <!-- Update map only if the robot is moving -->
      <param name="RGBD/LinearUpdate"            type="string" value="0.02"/>  <!-- Update map only if the robot is moving -->
      <param name="Mem/BadSignaturesIgnored"     type="string" value="false"/> <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
      <param name="Mem/RehearsalSimilarity"      type="string" value="0.6"/>   <!-- Similarity threshold -->

      <param name="Grid/FromDepth"               type="string" value="true"/>  <!-- Create occupancy grid from depth image(s), otherwise it is created from laser scan. -->
      <param name="Grid/DepthDecimation"         type="string" value="1"/>     <!-- Decimation of the depth image before creating cloud. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value). -->
      <param name="Grid/DepthMin"                type="string" value="0.3"/>   <!-- Minimum cloud's depth from sensor. -->
      <param name="Grid/DepthMax"                type="string" value="4.0"/>   <!-- Maximum cloud's depth from sensor. 0=inf. -->
      <param name="Grid/CellSize"                type="string" value="0.05"/>  <!-- Resolution of the occupancy grid. -->
      <param name="Grid/MapFrameProjection"      type="string" value="false"/> <!-- Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead. -->
      <param name="Grid/NormalsSegmentation"     type="string" value="true"/>  <!-- Segment ground from obstacles using point normals, otherwise a fast passthrough is used. -->
      <param name="Grid/MaxObstacleHeight"       type="string" value="0.8"/>   <!-- Maximum obstacles height (0=disabled). -->
      <param name="Grid/MinGroundHeight"         type="string" value="0.0"/>   <!-- Minimum ground height (0=disabled). -->
      <param name="Grid/MaxGroundHeight"         type="string" value="0.60"/>  <!-- Maximum ground height (0=disabled). -->
      <param name="Grid/MaxGroundAngle"          type="string" value="25"/>    <!-- Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles. -->
      <param name="Grid/NormalK"                 type="string" value="10"/>    <!-- K neighbors to compute normals. -->
      <param name="Grid/ClusterRadius"           type="string" value="0.1"/>   <!-- Cluster maximum radius. -->
      <param name="Grid/MinClusterSize"          type="string" value="10"/>    <!-- Minimum cluster size to project the points. -->
      <param name="Grid/FlatObstacleDetected"    type="string" value="true"/>  <!-- Flat obstacles detected. -->
      <param name="Grid/3D"                      type="string" value="true"/>  <!-- A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false. -->
      <param name="GridGlobal/FullUpdate"        type="string" value="true"/>  <!-- When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not. -->
      <param name="GridGlobal/FootprintRadius"   type="string" value="0.1"/>   <!-- Footprint radius (m) used to clear all obstacles under the graph. -->

My database file: link

What can I do to improve it?

Daniel

[matlabbe]

Hi,

To test more images for loop closure:

<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>

To extract more features (lowering the threshold):

<param name="SURF/HessianThreshold"        type="string" value="100"/>

For debugging, I would not use decimation right now, to extract more features (highest resolution generates more features):

<param name="Mem/ImagePreDecimation"       type="string" value="1"/> 
<param name="Mem/ImagePostDecimation"      type="string" value="1"/> 

However, the major problem is that most visual features are out of range of the sensor (the accuracy of R200 is only good under ~3.5 meters).


Frame data for one frame:



It seems that TF is not correctly set between robot frame and camera frame (map is tilted):


cheers,
Mathieu