preface

I don't know what to write in this preface. Let's encourage each other.

1, KeyFrame

In short: a key frame is a representative of several ordinary frames.

1. Function and significance

1. Reduce the information redundancy in local adjacent key frames;
2. In SLAM scheme, the depth of ordinary frame will be projected onto the key frame, so to a certain extent, the key frame is the result of ordinary frame filtering and optimization to prevent invalid and error information from entering the optimization process;
3. The compromise between computing power and accuracy for back-end optimization improves the efficiency of computing resources.

2. Selection

1. High quality: clear picture, sufficient number of feature points, uniform distribution of feature points, etc;
2. Good contact network: there is a certain common view relationship with other key frames, and the repeatability cannot be too high, that is, there are constraints and reduce redundant information.

Selection direction (you can select and then filter according to the above criteria):
① Whether the time (number of frames) interval between and the previous key frame is appropriate;
② Whether the distance from the previous key frame is far enough;
③ Track the quality of the local map (number of common view feature points)

2, Co visibility graph

1. Concept

Source: computer vision life

Common view key frame: any key frame that can observe the same map point as the key frame (primary key frame).
The primary key frame of the key frame itself is called the secondary key frame of the key frame.
Note: the number of the same key frames that can be observed is represented by the degree of common view.

Common view: a common view network constructed by taking any key frame as the center and connecting with its common view key frame and.
The nodes in the common view are keyframes; If there are more than 15 map points shared by two keyframes, an edge is used to connect the two. Use weight value θ Indicates the number of point clouds that can be observed jointly by two keyframes.

Generally, only the common view relationship and information of one adjacent level will be used, while the two adjacent common view relationships will be used for local map optimization.

2. Function

① Add map point information to optimize the map;
② Indicates the relationship and tightness between key frames.

3. Application scenarios in orb-slam2

① Track local maps and expand the search scope: tracking:: updatelocal keyframes();
② Create new map points between key frames in local mapping: LocalMapping::CreateNewMapPoints(), LocalMapping::SearchInNeighbors();
③ Closed loop detection and relocation detection: LoopClosing::DetectLoop(), LoopClosing::CorrectLoop(), KeyFrameDatabase::DetectLoopCandidates(), KeyFrameDatabase::DetectRelocalizationCandidates();
④ Optimization: Optimizer::OptimizeEssentialGraph().

3, Spinning Tree

It is composed of parent-child keyframes and is often used in Essential Graph.

Source: computer vision life

In the figure:
Red: parent keyframe;
Green: sub key frame;
Yellow: keyframes without parent-child relationships.

4, Essential Graph

An image formed for keyframes.

1. Features

1. Compared with the common view, the essential graph is sparse;
2. Nodes represent all keyframes, but the connected edges only retain the edges between closely connected keyframes, which makes the result more accurate;
3. The information contained in the figure is:
   ① The connection relationship of the extension tree;
   ② Form a closed-loop connection relationship;
   ③ After the closed loop, the new connection relationship caused by the change of map points;
   ④ Good connection relationship of common view relationship (at least 100 common view map points).

2. Function

In loop correct, similarity transform (Sim3) is used to correct scale drift, and the closed-loop errors are evenly spread in the essential diagram.

3. Advantages of essential graph compared with global BA

From the results,
① The global BA has a convergence problem. Even if the iteration is 100 times, the relative mean square error RMSE is relatively large;
② Essential Graph optimization can converge quickly and the results are more accurate. θ min indicates the minimum number of common view map points required to be selected as the Essential Graph node. From the results, θ The size of min has little effect on the accuracy, but it is larger θ min value can significantly reduce the running time;
③ Essential Graph optimization + global Full BA can improve the accuracy to some extent, but it will take more time.

The author's final strategy:
Essential graph optimization with at least 100 common view map points + global BA optimization with 20 iterations.

5, Codes related to the above

1. Data structure definition:

// KeyFrame.h file
    bool mbFirstConnection;                     // Is this the first spanning tree
    KeyFrame* mpParent;                         // Parent keyframe of the current keyframe (the one with the highest common view)
    std::set<KeyFrame*> mspChildrens;           // Stores the sub keys of the current key frame

1. Update the connection relationship when a new key frame is generated:

// KeyFrame.cc file
void KeyFrame::UpdateConnections()
{
	//Omit
	        // Step 5 update the connection of the spanning tree
        if(mbFirstConnection && mnId!=0)
        {
            // Initialize the parent key of this key frame to the key frame with the highest degree of CO viewing
            mpParent = mvpOrderedConnectedKeyFrames.front();
            // Establish a two-way connection relationship and take the current key frame as its sub key frame
            mpParent->AddChild(this);
            mbFirstConnection = false;
        }
    }
}


	// Add a sub key frame (that is, the key frame with the maximum common view relationship with the sub key frame is the current key frame)
	void KeyFrame::AddChild(KeyFrame *pKF)
	{
	    unique_lock<mutex> lockCon(mMutexConnections);
	    mspChildrens.insert(pKF);
	}
	
	// Delete a sub key
	void KeyFrame::EraseChild(KeyFrame *pKF)
	{
	    unique_lock<mutex> lockCon(mMutexConnections);
	    mspChildrens.erase(pKF);
	}
	
	// Changes the parent key of the current key
	void KeyFrame::ChangeParent(KeyFrame *pKF)
	{
	    unique_lock<mutex> lockCon(mMutexConnections);
	    // Add a two-way connection relationship
	    mpParent = pKF;
	    pKF->AddChild(this);
	}
	
	//Gets the sub key of the current key
	set<KeyFrame*> KeyFrame::GetChilds()
	{
	    unique_lock<mutex> lockCon(mMutexConnections);
	    return mspChildrens;
	}
	
	//Gets the parent key of the current key
	KeyFrame* KeyFrame::GetParent()
	{
	    unique_lock<mutex> lockCon(mMutexConnections);
	    return mpParent;
	}
	
	// Determines whether a key frame is a sub key of the current key frame
	bool KeyFrame::hasChild(KeyFrame *pKF)
	{
	    unique_lock<mutex> lockCon(mMutexConnections);
	    return mspChildrens.count(pKF);
	}

1. use
   ① Updatelockeyframe() in TrackLocalMap updates the key frames in the local map

void Tracking::UpdateLocalKeyFrames()
{
	// Omit
	// Strategy 2.2: take one's own sub key frame as a local key frame (draw neighbors' descendants into the group)
        const set<KeyFrame*> spChilds = pKF->GetChilds();
        for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++)
        {
            KeyFrame* pChildKF = *sit;
            if(!pChildKF->isBad())
            {
                if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
                {
                    mvpLocalKeyFrames.push_back(pChildKF);
                    pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                    //?  Find one and jump out of the for loop?
                    // yes
                    break;
                }
            }
        }

        // Strategy 2.3: own parent keyframe (bring in the parents of neighbors)
        KeyFrame* pParent = pKF->GetParent();
        if(pParent)
        {
            // Mntrackreferenceframe prevents repeated addition of local keys
            if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
            {
                mvpLocalKeyFrames.push_back(pParent);
                pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                //!  It feels like a bug! If the parent key is found, it will jump out of the whole cycle directly. This is an if at the beginning. There are still some that haven't been cycled
                break;
            }
        }
    }

② Optimize Essential Graph during closed-loop correction

// In Optimizer.cc
void Optimizer::OptimizeEssentialGraph()
{
	// Omit
	        // Spanning tree edge
        // Step 4.1: add the second kind of edge: the edge of the extended tree (with parent keys)
        // The parent keyframe is the keyframe with the highest degree of CO viewing with the current frame
        if(pParentKF)
        {
            int nIDj = pParentKF->mnId;

            g2o::Sim3 Sjw;

            LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF);

            //The posture without Sim3 propagation adjustment is preferred
            if(itj!=NonCorrectedSim3.end())
                Sjw = itj->second;
            else
                Sjw = vScw[nIDj];

            // Calculates the relative pose between parent and child keyframes
            g2o::Sim3 Sji = Sjw * Swi;

            g2o::EdgeSim3* e = new g2o::EdgeSim3();
            e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));
            e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
            // You want the pose difference between parent and child keyframes to be minimal
            e->setMeasurement(Sji);
            // The contribution of all elements is the same; The contribution of each error edge to the total error is also the same
            e->information() = matLambda;
            optimizer.addEdge(e);
        }
}

Wait