ZED2 rtabmap loop closure and twisted map

[acp]

Dear people.


I took the ZED2 just to play a bit with it 🙂

Just to summarized, the following was installed:

    ZED SDK V 3.2.2 for Ubunto 18.04
    RtabMap V 0.20.4 (from source)
    rtabmap_ros  (from source)
    Cuada V 11.0
    Octomap
    zed_wrapper
    Among others


 

In the figures shown below, the following can be noticed.

1.- The map is shifted a lot before loop closure.

2.-  When there is a loop closure the map reminds twisted and not aligned  with the floor.

3.-  The octomap map on the image is shifted

4.-   When the camera is in the same point of view and in already visited place,  it does not make a loop closure, the camera needs to move forward and backwards on the same direction till finally it gets a loop closure. In other words,  it takes time for the loop closure to happen.


The script I am using is shown as follows.




<arg name="ground_is_obstacle" default="false"/>
<arg name="align_with_ground"  default="false"/>
<group ns="/zed/">
    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
     <arg name="rtabmap_args"  value="--delete_db_on_start --Optimizer/GravitySigma 0.3 --Grid/3D true --Grid/RangeMax 7  --Grid/CellSize 0.3 --Grid/GroundIsObstacle true localization:=false" />

      <arg name="rgb_topic"               value="/zed/zed_node/rgb/image_rect_color" />
     
      <arg name="depth_topic"             value="/zed/zed_node/depth/depth_registered" />
      <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 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" />
     
    </include>
</group>















Any help I  appreciate it.

[matlabbe]

Hi,

It is difficult to see in the image if the loop closure is actually closed (it looks not). But yes, loop closure may not be instantaneous. Rtabmap/LoopThr can be lowered to try to with lower hypotheses. Can you share that database?

cheers,
Mathieu

[acp]

Hi Mathieu,

I have run the rosbag with two versions of rtabmap, namely;

rtabmap  V 0.0.20 under Ubuntu 16.04 and rtabmap

and

rtabmap V 0.20.4   under ubuntu 18,04.1

 
In version 0.20 the loop closure happens as soon the camera has visited a previous location and has the same point of view as you can see it with the red circle, also there are more loop closures as you can see it with the lines that connect the 2 paths.






In version 0.24 the loop closure happens  almost at the and of the trajectory as you can see it with the red circle, also there are less loop closures as you can see it with the lines that connect the 2 paths.





I do not have the same data base but I send you another one which is similar.

https://nextcloud.fit.vutbr.cz/s/gjYk5amdYWAmP5D https://nextcloud.fit.vutbr.cz/s/gjYk5amdYWAmP5D


I will play with the parameter Rtabmap/LoopThr


Just wondering if there is a parameter of a way to:


1.- Avoid the map gets to twisted before loop closure and also the map is not aligned with the ground.


2.- To avoid  a big gap just before loop closure happens

[matlabbe]

Hi,

One difference is the version of OpenCV libraries linked to. On 16.04, we have access to all OpenCV's features (even nonfree) because ROS provided its own OpenCV version. On 18.04, ROS relied on the system version of OpenCV, which has very limited features (not built with opencv_contrib or nonfree modules). With 16.04, GFTT/BRIEF is used by default, while on 18.04, GFTT/ORB is used.

In 0.20.4, a new parameter has been added to change the way quantization of binary features is done (to use less RAM and processing time). To use the older approach, set "Kp/ByteToFloat" to false. This may affect the matching.

To verify those hypotheses, I re-ran your database with different combinations with 0.20.4 (the 0.11 line is Rtabmap/LoopThr=0.11 by default):

1) GFTT/ORB

# left
$ rtabmap-reprocess --Kp/DetectorStrategy 8 --Kp/ByteToFloat true rtabmap.db output.db
#right
$ rtabmap-reprocess --Kp/DetectorStrategy 8 --Kp/ByteToFloat false rtabmap.db output.db

2) GFTT/BRIEF

# left
$ rtabmap-reprocess --Kp/DetectorStrategy 6 --Kp/ByteToFloat true rtabmap.db output.db
# right
$ rtabmap-reprocess --Kp/DetectorStrategy 6 --Kp/ByteToFloat false rtabmap.db output.db

3) SURF (baseline for loop closure detection)

$ rtabmap-reprocess --Kp/DetectorStrategy 0 rtabmap.db output.db

6) GFTT/ORB with some tuning

$ rtabmap-reprocess --Mem/STMSize 30  --Rtabmap/LoopThr 0.06 --Kp/DetectorStrategy 8 --Kp/ByteToFloat false rtabmap.db output.db

In conclusion, for this kind of environment, set Kp/ByteToFloat to false to get better loop closure hypotheses with binary descriptors. Rtabmap/LoopThr can be lowered to test more hypotheses. Mem/STMSize can be increased to avoid high loop closure hypotheses on just seen locations.

For your questions, you have set Optimizer/GravitySigma to 0.3, but there is no IMU added to map, so the map won't be optimized with gravity and won't be aligned with ground (unless input odometry is VIO, which seems not the case here). Verify that rtabmap node is received IMU data. To have less large gaps, as I said previusly, it is odometry drift. As you are using ZED's odometry, I cannot help more for that. Note also that your odometry covariance is set to Identity, this is quite bad if you want good map optimizations and to reject bad loop closures. If zed's odometry doesn't give good covariance, you can fix it on rtabmap node with odom_tf_angular_variance=0.001 and odom_tf_linear_variance=0.001.

cheers,
Mathieu

[acp]

Dear Mathieu
As you have suggested. I have set Kp/ByteToFloat to false to get better loop closure hypotheses with binary descriptors and also Rtabmap/LoopThr to 0.06 to test more hypotheses.

Since ZED2 has an imu included /zed/zed_node/imu/data, I have feed the imu data to rtabmap.

As you can see in the following .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 --Grid/3D true --Grid/RangeMax 7  --Grid/CellSize 0.3 --Grid/GroundIsObstacle true localization:=false  --Rtabmap/LoopThr 0.06  --Kp/ByteToFloat false" />



      <arg name = "rgb_topic"                value = "/zed/zed_node/rgb/image_rect_color" />
      <arg name = "depth_topic"              value = "/zed/zed_node/depth/depth_registered" />
      <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 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 = "/zed/zed_node/imu/data"/> 
      <arg name = "odom_tf_angular_variance"      value = "0.001"/>
      <arg name = "odom_tf_linear_variance"       value = "0.001"/>
     
    </include>
</group>

Also as you can see in the following figure, the gap before  the  loop closure is smaller. But still there is a big gap
 



After loop closure the map get much better and also is aligned with the floor but it  is rotated with respect to the grid.

Based on the previous I do have the following questions

1.- How can avoid the rotation of the map with the grid?

2.-Where do you see the odometry covariance  set to Identity?

3.- Why the  odometry covariance  set to Identity  is quite bad if I want good map optimizations and to reject bad loop closures?

4.-I am using ZED2's odometry as you have mentioned. However what do you suggest to get better odometry?

5 What does the Kp/DetectorStrategy does?
 
6.- Since ZED2 has barometer, magnetometer, and imu, how good would be to fuse them with kalman filters to get better estimation?

7.-How do you get the plots of the parameters?


Thank you very Much

[matlabbe]

1. The rotation can come from the optimization with gravity constraints. I don't know if you built with both GTSAM and g2o dependencies, but GTSAM seems to make the map rotate more with gravity constraints are added (I didn't see how to avoid that). Try comparing Optimizer/Strategy=1 and Optimizer/Strategy=2.

2. In databaseViewer->Constraint view, by moving the Neighbor slider, we can see odometry covariance.

3. For example, if you have a link in the graph with covariance of 1 meter and that the global odometry drift is 1 meter, it means that during graph optimization, the optimier can put the whole 1 meter of error in that single link instead of propagating equally the 1 meter error to all nodes. For the loop closure rejection approach, we look for links that are too much deformed after optimization. A too large deformation in comparison to covariance would mean we have a wrong loop closure. Setting Identity covariance will make accepting all loop closures because the graph can be deformed a lot under such large covariance.

4. rtabmap provides also a visual odometry, but there are also many other packages doing VIO out there that could replace zed's odometry too and feed the resulting odometry to rtabmap.

5. This is the type of feature used in the BOW vocabulary

6. If the resulting IMU data from zed2 has a quaternion, they may already do the estimation. Otherwise, for IMUs, you can use complementary filter or madgwick  filter ros packages.

7. In databaseViewer->Statistics panel, next to each statistic, there is a button to plot the values.

cheers,
Mathieu

[acp]

Hi Mathieu

Thank you for your answer

I have tried as you suggested with:

 Optimizer/Strategy=1 (g2o) and Optimizer/Strategy=2 (GTSAM) as you can see in the following figures:

This figure correspond to Optimizer/Strategy=1. I have run it many times and the rotation with respect to the grid is much better than with Optimizer/Strategy=2. However, as you can see the right wall is twisted and also the general map  has a small rotation.



This figure correspond to Optimizer/Strategy=2. I have run it many times and in general looks very good, but there are times where is rotated. The Figure shows a map where the rotation with respect to the grid is practically none.  

1. The rotation can come from the optimization with gravity constraints. I don't know if you built with both GTSAM and g2o dependencies, but GTSAM seems to make the map rotate more with gravity constraints are added (I didn't see how to avoid that). Try comparing Optimizer/Strategy=1 and Optimizer/Strategy=2.

  * Based on the previous figures, how can avoid the bending of the wall using Optimizer/Strategy=1

  * How can avoid the map rotates with respect to the grid when using Optimizer/Strategy=2

2. In databaseViewer->Constraint view, by moving the Neighbor slider, we can see odometry covariance.

  I could not find the databaseViewer->Constraint.

    I have search in Window/Show view  also in Window/Preferences

4. rtabmap provides also a visual odometry, but there are also many other packages doing VIO out there that could replace zed's odometry too and feed the resulting odometry to rtabmap.

  Which VIO package you recommend, knowing I have ZED2. And, is it possible to have an example or link

5. This is the type of feature used in the BOW vocabulary

   OK thank you, but  what exactly this parameter does ?

6. If the resulting IMU data from zed2 has a quaternion, they may already do the estimation. Otherwise, for IMUs, you can use complementary filter or madgwick  filter ros packages.

     * Do you know how to find out if ZED2 has a quaternion and may already do the estimation and how to use this estimation?

   * The madgwick package fuses imu/data_raw with imu/mag and publish imu/data. Thus, ZED2'barometer delivers /atm_press topic whic is the current atmospheric pressure in hPa (hectoPascals).

   * I was thinking to use robot_localization package. What do you think? and how shall be done  

7. In databaseViewer->Statistics panel, next to each statistic, there is a button to plot the values.

  * I see, I have found it, thank you


I am looking forward to your replay.