Keys and map points: Keys

Posted by canadian_angel on Tue, 04 Jan 2022 02:16:25 +0100

This time we will explain the key frames in ORBSLAM2. First, let's take a look at the relevant description of key frames in the paper: each key frame K i K_i Ki # stores the following:

  1. Camera pose T i w T_{iw} Tiw, notice that here is the transformation matrix from the camera to the world system
  2. Camera internal parameters, including principal point and focal length
  3. All the de distorted ORB features extracted in this frame

The creation conditions of map points and key frames are relatively loose, but then a very strict deletion mechanism will be used to eliminate redundant key frames and mismatched or untraceable map points.

The corresponding code is

    // Coordinates of SE3 pose and camera optical center
    cv::Mat Tcw;    // The pose of the current camera, from the world coordinate system to the camera coordinate system
    cv::Mat Twc;    // Inverse of the current camera pose
    cv::Mat Ow;     // The coordinates of the camera optical center (left eye) in the world coordinate system are the same as those in the normal frame

    cv::Mat Cw; ///< Stereo middel point. Only for visualization

    ///Map points observed by keyframes
    std::vector<MapPoint*> mvpMapPoints;

    // BoW dictionary
    KeyFrameDatabase* mpKeyFrameDB;
    // Visual words
    ORBVocabulary* mpORBvocabulary;

    // Mesh for accelerating feature matching 
    // In fact, it should be said that it is two-dimensional. The third vector stores the index of feature points in the grid
    std::vector< std::vector <std::vector<size_t> > > mGrid;

    // Common view
    // Key frames and weights connected to the key frame (at least 15 common view map points)
    std::map<KeyFrame*,int> mConnectedKeyFrameWeights;   
    // The keyframes whose weights are sorted from large to small in common view keyframes          
    std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames;            
    // The weight sorted from large to small in the common view key frame corresponds to the above
    std::vector<int> mvOrderedWeights;                             

    // =====================Spanning trees and closed-loop edges========================
    // std::set is a collection. Compared with vector, it will be sorted automatically when inserting data
    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 key frames of the current key frame, which is usually more than one
    std::set<KeyFrame*> mspLoopEdges;           // A key frame that forms a loopback relationship with the current key frame

    // Bad flags
    bool mbNotErase;            // The current key frame has formed a loopback relationship with other key frames, so it should not be deleted in the process of various optimization
    bool mbToBeErased;          // Flag to be deleted
    bool mbBad;                 // Flag with Bad key frame 

    float mHalfBaseline;        // For a binocular camera, half the baseline length of the binocular camera Only for visualization

    Map* mpMap;                 // Local map

    ///Mutexes associated when manipulating poses
    std::mutex mMutexPose;
    ///Mutex used when operating the co view relationship between the current key frame and other key frames
    std::mutex mMutexConnections;
    ///Mutex when manipulating variables related to feature points
    std::mutex mMutexFeatures;

Next, we need to understand the key frame selection strategy in ORBSLAM2. To insert a new key frame, the following conditions must be met:

  1. At least 20 frames from the last global relocation
  2. The local mapping thread is idle, or at least 20 frames have passed since the last key frame was inserted
  3. At least 50 map points are tracked in the current frame
  4. The number of map points tracked by the current frame is less than the reference key frame K r e f K_{ref} Kref 90% of the number of map points

Here, the distance from other key frames is not used as the condition for judging whether to insert a key frame, but the visual change is used (condition 4). Condition 1 ensures good relocation and condition 3 ensures good tracking. If a key frame is inserted when the local mapping thread is busy (the second part of condition 2), a signal will be sent to stop the local BA so that it can process the new key frame as soon as possible.

The corresponding code is

  • Determine whether keyframes need to be inserted
     * @brief Determine whether the current frame needs to insert a key frame
     * Step 1: In the pure VO mode, no key frame is inserted. If the local map is used by closed-loop detection, no key frame is inserted
     * Step 2: If it is close to the last relocation, or the number of keyframes exceeds the maximum limit, no keyframes are inserted
     * Step 3: Get the number of map points tracked by the reference key frame
     * Step 4: Query whether the local map manager is busy, that is, whether it can accept new keyframes at present
     * Step 5: For binocular or RGBD cameras, count the total number of effective map points that can be added and the number of map points tracked
     * Step 6: Decide whether to insert keys
     * @return true         need
     * @return false        unwanted
    bool Tracking::NeedNewKeyFrame()
        // Step 1: no keyframes are inserted in pure VO mode
            return false;
        // If Local Mapping is freezed by a Loop Closure do not insert keyframes
        // Step 2: if the local map thread is used by closed-loop detection, no key frame is inserted
        if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
            return false;
        // Gets the number of keyframes in the current map
        const int nKFs = mpMap->KeyFramesInMap();
        // Do not insert keyframes if not enough frames have passed from last relocalisation
        // mCurrentFrame.mnId is the ID of the current frame
        // mnLastRelocFrameId is the ID of the last relocation frame
        // mMaxFrames is equal to the frame rate of the image input
        //  Step 3: if it is close to the last relocation and the number of keyframes exceeds the maximum limit, no keyframes will be inserted
        if( mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && nKFs>mMaxFrames)                                     
            return false;
        // Tracked MapPoints in the reference keyframe
        // Step 4: get the number of map points tracked by the reference key frame
        // The updatelockeyframes function sets the key frame with the highest degree of CO viewing with the current key frame as the reference key frame of the current frame 
        // Minimum observation times of map points
        int nMinObs = 3;
        // Number of observations in map points of reference key frame > = number of map points of nminobs
        int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);
        // Local Mapping accept keyframes?
        // Step 5: query whether the local map thread is busy and whether it can accept new key frames
        bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
        // Check how many "close" points are being tracked and how many could be potentially created.
        // Step 6: for binocular or RGBD cameras, count the number of near points successfully tracked. If too few near points are tracked and too many near points are not tracked, key frames can be inserted
         int nNonTrackedClose = 0;  //Near points not tracked in binocular or RGB-D
        int nTrackedClose= 0;       //Near point successfully tracked in binocular or RGB-D (3D point)
            for(int i =0; i<mCurrentFrame.N; i++)
                // The depth value is within the valid range
                if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth)
                    if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
        // In the case of binocular or RGBD: there are too few near points in the tracked map points and too many 3D points are not tracked, so you can insert key frames
        // false when monocular
        bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);
        // Step 7: decide whether to insert keyframes
        // Step 7.1: set the scale threshold. The scale of the point tracked by the current frame and the reference key frame. The larger the scale, the more likely it is to increase the key frame
        float thRefRatio = 0.75f;
        // If there is only one key frame, the threshold value for inserting a key frame is set lower and the insertion frequency is lower
            thRefRatio = 0.4f;
        //The frequency of inserting keyframes in monocular case is very high    
            thRefRatio = 0.9f;
        // Condition 1a: More than "MaxFrames" have passed from last keyframe insertion
        // Step 7.2: keyframes can be inserted if they have not been inserted for a long time
        const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;
        // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
        // Step 7.3: meet the minimum interval for inserting key frames, and localMapper is idle. You can insert keys
        const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
        // Condition 1c: tracking is weak
        // Step 7.4: in the case of binocular and RGB-D, the points tracked by the current frame are less than 0.25 times of the reference key frame, or meet bNeedToInsertClose
        const bool c1c =  mSensor!=System::MONOCULAR &&             //Only consider the case of binocular, RGB-D
                        (mnMatchesInliers<nRefMatches*0.25 ||       //The current frame matches very few map points
                          bNeedToInsertClose) ;                     //Need to insert
        // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
        // Step 7.5: compared with the reference frame, the currently tracked points are too few or meet bNeedToInsertClose; At the same time, not too few interior points can be tracked
        const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);
            // If the mapping accepts keyframes, insert keyframe.
            // Otherwise send a signal to interrupt BA
            // Step 7.6: local mapping can be inserted directly when it is idle. When it is not idle, it should be inserted according to the situation
                //You can insert keys
                return true;
                    // You can't block too many keyframes in the queue
                    // The tracking key frame is not inserted directly, and is first inserted into mlNewKeyFrames,
                    // Then the localmapper pop s out one by one and inserts it into mspKeyFrames
                        //The number of keyframes in the queue is not large and can be inserted
                        return true;
                        //There are too many buffered key frames in the queue to be inserted temporarily
                        return false;
                    //For monocular cases, keyframes cannot be inserted directly
                    //?  Why is the monocular case handled differently here?
                    //Answer: monocular keyframes may be relatively dense
                    return false;
            //If the above conditions are not met, keyframes cannot be inserted naturally
            return false;

    The judgment conditions here are complex. The meaning of some variables has not been mentioned yet. You can wait until later to understand them carefully

  • Insert keys:
     * @brief Create a new keyframe
     * For non single purpose cases, new MapPoints are created at the same time
     * Step 1: Keyframe the current frame
     * Step 2: Sets the current key as the reference key for the current frame
     * Step 3: For binocular or rgbd cameras, generate new MapPoints for the current frame
    void Tracking::CreateNewKeyFrame()
        // If the local mapping thread is closed, keyframes cannot be inserted
        // Step 1: keyframe the current frame
        KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
        // Step 2: set the current keyframe as the reference keyframe of the current frame
        // In the updatelockeyframes function, the key frame with the highest degree of CO viewing with the current key frame is set as the reference key frame of the current frame
        mpReferenceKF = pKF;
        mCurrentFrame.mpReferenceKF = pKF;
        // This code has the same function as that in Tracking::UpdateLastFrame
        // Step 3: for binocular or rgbd cameras, generate new map points for the current frame; Monocular no operation
            // Calculate mRcw, mtcw, mRwc and mOw according to Tcw
            // We sort points by the measured depth by the stereo/RGBD sensor.
            // We create all those MapPoints whose depth < mThDepth.
            // If there are less than 100 close points we create the 100 closest.
            // Step 3.1: get the feature points (not necessarily map points) with depth value in the current frame
            vector<pair<float,int> > vDepthIdx;
            // Traverse the map points of the current frame
            for(int i=0; i<mCurrentFrame.N; i++)
                // Map point depth
                float z = mCurrentFrame.mvDepth[i];
                    // The first element is the depth, and the second element is the id of the corresponding feature point
                // Step 3.2: sort by depth from small to large
                // Step 3.3: find the generated temporary map points that are not map points 
                // Number of near points processed
                int nPoints = 0;
                for(size_t j=0; j<vDepthIdx.size();j++)
                    // Map point id
                    int i = vDepthIdx[j].second;
                    bool bCreateNew = false;
                    // If this point does not correspond to the map point in the previous frame, or is not observed after creation, a temporary map point is generated
                    MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
                        bCreateNew = true;
                    else if(pMP->Observations()<1)
                        bCreateNew = true;
                        mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
                    // If necessary, create new map points. The map points here are not temporary, but new map points in the global map for tracking
                        cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
                        MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);
                        // These operations to add attributes are done every time a MapPoint is created
                        // Because it is sorted from near to far, the number of map points that do not need to be created is recorded
                    // Step 3.4: to stop creating new map points, the following conditions must be met at the same time:
                    // 1. The depth of the current point has exceeded the set depth threshold (35 times the baseline)
                    // 2. nPoints have exceeded 100 points, indicating that the distance is far away and may be inaccurate. Stop and exit
                    if(vDepthIdx[j].first>mThDepth && nPoints>100)
        // Step 4: insert keys
        // Insert the key frames into the list mlNewKeyFrames and wait for the local mapping thread to arrive
        // After insertion, local drawing is allowed to stop
        // The current frame becomes a new keyframe and is updated
        mnLastKeyFrameId = mCurrentFrame.mnId;
        mpLastKeyFrame = pKF;

Finally, let's take a look at the more important functions corresponding to key frames

  • Update connection relationship

    // 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
    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);
    // Delete a sub key
    void KeyFrame::EraseChild(KeyFrame *pKF)
    unique_lock<mutex> lockCon(mMutexConnections);
    // 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;
    //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);
  • Update local keys

    void Tracking::UpdateLocalKeyFrames()
    // 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;
    //?  Find one and jump out of the for loop?
    // Strategy 2.3: own parent keyframe (bring the parents of neighbors into the group)
    KeyFrame* pParent = pKF->GetParent();
    // Mntrackreferenceframe prevents repeated addition of local keys
    //!  It feels like a bug! If the parent key is found, it will jump out of the whole cycle directly
    // Omit

    Most of the code will be described in detail when we talk about the three threads

Topics: Autonomous vehicles ORBSLAM2