December 2, 2020

ORB-SLAM2 Initialize

orbslam2-initializer

This blog post mainly describes the initialization process in ORB-SLAM2.

  • Monocular initialization

First, when the system has not yet been initialized, the current system is initialized, provided of course that the number of feature points in the current frame meets the requirement. An initializer is built from the current frame. When the number of feature points in the next frame also meets the requirement, the matcher is used to perform feature-point matching. If the number of matches meets the requirement, initialization is attempted. As shown in the figure below, two consecutive frames from the KITTI dataset are used for initialization, and the feature-point matching result of the two frames is shown in the figure:

By calling:

mpInitializer->Initialize(
            mCurrentFrame,      // current frame
            mvIniMatches,       // feature-point correspondences between the current frame and the reference frame
            Rcw, tcw,           // camera pose obtained from initialization
            mvIniP3D,           // set of 3D points obtained by triangulation
            vbTriangulated)

we obtain the rotation and translation between the two frames, the initial triangulated points, and a flag indicating whether triangulation succeeded. Then, based on the initialization result, the monocular initialization map is created. The procedure is as follows:

  1. Build keyframes from the current frame and the reference frame, and insert the keyframes into the map.
  2. Generate map points from the initial correspondences, create the map points and insert them into the map, and establish the observation and connection relationships among the three parties: keyframes, map points, and the map.
  3. Perform a global optimization on the two initialized frames and the map points. The figure shows how the residual changes during optimization:
  4. Compute the median depth of the scene from the triangulated map points, and use the median depth to normalize the transform and the map points (optional).
  5. Build the local map, local keyframes, and local map points.

The result after initialization is shown in the figure below: Here, the blue boxes are the two successfully initialized keyframes, the green box is the current camera, the red points are the reference map points during initialization, and the black points are all the map points. The points beyond the reference map points are some new map points generated by the local mapping thread through feature-point matching. Because the feature matching during initialization is not sufficient, every time the tracking thread sends a keyframe to the local mapping thread, it enters the local mapping loop and adds some new map points by performing BoW matching between the two keyframes.

This completes the monocular initialization. Next, we analyze the initialization function Initialize. The basic steps are as follows:

  1. Build the set of 8-point index groups for RANSAC based on the feature-point correspondences between the reference frame and the current frame.
  2. Compute the homography and fundamental matrices in parallel using two threads.
  3. Select one of them based on a score (bidirectional reprojection error) to recover the inter-frame transform and the triangulated 3D points.

Both the homography matrix and the fundamental matrix are computed using 8 normalized points via SVD decomposition. When recovering the transform matrix from H or F, multiple solutions are produced; the solution that yields the most triangulated points is selected as the final solution. There is also a corresponding ratio test (the number of 3D points of the best solution must be far greater than that of the second-best solution), the parallax angle must satisfy a threshold, the number of triangulated points must satisfy a threshold, and so on.

  • Stereo and RGB-D initialization

Initialization is performed directly through back-projection: the map points are inserted into the map, and the initial frame is also inserted into the map as a keyframe. The initial frame is passed to local mapping, local keyframes are set, and so on. As shown in the figure:

At this point, the reference map points are all the map points; initialization is completed directly from the first frame. Because there is only one frame in local mapping, no new map points are generated. Stereo initialization is simple, and compared with monocular it can determine the scale, which is a significant advantage.

Comments