对于第一个 特征点数量>500 的帧(Frame)进行初始化。 将其设置为世界坐标系。 以该帧为源帧生成关键帧(KeyFrame)，并将该关键帧添加到地图(Map)。 由该帧中所有具有深度的特征生成地图点(MapPoint)，关联地图点、关键帧和当前帧。 将关键帧添加到局部地图(LocalMapping)。 保存Tracking类的成员变量：
//Local mpLocalMapper->InsertKeyFrame(pKFini); //将关键帧保存至mpLocalMapper的mlNewKeyFrames,并令mplocalmapper的mbAbortBA=true; //Last mLastFrame = Frame(mCurrentFrame); mnLastKeyFrameId=mCurrentFrame.mnId;//关键帧的源帧id mpLastKeyFrame = pKFini; //Local mvpLocalKeyFrames.push_back(pKFini); mvpLocalMapPoints=mpMap->GetAllMapPoints(); //将mvLocalMapPoints赋值为mpMap的mspMapPoints //Referecnce mpReferenceKF = pKFini; //参考关键帧指向新加入的关键帧 mCurrentFrame.mpReferenceKF = pKFini; //当前帧的参考关键帧指向新加入的关键帧 //Map mpMap->SetReferenceMapPoints(mvpLocalMapPoints); // 令mpMap的mvpReferenceMapPoints = mvpLocalMapPoints;格式vector<MapPoint*> mpMap->mvpKeyFrameOrigins.push_back(pKFini); //在 mpMap的vector<KeyFrame*> mvpKeyFrameOrigins中保存当前关键帧 //Drawer mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw); //状态 mState=OK; //状态设置为OK