December 24, 2020

OpenVSLAM Map-Localization

OPENVSLAM Map-Localization

This blog post mainly describes the details of localization based on a prior feature map in feature-point-based SLAM. Here we take openvslam as an example; its workflow is basically the same as that of ORB-SLAM2, except that openvslam provides multiple functions such as map dump, load, localization, and SLAM.

Feature-Map Format

When running openvslam in SLAM mode, a feature-point map can be generated. This is also the prior map used for subsequent localization. Here we give a brief description of the map contents:

The stored map includes information such as map_db and cam_db. The data structure of its map_database, which is the map structure in ORB-SLAM2, is as follows:

  1. keyframes
  2. landmarks
  3. local landmarks
  4. origin_keyframes (origin keyframes)

The saved map file mainly contains keyframe information, landmark information, and their correspondences. The above information is decoded out of the map.

  • Building keyframes

First, the keyframes are decoded, parsing the keyframe information in the map into the keyframes of the map_database. First, the meta data of the current keyframe is injected, including:

  1. n_keypts: the number of keypoints

  2. ts: timestamp

  3. cam: camera name

  4. depth_thr: depth threshold

Then the other information of the current keyframe is injected:

  1. pose information
  2. keypoint information (coordinates, orientation, level),
  3. keypoint descriptors
  4. x_right and depth

The keyframe is constructed from this information and added to the keyframes of the map_database.

  • Building landmarks

Then the landmarks are decoded, parsing the stored landmark information into the landmarks of the map_database. The landmark information in the map mainly includes:

  1. 1st_keyfrm: the first keyframe that observed this landmark
  2. pow: the 3D position
  3. ref keyframe: the reference keyframe of this landmark
  4. n_vis
  5. n_fnd
  • Building the spanning tree

Then a spanning tree is built from the stored keyframe link relationships, forming a connection graph between keyframes. The decoded keyframe information mainly includes:

  1. span_parent: the parent node of the current keyframe (unique)
  2. span_children: the child nodes of the current keyframe (there may be several)
  3. loop_edges: the loop-closure frames of the current frame The above information is used to build the graph node of the current keyframe (parent, children, loop edges).
  • Establishing links between keyframes and map points

Then the link relationships between the parsed keyframes and map points are established. The parsed keyframe information is lm_ids, which marks the indices in landmarks of the map points corresponding to the feature points of the current keyframe. Then there are two link relationships:

keyfrm->add_landmark(lm, idx) \\ set the landmark corresponding to the idx-th feature point of the keyframe
lm->add_observation(keyfrm, idx) \\ set that this landmark is observed by the idx-th feature point of the keyframe
  • update covisibility graph

Based on the correspondence between keyframes and map points, the covisibility relationships between keyframes are updated using the observation information of the current keyframe’s map points, forming the covisibility graph.

  • update landmark geometry and desc

The normal direction, depth, and descriptor of each landmark are updated based on the multiple keyframes that observed it.

The constructed feature-point map is shown in the figure below:

It includes landmarks, keyframes, and the graph.

Map-Based Relocalization

With a prior map available, initialization is not required as it is for monocular SLAM mode. At the start, the tracker’s state is lost; then, at the first frame, it enters relocalization mode and performs relocalization using the current frame and the keyframe_database. The main steps include:

  1. Compute the candidate keyframes for the current frame based on its bag-of-words vector.
  2. Perform BoW matching between the current frame and each candidate keyframe.
  3. Build a PnP solver from the feature points and their corresponding landmarks, and compute the initial value of the current camera pose with EPnP-RANSAC.
  4. Optimize the pose of the current frame with the pose optimizer based on the PnP inlier correspondences.
  5. Use the optimized pose for projection matching to obtain more 2D-3D correspondences, and optimize again with the pose optimizer.
  6. If the number of inliers exceeds the threshold, relocalization succeeds; otherwise, perform another projection matching based on the already-found landmarks and attempt pose optimization again.

It can be seen that it consistently follows this paradigm of pose solving: first solve an initial pose with an algebraic algorithm (using the fewest correspondences), then refine the pose using more correspondences, and then use the more accurate pose to obtain even more correspondences through projection matching for further optimization. The pose accuracy goes from coarse to fine, and the amount of data used goes from less to more.

The figure below shows a frame in which relocalization succeeded:

The figure below shows the result of performing relocalization on every frame; stable localization can be achieved in most cases.

Map-Based Tracking

After relocalization succeeds, the localization-tracking mode begins. It first tracks against the previous frame or the reference frame; after successful tracking, it updates the local map and tracks using the local map. The key code is as follows:

// set the system's reference keyframe as the reference keyframe of the current frame
curr_frm.ref_keyfrm_ = ref_keyfrm_;
auto succeeded = track_current_frame();
if(succeeded) {
    update_local_map();
    succeeded = optimize_current_frame_with_local_map();
}
if(succeeded) {
    update_motion_model();
}

Here, track_current_frame includes four tracking modes: in the tracking state, they are:

  1. motion_based_track
  2. bow_match_based_track
  3. robust_match_based_track as well as relocalize in the lost state (already analyzed above).

Under normal conditions, when velocity is valid, tracking can be completed with motion_track. When (1) fails, the bow_match method is used to complete tracking; when (2) also fails, brute-force feature-point matching is used to complete tracking. The details of motion_track are as follows:

  1. Update the pose of the current frame based on the motion model.
  2. Using the pose, perform projection matching between the current frame and the previous frame to determine the map points corresponding to the feature points of the current frame. If there are too few matches, try enlarging the search radius for matching; if it is still too few, motion tracking fails.
  3. Optimize the pose with pose_optimizer based on the correspondences; during optimization, determine whether a point is an outlier according to the reprojection error of the landmark.
  4. Discard the outlier observations of the current frame.

The workflow of bow_match_track is basically the same as motion_track, except that matching is changed to BoW matching against the keyframe of the current frame. Matching determines the map points corresponding to the feature points of the current frame; the initial value of the current frame is then set to that of the previous frame, the pose is optimized with pose_optimizer, and discard_outlier is performed after optimization.

The workflow of robust_match_track is basically the same as the other types of tracking.

The result of tracking using only the previous frame or the reference frame is shown in the figure below:

After successful frame-to-frame tracking, update_local_map is performed (update local keyframes, update local landmarks). The details of updating the local keyframes are as follows:

  1. Build keyfrm_weights based on the landmarks matched by the current frame and their observations in other keyframes.
  2. Add the keyframes that share covisibility into local_keyfrms, and compute the keyframe with the most covisibility.
  3. Expand the range of local keyframes by also adding the covisible frames of the first-order keyframes, along with their spanning_children and spanning_parent.
  4. Set the most-covisible keyframe as the system’s reference keyframe and the current frame’s reference keyframe.

The details of updating the local landmarks are as follows:

  1. Iterate over the local keyframes.
  2. Add the landmarks observed by each keyframe into the local landmarks.

Finally, based on the updated local map, optimize_current_frame_with_local_map() is performed. Its implementation is as follows:

  1. Filter out the local landmarks that can be observed by the current frame (through 3D point projection).
  2. Obtain more feature-point-to-map-point correspondences for the current frame using projection matching.
  3. Perform pose optimization based on the correspondences.

Comments