orbSLAM2 之 Stereo初始化
对于第一个 特征点数量>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