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