Skip to content

[feat] Integrate teaserpp for automatic loop detection

Tong Fu requested to merge feat/IntegrateTeaserpp into master

This automatic loop closure detection is base on the registration quality using teaser++ algorithm between a query frame and its candidate frames. (TEASER++ is a fast and certifiably-robust point cloud registration library.) It can detect whether there is a loop or not for a query frame and give the revisited frame if there is a loop. A total automatic loop closure detection, which gives all the loop closure in a map, can be implemented easily from this MR.

The idea is that for a query frame, some candidate frames are sampled from frames before the query frame. The submaps is built around the query frame and the candidate frames for registration. Then the registration by teaser++ is performed iteratively between the query frame and each candidate frame. An overlap estimator is computed at the meantime. If the overlap is greater than a threshold, the best match among candidate frames is considered as the revisited frame of the query frame.

To use this feature, we need to stop slam and trigger OptimizeGraph process with teaserpp detector chosen. If a loop is detected for the current frame, the teaserpp detector give a revisited index. Then a constraint between the query frame and the revisited frame is added into pose graph.

The important changes for this merge request are as following :

  • Regroup all loop parameters in a struct and refact get/set macros for a struct in SLAM
  • Add teaser++ library to the superbuild of SLAM (the superbuild is no longer in this repo, see MR !12 in slam-superbuild)
  • Add teaser++ and optional pcl components as dependency of Slam
  • Add LoopClosureDetector to choose the method of detection
  • Add a basic overlap estimator to evaluate the registration result given by teaser
  • Add function ComputeFPFHFeatures
  • Add functions CalculateCorrespondences
  • Add function DetectLoopWithTeaser. The relevant parameters are:
    • GapLength to indicate the travelled distance before the query frame where is condidered no loop formed.
    • SampleStep to indicate the sample step (in meters) for the candidate frames.
    • EvaluationThreshold is the threshold to validate the loop detection.
    • DetectionTransform save the pose of the query frame estimated by automatic loop detection method.

Detail for execution time in function DetectLoopWithTeaser:

Based on an example of the execution time tested on a ourdoor vehicule scene of different steps in principle function DetectLoopWithTeaser

  • Compute fpfh feature for query points ~ 29ms for 11 778 points
  • In the loop for candidate sub maps
    • Compute fpfh feature for keypoints of each candidate sub maps ~ average time about 65ms for 32305 points
    • Compute correpondances between query sub maps and each candidate sub maps ~ average time about 1505ms
    • Teaser registration for each candidate sub maps ~ about 59ms
  • Estimate overlap ~ 7ms
  • Total time for detection is 28726ms (registration with 15 candidate submaps)

Next improvements following this MR are listed here.

Edited by Julia Sanchez

Merge request reports