Visual SLAM

Visual SLAM, as in ORB-SLAM, runs three jobs at once. Tracking estimates the camera pose in every frame from the features it sees. Local mapping adds keyframes and new map points. Loop closing watches for places seen before, and when it finds one, it corrects the drift that built up along the way.

Tracking drift Allow loop closure

Three threads

Tracking matches features in the new image to map points and solves for the camera pose. Local mapping decides when a frame is new enough to become a keyframe, then triangulates new map points and runs a small bundle adjustment. Loop closing compares each keyframe to old ones using place recognition. When it finds a match, it adds a constraint between the two keyframes and optimizes the whole pose graph:

$$\min_{\{T_k\}}\ \sum_{k} \big\lVert \log\!\left(\Delta T_{k,k+1}^{-1}\,T_k^{-1}T_{k+1}\right) \big\rVert^2 + \big\lVert \log\!\left(\Delta T_{\text{loop}}^{-1}\,T_{\text{last}}^{-1}T_0\right) \big\rVert^2$$

What to notice

As the robot drives, small tracking errors add up, so the red path and the map points it builds slowly drift from the truth. When the robot returns to the start and recognizes it, the loop closes and the error is spread back along the whole path. Without loop closure, the drift just keeps growing. This is why mapping a building usually means driving loops.