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:
- Camera pose T i w T_{iw} Tiw, notice that here is the transformation matrix from the camera to the world system
- Camera internal parameters, including principal point and focal length
- 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:
- At least 20 frames from the last global relocation
- The local mapping thread is idle, or at least 20 frames have passed since the last key frame was inserted
- At least 50 map points are tracked in the current frame
- 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 if(mbOnlyTracking) 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; if(nKFs<=2) nMinObs=2; // 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) if(mSensor!=System::MONOCULAR) { 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]) nTrackedClose++; else nNonTrackedClose++; } } } // 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 if(nKFs<2) thRefRatio = 0.4f; //The frequency of inserting keyframes in monocular case is very high if(mSensor==System::MONOCULAR) 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((c1a||c1b||c1c)&&c2) { // 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 if(bLocalMappingIdle) { //You can insert keys return true; } else { mpLocalMapper->InterruptBA(); if(mSensor!=System::MONOCULAR) { // 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 if(mpLocalMapper->KeyframesInQueue()<3) //The number of keyframes in the queue is not large and can be inserted return true; else //There are too many buffered key frames in the queue to be inserted temporarily return false; } else //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; } } else //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 if(!mpLocalMapper->SetNotStop(true)) return; // 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 if(mSensor!=System::MONOCULAR) { // Calculate mRcw, mtcw, mRwc and mOw according to Tcw mCurrentFrame.UpdatePoseMatrices(); // 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; vDepthIdx.reserve(mCurrentFrame.N); // Traverse the map points of the current frame for(int i=0; i<mCurrentFrame.N; i++) { // Map point depth float z = mCurrentFrame.mvDepth[i]; if(z>0) { // The first element is the depth, and the second element is the id of the corresponding feature point vDepthIdx.push_back(make_pair(z,i)); } } if(!vDepthIdx.empty()) { // Step 3.2: sort by depth from small to large sort(vDepthIdx.begin(),vDepthIdx.end()); // 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]; if(!pMP) 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 if(bCreateNew) { 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 pNewMP->AddObservation(pKF,i); pKF->AddMapPoint(pNewMP,i); pNewMP->ComputeDistinctiveDescriptors(); pNewMP->UpdateNormalAndDepth(); mpMap->AddMapPoint(pNewMP); mCurrentFrame.mvpMapPoints[i]=pNewMP; nPoints++; } else { // Because it is sorted from near to far, the number of map points that do not need to be created is recorded nPoints++; } // 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) break; } } } // Step 4: insert keys // Insert the key frames into the list mlNewKeyFrames and wait for the local mapping thread to arrive mpLocalMapper->InsertKeyFrame(pKF); // After insertion, local drawing is allowed to stop mpLocalMapper->SetNotStop(false); // 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
//KeyFrame.cc 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); } -
Update local keys
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? break; } } } // Strategy 2.3: own parent keyframe (bring the parents of neighbors into the group) 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 break; } } // Omit }Most of the code will be described in detail when we talk about the three threads