Loop closures and neighbouring links only make my map worse.

[Aspen]

I am running in a simulated environment with a simulated downwards facing Lidar and RGB camera. My odometry measurements are perfect (given that they are the exact values published from my simulator environment) so I know that running with no loop closures or neighbouring links should (and does) produce a 'perfect' map. Perfect in the sense that the output 3D occupancy grid looks correct. See below for the expected result:




However when I have loop closures turned on by enabling Kp/MaxFeatures > -1
 `rtabmap-reprocess --Kp/MaxFeatures 0 old.db new.db`  

My neighbour links steadily get worse and worse making the map diverge from reality to the point where loop closures are rejected due to being filtered out as they are trying to correct for something greater than my maximum error. See the below image for the output.





I'm at a bit of a loss as to what would be causing this error
I use pointcloud2depth to create a depth image and sync that with an RGB image stream.

Below is the config parameters that I change for the rtabmap node in my launch file.

Grid/3D                    =" true
Grid/FromDepth             =" true
Grid/FlatObstacleDetected  =" true
Grid/GroundIsObstacle      =" true
Grid/NormalK               =" 20
Grid/NormalsSegmentation   =" true
Grid/RayTracing            =" false
Grid/CellSize              =" 0.5
Grid/RangeMax              =" 0.0
Grid/RangeMin              =" 0.0
Rtabmap/TimeThr  =  1000
Rtabmap/PublishLastSignature  =  true
Rtabmap/SaveWMState  =  true
Rtabmap/MemoryThr  =  50"
Rtabmap/MaxRetrieved  =  4
Mem/STMSize  =  10
Rtabmap/LoopThr  =  0.05
Mem/RehearsalSimilarity  =  0.2
Mem/ReduceGraph  =  true
Rtabmap/CreateIntermediateNodes  =  false
RGBD/OptimizeMaxError  =  0.5
RGBD/LoopClosureReextractFeatures  =  true
Vis/MaxFeatures  =  1500
Vis/MinInliers  =  20
Vis/EstimationType  =  1
Vis/CorType  =  1
OdomF2M/MaxSize  =  2000
Mem/UseOdomGravity  =  true"
Optimizer/GravitySigma  =  0.2
SURF/HessianThreshold  =  300


Is there a good procedure I can follow to help debug my issue?

Is there any more information I can provide to help explain my issue or help find a solution?

I'm under the assumption that in a simulated environment rtabmap should perform far better than I what would expect when I run this on a real drone (less noise, perfect odom, no motion blur etc).

The intention is to be running this on a drone so flying fast and high is the ideal scenario, are there particular methods of feature detection that would be recommended as better to use for high speed (ORB vs SIFT for example)?

Any help would be really appreciated.

Thanks!

[matlabbe]

Can you share that database?

[Aspen]

Yes, here is the link to the .db

[matlabbe]

Hi,

The main reason is that loop closure transformation with PnP gives poor angular accuracy with this kind of dataset. Because the camera is far from the 3D points, a small orientation error can produce large jumps at drone height (it is similar to AprilTag orientation issue when the tag looks very small in the images and the robot is far from the tag). PnP minimizes at pixel level, so when the points are far from the camera, minimizing in pixel space is not super accurate (in particular with features that are not subpixel). Image resolution is 800x600 and only a small part of the image has depth overlap (on the right we can see the poor overlap between the two corresponding point clouds):


In the examples below, I removed graph reduction and memory management to just look at the actual loop closures accuracy when keeping all data. The first approach is using Vis/EstimationType=0 (3D->3D) instead of 1 (PnP 2D->3D) to minimize the 3D point distances instead of pixel. Overlap is a lot better (we see more blue and red points interleaved):

rtabmap-reprocess Mem/ReduceGraph false \
   Mem/UseOdomGravity true \
   Optimizer/GravitySigma 0.3 \
   Rtabmap/MemoryThr 0 \
   RGBD/LoopClosureReextractFeatures false \
   RGBD/OptimizeMaxError 4  \
   Rtabmap/LoopThr 0.11 \
   Mem/STMSize 30 \
   Reg/Strategy 0 \
   Vis/EstimationType 0 \
   Vis/InlierDistance 0.5 \
   Mem/UseOdomFeatures false \
   optix.db output_3d3d.db

Here is another example with ICP (libpointmatcher required here):

rtabmap-reprocess Mem/ReduceGraph false \
   Mem/UseOdomGravity true \
   Optimizer/GravitySigma 0.3 \
   Rtabmap/MemoryThr 0 \
   RGBD/LoopClosureReextractFeatures false \
   RGBD/OptimizeMaxError 4  \
   Rtabmap/LoopThr 0.11 \
   Mem/STMSize 30 \
   -scan_from_depth -scan_voxel_size 0.5 -scan_normal_k 10 \
   Icp/PM true \
   Icp/MaxTranslation 0 \
   Icp/MaxCorrespondenceDistance 1\
   Icp/PointToPlane true \
   Icp/PointToPlaneMinComplexity 0 \
   Reg/Strategy 1 \
   RGBD/ProximityPathMaxNeighbors 0 \
   Icp/PMOutlierRatio 0.85 \
   optix.db output_icp.db

Both 3D-3D and ICP approaches would give results similar to this, with ICP having more accepted loop closures and proximity detections (322 for ICP and 150 for 3D-3D):

Grid is 1 meter cell size:


Regards,
Mathieu