stereo_odometry and changing lighting conditions - ZED2i/ZEDx on an UAV for outdoors environments

[aaraujo11]

I'm trying to config rtabmap to use with the ZED2i camera (with a Jetson Xavier NX 16G), on an UAV for outdoors forestry environments, in terms of camera setup its installed in the front with a 35º pitch down, i intend to flight in low altitudes (2 - 4m). I have available multiple inputs of IMU (100 and 200Hz), and GPS RTK (5Hz and "Float" mainly all the time under the vegetation).

To achieve the best results for SLAM, do you have suggestion in terms of parameterization and the best approach for the following queires:

• For odom should i use rgbd_odometry or stereo_odometry?
• Camera frequency and resolution?
• External IMU (100HZ) of ZED2i internal IMU (200Hz)
• Main params to tune in rtabmap_slam and rgbd_odometry/stereo_odometry?

I'm been checking some examples for outdoors purposes but mainly for ground mobile robots, where the results are not satisfying, since there is always huge shift mainly in Z axis. I check also rtabmap_drone_example, @matlabbe can you please provide some tips how to best tune rtabmap, since you had already work with UAV (even if indoors).

[aaraujo11]

Just an update the tuning process, using as start base the demo_stereo_outdoor.launch, it was possible to achieve interesting results using stereo odometry, though it looses track of odometry once it crosses areas with different light conditions, as you can see in the following images:

It seems to be some contrast threshold where the features get clustered in the light area, and once the following frames go to darker areas, it looses it track.

Modified stereo_odometry parameters:

<param name="Odom/Strategy"        value="1"/>
<param name="OdomF2M/BundleAdjustment"        value="0"/>
<param name="Vis/MinInliers"        value="10"/>
<param name="Stereo/MaxDisparity"        value="200"/>
<param name="GFTT/MinDistance"        value="10"/>
<param name="GFTT/QualityLevel"        value="0.00001"/>

[aaraujo11]

Stereo_odometry Issue solved by disabling the auto_exposure of the camera, however it appear a new issue related with the rtabmap_slam Z axis drifting, created a new issue.

[matlabbe]

I guess the closest experience of your application that I tried is this one https://www.youtube.com/watch?v=G-5jesjNfLc (this contains some strong exposure time variations)

For your case above, I would tune Odom/KeyFrameThr to something higher because it takes too long to generate a new key frame. Accordingly to this paper (table 2), with F2F (Odom/Strategy=1), the threshold is set to 0.6.

For other parameters, I would need to see the data to make suggestions. For general recommendation, use stereo_odometry with resolution around 480p. For 720p cameras, you can increase GFTT/MinDistance. For 1080p+, I'll suggest to increase Odom/imageDecimation if your camera driver cannot publish lower resolution.

For IMU, I would take the one from the drone.

[aaraujo11]

Thank you @matlabbe for the feedback, I tried what you suggested and I'm having similar results as before. Disabling the auto-exposure, auto-whilebalance, and reduce the exposure to very low values, the stereo_odometry never fails and can identify more features, however:

1. It seems not ideal for loop closure, it barely happen during the experience as you can verify here.

2. Although the fine tuning of the static TF of the position of the camera (#929), it really improve the drift in Z, as you can see in the following PS:

But still, in the return you can verify it starts to drift in Z axis.

3. As far i understand without loop closure, it will be hard for the graph optimize the final map correct? Is it possible to access to some metric of the number of loop closures or quality of those, to use as a tuning reference?

4. Do you think even if the UAV crosses the same area, but in different altitudes, it may cause the lack of loop closures?

5. Since i will always land in the same position as the takeoff, I'm thinking to use a April tag, do you think it will help?

6. You suggest VGA resolution, but that won't affect the SLAM quality, since we have access to a decent frame rate at 20Hz HD720?

7. Any suggestion to adjust more parameters to improve the results? Can you please provide the parameters you use in that outdoors experiment, to try it?

Final remark: Im using the the embedded IMU of the ZED2i at 200Hz.

[matlabbe]

Hi,

1-2-3-4: The default ORB features may not be appropriate for loop closure detection in that environment. Also with the change of altitude, it can change slightly the look of some features. Another issue is the illumination variations. I would go for float descriptors like SIFT or SURF, or even better, SuperPoint. While looking parameters to answer your question 7, I saw that in my video I used SuperPoint for loop closure detection (SuperPoint really helps when lighting is changing). You may keep the current settings for odometry though.

5: Yes, but if you exactly land with same orientation than during the take off, loop closure should be found. However, if it was cloudy (shade) during take off and very sunny on landing, visual loop closure detection may have difficulty, so an apriltag could be used to make sure at least you get the loop closure at the end.

6: 20 Hz is not too bad indeed. For loop closure detection, HD images make binary features not very discriminative, maybe why loop closure detection doesn't work. With float descriptors as suggested above, they can work with 720p.

7: Look from 40:00 of the video

[aaraujo11]

Hello @matlabbe,

We manage to test with SURF and SIFT, with SIFT we had a better results, thought the stereo odometry still crash, in some light conditions changes, we want to test the SuperPoint:

1. Do you have any available docker with SuperPoint working?
2. Somehow in rtabmapViz, we stop to have access to the "accumulate" map and the loopclosures information, is because we are using docker or some option that is disable?
3. Is it possible to reset the odometry to the last know one, after a certain timeout?
4. Regarding the loop closure by GNSS, it will only be used in case of a FIX data or a FLOAT will be consider as well? Do we need to active any option to be consider the GNSS RTK information?

Thank you!

[matlabbe]

1. I just created one, see https://github.com/introlab/rtabmap_ros/blob/master/docker/noetic/superpoint/Dockerfile
2. RAM full, too many point clouds to render... for very large maps, I recommend to downsample the point clouds in Preferences->3D Rendering->Decimation (default 4, try with 8). Check also if odometry is still going on and if there are warnings/errors on terminal. Docker should not be a problem.
3. See Odom/ResetCountdown parameter (rtabmap --params | grep ResetCountdown)
4. For gps, there is currently only /gps/fix topic that rtabmap supports. By default (Rtabmap/LoopGPS parameter) it will be used to limit loop closure detection search (inside RGBD/LocalRadius, see this documentation), but won't be used by default for graph optimization. To fuse gps into SLAM estimation, you will need to set Optimizer/PriorsIgnored to false.

cheers, Mathieu

[aaraujo11]

Hello @matlabbe,

1. Amazing, i really appreciated it :) !

2. Solved, no issues running it in the docker. Though, if i try to set the Vis/FeatureType to 11 (superpoint) in stereo odom (not in mapping, it run smoothly) i stop to be able to see the mapping, but no errors are displayed.

3. Regarding the Odom/ResetCountdown it works, but ofc it also reset the mapping component. Is it possible somehow, reset the odom and keep the map? For instance, to the last know position even if not in the correct position?

4. Yes we test it, and read the documentation as you suggest, but since we are using stereo odom and not RGBD, that RGBD/LocalRadius it will not be used correct, means we can't use in StereoOdometry (SO)?

5. In general its visible that the SuperPoint performance is really better in this type of scenario (in the mapping component), with a several loop closures detected, however, the SO component insist to fail in certain situations. I notice this method, have a tendency to search for features on higher contrasts areas of the image, for instance the sky and top silhouette of the trees, and that cause SO to fail. I improve it by distributing the inliers all over the frame Vis/MinInliersDistribution to 0.0001. I try other several detector strategy, and other sub parameters, but still something is missing in the SO, it keeps failing. The best configurations until the moment are:

rtabmap_args:
--delete_db_on_start
--SuperPoint/ModelPath /workspace/superpoint_v1.pt
--PyMatcher/Path /workspace/SuperGluePretrainedNetwork/rtabmap_superglue.py
--Kp/DetectorStrategy 11
--Vis/CorNNType 6
--Reg/RepeatOnce false
--Vis/CorGuessWinSize 0 
--Rtabmap/WithGPS true 
--RGBD/LocalRadius 5 
--Optimizer/PriorsIgnored false

odom_args:
--Vis/CorNNType 1
--Reg/RepeatOnce true
--Vis/CorGuessWinSize 40
--Stereo/MaxDisparity 200
--Vis/FeatureType 8  # 11 not working
--Vis/MinInliersDistribution 0.0001
--Vis/MaxFeatures 1000
--Vis/MinInliers 5
--Odom/ResetCountdown 0
--Odom/ParticleSize 500

Im running out of ideas, do you think you could take a quick look to the dataset pls, since you have more experience in several case studies. I prepared a ready to play ros package with everything you need to replicate what Im testing. SO fails normally after 480 sec in the rosbag.

6. As a work around to make the odom source more robust, Im thinking to add a extra layer with robot_localization with SO from rtabamp + GPS RTK (+ RGBDodom rtabmap if needed), and the output feed the rtabmapping instead of the SO directly. Do you think it will help to make it more robust, have you ever try it?

Thank you!

[matlabbe]

The command I used (pytorch was not correctly setup on that computer, so I removed python related stuff (superglue)):

roslaunch rtabmap_launch rtabmap.launch \
   rtabmap_viz:=true \
   rtabmap_args:="--delete_db_on_start \
      --SuperPoint/ModelPath ~/superpoint_v1.pt \
      --Kp/DetectorStrategy 11 \
      --Vis/CorNNDR 0.6 \
      --Rtabmap/WithGPS true \
      --RGBD/LocalRadius 5 \
      --Optimizer/PriorsIgnored false \
      --Mem/ImagePostDecimation 2 \
      --Mem/ImagePreDecimation 2 \
      --RGBD/CreateOccupancyGrid false" \
      --RGBD/OptimizeMaxError 5"
      --RGBD/ProximityPathFilteringRadius 10 \
   odom_args:="--Vis/CorNNDR 0.8 \
      --Stereo/MaxDisparity 200 \
      --Vis/MaxFeatures 1000 \
      --Odom/ImageDecimation 2" \
   left_image_topic:=/zedx_u/zed_node/left/image_rect_color \
   right_image_topic:=/zedx_u/zed_node/right/image_rect_color \
   left_camera_info_topic:=/zedx_u/zed_node/left/camera_info \
   right_camera_info_topic:=/zedx_u/zed_node/right/camera_info stereo:=true \
   wait_imu_to_init:=true \
   imu_topic:=/zedx_u/zed_node/imu/data \
   frame_id:=base_link

I had to increase RGBD/ProximityPathFilteringRadius (default 1 meters) to accept loop closures with larger distance like:

Overall, it seems that most loop closures are detected. With GPS as priors, the VSLAM trajectory is very close to GPS RTK trajectory. For comparison to raw VSLAM (without GPS), by setting Optimizer/PriorsIgnored to true, here the trajectories (blue=VSLAM, green=GPS RTK):

Zooming at the end/start, we can see the overall drift. Having the drone landing in same orientation than the take off would have close the loop, to get a more globally consistent trajectory:

For your questions:

2. In my test above, I used GFTT for odometry.
3. rtabmap node will start a new session when it detects that odometry has very large covariance (>=9999). Visual odometry node will set 9999 as covariance when it is reset. The pose would be the last known odometry pose. One way to avoid a new session on rtabmap side would be to set publish_null_when_lost to false for visual odometry, then rtabmap won't notice that odometry has been reset. Note however that it will add wrong neighbor constraints in the graph difficult to correct afterwards (may even make all new loop closures rejected).
4. RGBD/LocalRadius is used only on rtabmap node part, not visual odometry. It should work either with RGB-D or stereo visual odometry. Not sure it didn't work from one case on your side.
5. For lack of time, I couldn't test VO with superpoint on your dataset. You will see for mapping part above I decimated the images, this may help for VO for your issues.
6. Is the GPS RTK always be available? You may set the RTK as guess_frame_id for visual odometry. This could also help at the same time to reset at the right place visual odometry when it fails. Otherwise for robot_localization, it may not be trivial to have rtabmap filtering GPS constraints in graph optimization while having robot_localization doing something similar in EKF fashion, the map frame computed by both packages may diverge.

and thanks for sharing the dataset, very interesting!

cheers, Mathieu

[zhuoyan28]

Can someone tell 1, RTAB-MAP commands to run KITTI image dataset alone, such as binocular images? Where will the output be stored in the ros environment? 2. ORBSLAM+RTAB-MAP running KITTI image dataset, such as binocular image command? Where will the output be stored in ros environment?

[matlabbe]

@zhuoyan28 see https://github.com/introlab/rtabmap_ros/issues/363#issuecomment-2017035795