rtabmap does not find loop closures

[acp]

Dear People,

I do have a ZED camera and a Jetson TX2 mounted on a drone. I have taken the drone with my hands to map an outdoor place by means of the rtabmap.launch node.



 I do have some problems when I map the outdoor place. I have taken two pictures of the RVIZ pointcloud.





In the first picture we can see that when the drone makes one loop and then when it returns to the starting point it finds loop closures and the 3D map looks good.

In the second picture we can see that when the drone follow 2 diagonals, in the upper part of the picture inside the red eclipse, the rtabmap did not find any loop closures even though the drone passes  the same path twice.








Here is a .db file

 http://www.fit.vutbr.cz/~plascencia/fityard3.db 


this is the rtabmap.launch


<group ns="/zed/">
    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
       <arg name="rtabmap_args"        value="--delete_db_on_start --Optimizer/GravitySigma 0.3" />
      <arg name="rgb_topic"               value="/zed/zed_node/rgb/image_rect_color" />
      <arg name="depth_topic"             value="/zed/depth/depth_filtered" />
      <arg name="camera_info_topic"       value="/zed/zed_node/rgb/camera_info" />
      <arg name="depth_camera_info_topic" value="/zed/zed_node/depth/camera_info" />
      <arg name="frame_id"                      value="$(arg camera_frame)" />
      <arg name="odom_frame_id"                 value="odom" />
      <arg name="approx_sync"                   value="false" />
      <arg name="visual_odometry"               value="false" />
      <arg name="rtabmapviz"                    value="false" />
      <arg name="rviz"                          value="false" />
      <arg name="odom_topic"                    value="/zed/zed_node/odom" />

      <arg name="imu_topic"                     value="/mavros/imu/data"/> 
      <(param) name = "Optimizer/GravitySigma "     type="string"  value="0.3"/>
      <(param) name = "publish_tf"                  type="string"  value="false"/>

     
     

         
         <(param) name="Grid/DepthDecimation"              type="string" value="4"/>
         <(param) name="Grid/FlatObstacleDetected"         type="string" value="true"/>
         <(param) name="Kp/MaxDepth"                       type="string" value="0"/>
         <(param) name="Kp/DetectorStrategy"               type="string" value="6"/>  
         <(param) name="Vis/EstimationType"                type="string" value="1"/>  
         <(param) name="Vis/MaxDepth"                      type="string" value="0"/>
         <(param) name="RGBD/CreateOccupancyGrid"          type="string" value="true"/-->
   
         
         

<(param) name="Grid/NormalsSegmentation" type="bool" value="false"/>
<(param) name =" Grid/MaxGroundHeight" type="double" value="0.05"/>
<(param) name="Optimizer/Strategy"  type="string" value="1"/>
<(param) name="RGBD/OptimizeStrategy" type="string" value="1"/>

    </include>
</group>

I am just wondering why the rtabmap.launch did not find any loop closure even though the drone passed over the same place twice. Or, what depends for the algorithm to find a loop closure so the map can be corrected and look good.

[matlabbe]

Hi,

Based on the shared database, the drone traversed the same area but in different direction. As the point of view is different, loop closures cannot be detected. RTAB-Map will detect loop closures if the camera is close to an already visited location and if the camera point of view is similar (looking at the same direction).

cheers,
Mathieu

[acp]

matlabbe wrote

Based on the shared database, the drone traversed the same area but in different direction. As the point of view is different, loop closures cannot be detected. RTAB-Map  detects loop closures if the camera is close to an already visited location and if the camera point of view is similar (looking at the same direction).

Hi Mathieu, thank you for your answer. I have noticed what you have commented, that when RTAB-Map  detects a loop closure if the camera is close to an already visited location and  if the camera point of view is similar (looking at the same direction).  However, I have few questions, if you do not mind I would appreciate your replay  


1,- What type of local or global optimization to use for a better loop closure detection. I mean I am a bit confuse about them :)


TORO, GTSAM, SURF or g2o


So far I have $rosparam get   /zed/rtabmap/g2o/Optimizer   '0'

Which means: Optimization approach used: 0=TORO, 1=g2o and 2=GTSAM.

How about using the following parameters:

<"RGBD/OptimizeMaxError"     = "0,0"/>
<"g2o/Optimizer"  = "1"/>
<"RGBD/LoopClosureReextractFeatures" = "true"/>

2.- How close the camera field of view must be to a visited location.



3.-What depends to make a good map?


4.- To make a good map is it recommendable  to pass twice in the same direction and close to an existing map?

[matlabbe]

Hi,

1) Look at Section 3.5 of this paper:

RTAB-Map integrates three graph optimization approaches:  TORO [Grisetti et al., 2010], g2o [Kummerle et al.,2011] and GTSAM [Dellaert, 2012].  g2o and GTSAM converge faster than TORO, but are less robust to multi-session mapping when multiple independent graphes have to be merged together.  TORO is also lesss ensitive to poorly estimated odometry covariance.  However, for single map, based on empirical data, g2o and GTSAM optimization quality is better than TORO, particularly for 6DoF maps.  GTSAM is slightly more robust to multi-session than g2o, and thus is the strategy now used by default in RTAB-Map contrarily to our previous works using TORO.

RGBD/OptimizeMaxError is also explained in that section. Note that you can do "rtabmap --params"  to have a description of all parameters. g2o/Optimizer is the optimization approach used in g2o, which is [0=Levenberg 1=GaussNewton]. RGBD/LoopClosureReextractFeatures to true can be used to re-do old approach on loop closures, where we re-extracted features. In recent versions, we don't need it as we have already extracted all features we needed to compute loop closure transformation.

2) It depends on the sensor used, but as long as the camera can see the same features, it could theoretically close the loop.

3) Low odomety drift, frequent loop closure detection.

4) Not necessary the whole trajectory again, but at least if you know that you are passing by a nearby already visited location, you can go there, close the loop, then return back exploring.

[acp]

Hi Mathieu

Thank you for your answer.

matlabbe wrote

1) Look at Section 3.5 of this paper:

RTAB-Map integrates three graph optimization approaches:  TORO [Grisetti et al., 2010], g2o [Kummerle et al.,2011] and GTSAM [Dellaert, 2012].  g2o and GTSAM converge faster than TORO, but are less robust to multi-session mapping when multiple independent graphes have to be merged together.  TORO is also lesss ensitive to poorly estimated odometry covariance.  However, for single map, based on empirical data, g2o and GTSAM optimization quality is better than TORO, particularly for 6DoF maps.  GTSAM is slightly more robust to multi-session than g2o, and thus is the strategy now used by default in RTAB-Map contrarily to our previous works using TORO.


RGBD/OptimizeMaxError is also explained in that section. Note that you can do "rtabmap --params"  to have a description of all parameters. g2o/Optimizer is the optimization approach used in g2o, which is [0=Levenberg 1=GaussNewton]. RGBD/LoopClosureReextractFeatures to true can be used to re-do old approach on loop closures, where we re-extracted features. In recent versions, we don't need it as we have already extracted all features we needed to compute loop closure transformation.

I have read the section  3.5  and according what you have explained and the section 3.5 of the paper, the default optimiser used in rtabmap_ros is GTSAM together with   RGBD/OptimizeMaxError, right?

Based  on the previous, I have few questions

1.- I have read in this link  that to set GTSAM the follwoing parameters must be set RGBD/OptimizeStrategy = 2,   RGBD/OptimizeMaxError = 0,  RGBD/OptimizeRobust = true. Since GTSAM is now the default optimiser in rtabmap_ros, I just wanted to check that those parameters have that value.


When I run $rosparam list
I get:

/zed/rtabmap/Optimizer/Strategy
/zed/rtabmap/RGBD/OptimizeMaxError
/zed/rtabmap/Optimizer/Robust



And

When I  run:

 $ rosparam get /zed/rtabmap/Optimizer/Strategy
'1'

$ rosparam get /zed/rtabmap/RGBD/OptimizeMaxError
'3.0'

$ rosparam get /zed/rtabmap/Optimizer/Robust
'false'



 This mean that I do have g2o optimization as a default with RGBD/OptimizeMaxError to reject wrong loop closures, right?

So if I want to try GTSAM

I need to set the following parameters, right?

$ rosparam set /zed/rtabmap/Optimizer/Strategy 2

$ rosparam set  /zed/rtabmap/RGBD/OptimizeMaxError 0

$ rosparam set /zed/rtabmap/Optimizer/Robust true

$ rosparam set /zed/rtabmap/Optimizer/GravitySigma 0.3


2.-  Do you recommend that I shall try to set GTSAM or just leave the g20 as a default.  Or, try to run both and compare them.

3.- Well I can see in rtabmapviz/prferences/RGB-D-SLAM/Graph-Optimization tab  TORO, g2o and GTSAM, however  GTSAM is grayout,  maybe the version of rtabmap_ros I have does not support GTSAM.

4,-  Just to mentioned for some one who may be helpful that to check individual info parameters:
    $rosrun rtabmap_ros rtabmap --params | grep Optimizer/Robust








Thank you and looking forward to your replay.

[matlabbe]

Hi,

To use GTSAM, only RGBD/OptimizeStrategy = 2 is needed. RGBD/OptimizeStrategy=1 means g2o, and RGBD/OptimizeStrategy=0 means TORO. If rtabmap is not built with GTSAM support, RGBD/OptimizeStrategy will be set to 1 by default and the option will be grayed out in the GUI. If you set it to 2 with rosparam, you would have a warning in the terminal saying that you cannot use GTSAM (rtabmap not built with it), so g2o is used instead.

Parameters RGBD/OptimizeMaxError and Optimizer/Robust are mutually exclusive for loop closure rejection, and they can work with g2o and gtsam. Only Optimizer/Robust won't work with TORO.

cheers,
Mathieu

[acp]

Hi Mathiu.


Thank you for your answer.

 

matlabbe wrote

To use GTSAM, only RGBD/OptimizeStrategy = 2 is needed. RGBD/OptimizeStrategy=1 means g2o, and RGBD/OptimizeStrategy=0 means TORO. If rtabmap is not built with GTSAM support, RGBD/OptimizeStrategy will be set to 1 by default and the option will be grayed out in the GUI. If you set it to 2 with rosparam, you would have a warning in the terminal saying that you cannot use GTSAM (rtabmap not built with it), so g2o is used instead.

Parameters RGBD/OptimizeMaxError and Optimizer/Robust are mutually exclusive for loop closure rejection, and they can work with g2o and gtsam. Only Optimizer/Robust won't work with TORO.

When I write $rosparam list I do not see RGBD/OptimizeStrategy however I can see Optimizer/Strategy.

Then when I write $rosrun rtabmap_ros rtabmap --params | grep Optimizer/Strategy I get: [Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.]


When I set $rosparam set /zed/rtabmap/Optimizer/Strategy 2 I do not get any error then when I write:

rosparam get /zed/rtabmap/Optimizer/Strategy
'2'

Then in the GUI the option GTSAM is  grayed out.

I am confused now :)


I am looking forward to your replay :)

[matlabbe]

Hi,

I was using the parameters you've shown in previous posts. In recent versions, the parameter is now called Optimizer/Strategy. If you set RGBD/OptimizeStrategy, it will still be recognized for backward compatibility (copied to Optimizer/Strategy under the hood).

To enable GTSAM, you should rebuild rtabmap with GTSAM support, then the option won't be grayed out anymore.

You can see your current build dependencies with:

$ rtabmap --version
...
With GTSAM:               true
...

If with GTSAM is false, install GTSAM and rebuild rtabmap. make sure in cmake status GTSAM has been found before compiling.

$ git clone --branch 4.0.0-alpha2 https://github.com/borglab/gtsam.git gtsam-4.0.0-alpha2
$ cd gtsam-4.0.0-alpha2
$ mkdir build
$ cd build
$ cmake -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_UNSTABLE=OFF ..
$ make -j4
$ sudo make install

Then

$ cd rtabmap/build
$ cmake ..
-- --------------------------------------------
-- Info :
--   Version : 0.19.7
...
--  Solvers:
--   With TORO                 = YES (License: Creative Commons [Attribution-NonCommercial-ShareAlike])
--  *With g2o                  = YES (License: BSD)
--  *With GTSAM                = YES (License: BSD)
--  *With Ceres                = YES (License: BSD)
--   With VERTIGO              = YES (License: GPLv3)
--   With cvsba                = NO (cvsba not found)
--  *With libpointmatcher      = YES (License: BSD)
...

[acp]

Hi Mathiu.


Thank you for your answer.

What do you mean when you say: If you set RGBD/OptimizeStrategy, it will still be recognized for backward compatibility (copied to Optimizer/Strategy under the hood). sorry for not understanding :)

It sounds good to rebuild rtabmap with GTSAM support, however I have been using the binaries rtabmap_ros, e,g, I have installed rtabmap_ros over $sudo apt-get install 


well, so far I would like to keep the rtabmap_ros as it is:


When I run:
$rtabmap --version
RTAB-Map:               0.19.3
OpenCV:              3.3.1-dev
With TORO:                true
With g2o:                 true
With GTSAM:              false
With Vertigo:             true
With ZED:                false


So far I have been using:

$ rosparam get /zed/rtabmap/Optimizer/Strategy
'1'

$ rosparam get /zed/rtabmap/RGBD/OptimizeMaxError
'3.0'

$ rosparam get /zed/rtabmap/Optimizer/Robust
'false'

$ rosparam get /zed/rtabmap/Optimizer/GravitySigma
'0.3'


I have a question:

When I try to make a 3D outdoor map, sometimes I get a good map and sometimes I get the map you can see in the below pictures. The main problem is that when the odom catches or finds a odom-path, it gets deformed as you can see in the two pictures with red circles. Then the final 3D map is totally deformed.

Do you have a clue what can be wrong or do i need to set some  parameter?

[matlabbe]

Hi,

When using Optimizer/GravitySigma parameter, we should make sure that the TF between the IMU and the base frame used by rtabmap is accurate, otherwise it could optimize badly the map. For example, open the map very deformed that you shown in rtabmap standalone, go in Preference->Optimizer and change gravity sigma prameter to 0, close disalog, and do Edit->Download Graph (Optimized). It will re-optimized to map without gravity constraints. IF the map is better, it means TF between IMU and base link is not accurate. It is possible to set Optimizer/GravitySigma>=1 when this TF is not accurate, so that the map gets less deformed.

If you set old parameter name RGBD/OptimizeStrategy=1, rtabmap will automatically set Optimizer/Strategy=1 (the new parameter name) for convenience.

cheers,
Mathieu

[acp]

matlabbe wrote

Hi,

When using Optimizer/GravitySigma parameter, we should make sure that the TF between the IMU and the base frame used by rtabmap is accurate, otherwise it could optimize badly the map. For example, open the map very deformed that you shown in rtabmap standalone, go in Preference->Optimizer and change gravity sigma prameter to 0, close disalog, and do Edit->Download Graph (Optimized). It will re-optimized to map without gravity constraints. IF the map is better, it means TF between IMU and base link is not accurate. It is possible to set Optimizer/GravitySigma>=1 when this TF is not accurate, so that the map gets less deformed.

If you set old parameter name RGBD/OptimizeStrategy=1, rtabmap will automatically set Optimizer/Strategy=1 (the new parameter name) for convenience.

cheers,
Mathieu

I have tried the following:
Preference->Optimizer and change gravity sigma prameter to 0, close disalog, and do Edit->Download Graph (Optimized)

But since the maps are really noisy it is difficult to see the difference.


I have tried two modes:
///////////////////////////////////////////////////////////////////////////////////////
1.- In this one I try g2o with robust.

$ rosparam get /zed/rtabmap/Optimizer/Strategy
'1'

$ rosparam get /zed/rtabmap/RGBD/OptimizeMaxError
'0'

$ rosparam get /zed/rtabmap/Optimizer/Robust
'true'

$ rosparam get /zed/rtabmap/Optimizer/GravitySigma
'0'

VIDEO
https://www.dropbox.com/s/xt9kyuccjv1y03z/tbsd11.mp4?dl=0
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


/////////////////////////////////////////////////////////////////////////////////////////////////////////
2.-This one is as you have suggested, with gravitySigma=1 and g2o.

$ rosparam get /zed/rtabmap/Optimizer/Strategy
'1'

$ rosparam get /zed/rtabmap/RGBD/OptimizeMaxError
'3'

$ rosparam get /zed/rtabmap/Optimizer/Robust
'false'

$ rosparam get /zed/rtabmap/Optimizer/GravitySigma
'1'

VIDEO
https://www.dropbox.com/s/owmll2al88l0js4/tbsd14.mp4?dl=0
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


1.- As you can see in both videos the maps get distorted and after passing the drone again over the same path,  the map some how gets better.

2.- With the combination g2o,  optimize/Robust = true and  /Optimizer/GravitySigma = 0 somehow I get better results, but sometimes the map does not close even though a loop closure has been detected.

3.- I have calibrated IMU and the camera twice and set the tf in the zed.urdf  file. Since is difficult to test the imu-camera calibration is accurate, therefore  I have assumed that the calibration is not good.  Therefore as you have suggested I have done the step 2. As you can see in the video the map gets distorted.

4.- What can I do to get a good map?


I appreciate your help and looking forward to your replay.

[matlabbe]

Hi,

With Optimizer/Robust, a loop closure can be detected but the graph doesn't get optimized as some loop closure links may be wrongly ignored by Vertigo. This option is difficult to tune because we should carefully set covariance of odometry links and loop closure links.

You may try also this config: Optimizer/Strategy=1, Optimizer/GravitySigma=0, Optimizer/Robust=false, RGBD/OptimizeMaxError=3


To make sure that Optimizer/GravitySigma is working properly, you can show the gravity links in rtabmapviz (Preferences->3D Rendering, scroll down, enable "Show IMU filtered gravity."). If the gravity lines are not always vertical going down, then enabling Optimizer/GravitySigma will make it worst. This could help to see if there is a strong TF problem between camera and iMU.

cheers,
Mathieu

[acp]

Hi Mathieu


I have tried to do what you have suggested, however I can not see Show IMU filtered gravity

Under GeneralSettings/3DRendering/

as you can see in the picture.



Just wondering why is not there?

Also, how can I check that the TF between camera and iMU is good enough?

Looking forward to your replay.

[matlabbe]

To get IMU filtered gravity option, you should build latest version of rtabmap and rtabmap_ros. (make sure to uninstall rtabmap ros binaries if you do so)

[Masoumeh]

Dear Mattieu,

I have followed the installation procedure , but it is not showing "yes",28.png

what should i do in this step?

Thanks

[Masoumeh]

it seems that although i have installed libpointmatcher, it shows false, in rtabmap.

How can i activate it?
what about gtsam?

Thanks

[matlabbe]

Hi,

if you built with libpointmatcher (With libpointmatcher = YES), and it doesn't show up in rtabmap app About dialog, it is because you are not starting the right rtabmap executable. Do "which rtabmap".

In your log, you set cmake options WITH_G2O=OFF and WITH_GTSAM=OFF, so they won't be included in the build.