当前位置：首页 > news >正文

ORB_SLAM2原理及代码解析：Tracking::CreateInitialMapMonocular() 函数

news 2025/10/5 5:14:10

1 作用

创建初始化地图。

2 参数及含义

3 代码

void Tracking::CreateInitialMapMonocular()
{// Create KeyFramesKeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);pKFini->ComputeBoW();pKFcur->ComputeBoW();// Insert KFs in the mapmpMap->AddKeyFrame(pKFini);mpMap->AddKeyFrame(pKFcur);// Create MapPoints and asscoiate to keyframesfor(size_t i=0; i<mvIniMatches.size();i++){if(mvIniMatches[i]<0)continue;//Create MapPoint.cv::Mat worldPos(mvIniP3D[i]);MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);pKFini->AddMapPoint(pMP,i);pKFcur->AddMapPoint(pMP,mvIniMatches[i]);pMP->AddObservation(pKFini,i);pMP->AddObservation(pKFcur,mvIniMatches[i]);pMP->ComputeDistinctiveDescriptors();pMP->UpdateNormalAndDepth();//Fill Current Frame structuremCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;//Add to MapmpMap->AddMapPoint(pMP);}// Update ConnectionspKFini->UpdateConnections();pKFcur->UpdateConnections();// Bundle Adjustmentcout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl;Optimizer::GlobalBundleAdjustemnt(mpMap,20);// Set median depth to 1float medianDepth = pKFini->ComputeSceneMedianDepth(2);float invMedianDepth = 1.0f/medianDepth;if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100){cout << "Wrong initialization, reseting..." << endl;Reset();return;}// Scale initial baselinecv::Mat Tc2w = pKFcur->GetPose();Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;pKFcur->SetPose(Tc2w);// Scale pointsvector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++){if(vpAllMapPoints[iMP]){MapPoint* pMP = vpAllMapPoints[iMP];pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth);}}mpLocalMapper->InsertKeyFrame(pKFini);mpLocalMapper->InsertKeyFrame(pKFcur);mCurrentFrame.SetPose(pKFcur->GetPose());mnLastKeyFrameId=mCurrentFrame.mnId;mpLastKeyFrame = pKFcur;mvpLocalKeyFrames.push_back(pKFcur);mvpLocalKeyFrames.push_back(pKFini);mvpLocalMapPoints=mpMap->GetAllMapPoints();mpReferenceKF = pKFcur;mCurrentFrame.mpReferenceKF = pKFcur;mLastFrame = Frame(mCurrentFrame);mpMap->SetReferenceMapPoints(mvpLocalMapPoints);mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());mpMap->mvpKeyFrameOrigins.push_back(pKFini);mState=OK;
}