DynaSLAM代码详解(5) — Tracking.cc跟踪线程

目录

5.1 DynaSLAM中Tracking线程简介

5 .2 RGBD模式下跟踪流程

5.3 DynaSLAM的低成本跟踪

(1) Tracking::LightTrack() 低成本跟踪函数

(2) Tracking::LightTrackWithMotionModel() 低成本的恒速模型跟踪流程

5.4 DynaSLAM的正常跟踪

文章着重将与ORB-SLAM2不同的地方，关于Tracking中具体跟踪过程参考ORB-SLAM2方面的博文

5.1 DynaSLAM中Tracking线程简介

        DynaSLAM的跟踪线程与ORB-SLAM2的跟踪线程类似，负责估计运动信息、跟踪局部地图。
总体来说，ORB-SLAM2跟踪部分主要包括两个阶段，第一个阶段包括三种跟踪方法：用参考关键帧来跟踪、恒速模型跟踪、重定位跟踪，它们的目的是保证能够“跟的上”，但估计出来的位姿可能没那么准确。第二个阶段是局部地图跟踪，将当前帧的局部关键帧对应的局部地图点投影到该帧，得到更多的特征点匹配关系，对第一阶段的位姿再次优化得到相对准确的位姿。

与ORB-SLAM不同：DynaSLAM会有一个Low-Cost Tracking。

为什么会有低成本的跟踪模式：在RGB-D情况下，在获得了动态物体分割的结果后，有必要了解相机位姿，为此，我们实现了一个低成本的跟踪模块，用于在已创建的场景地图中定位相机。

5 .2 RGBD模式下跟踪流程

跟踪线程的流程以RGBD模式为例，输入RGB或RGBA图像、深度图、动态物体分割Mask；首先将图像转为mImGray和imDepth；然后获取深度相机的真实Depth；初始化mCurrentFrame并进行低成本tracking过程；初始化mCurrentFrame进行正常tracking过程；最后输出世界坐标系到该帧相机坐标系的变换矩阵。

与ORB-SLAM不同：DynaSLAM会有一个低成本的跟踪LightTrack()

// 输入RGB或RGBA图像、深度图、动态物体分割Mask
// 1、将图像转为mImGray和imDepth
// 2、获取深度相机的真实Depth
// 3、初始化mCurrentFrame后进行低成本tracking过程
// 4、初始化mCurrentFrame后进行正常tracking过程
// 输出世界坐标系到该帧相机坐标系的变换矩阵
cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, cv::Mat &mask,const double &timestamp, cv::Mat &imRGBOut,cv::Mat &imDOut, cv::Mat &maskOut)
{mImGray = imRGB;cv::Mat imDepth = imD;cv::Mat imMask = mask;cv::Mat _imRGB = imRGB;// step1：将RGB或RGBA图像转为灰度图像if(mImGray.channels()==3){if(mbRGB)cvtColor(mImGray,mImGray,CV_RGB2GRAY);elsecvtColor(mImGray,mImGray,CV_BGR2GRAY);}else if(mImGray.channels()==4){if(mbRGB)cvtColor(mImGray,mImGray,CV_RGBA2GRAY);elsecvtColor(mImGray,mImGray,CV_BGRA2GRAY);}// step2：将深度相机的disparity转为Depth , 也就是转换成为真正尺度下的深度if((fabs(mDepthMapFactor-1.0f)>1e-5) || imDepth.type()!=CV_32F)imDepth.convertTo(imDepth,      //输出图像CV_32F,       //输出图像的数据类型mDepthMapFactor);     //缩放系数// step3：构造Frame用于低成本跟踪mCurrentFrame = Frame(mImGray,            //灰度图像imDepth,            //深度图像imMask,             //动态物体检测Mask_imRGB,             //RGB图像timestamp,          //时间戳mpORBextractorLeft, //ORB特征提取器mpORBVocabulary,    //词典mK,                 //相机内参矩阵mDistCoef,          //相机的去畸变参数mbf,                //相机基线*相机焦距mThDepth);          //内外点区分深度阈值// 低成本跟踪LightTrack();//将低成本跟踪的结果赋值给imRGBOutimRGBOut = _imRGB;//！ 如果低成本跟踪求解的位姿不可用，则利用多视图几何的方法if (!mCurrentFrame.mTcw.empty()){mGeometry.GeometricModelCorrection(mCurrentFrame,imDepth,imMask);}// step4：构造Frame用于正常跟踪mCurrentFrame = Frame(mImGray,imDepth,imMask,imRGBOut,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);// 正常跟踪Track();if (!mCurrentFrame.mTcw.empty()){mGeometry.InpaintFrames(mCurrentFrame, mImGray, imDepth, imRGBOut, imMask);}mGeometry.GeometricModelUpdateDB(mCurrentFrame);// step5：输出跟踪结果imDOut = imDepth;imDepth.convertTo(imDOut,CV_16U,1./mDepthMapFactor);maskOut = imMask;return mCurrentFrame.mTcw.clone();
}

5.3 DynaSLAM的低成本跟踪

(1) Tracking::LightTrack() 低成本跟踪函数

低成本跟踪的整体流程：

与正常跟踪的对比：低成本跟踪相当于是简化版的跟踪流程，没有仅定位模式跟踪和局部地图跟踪的流程。

void Tracking::LightTrack()
{// Get Map Mutex -> Map cannot be changedunique_lock<mutex> lock(mpMap->mMutexMapUpdate);// 是否使用恒速模型跟踪bool useMotionModel = true; //set true// 如果跟踪线程没有被初始化则退出if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET){cout << "Light Tracking not working because Tracking is not initialized..." << endl;return;}// 系统已初始化，开始进行跟踪状态的判断else{// System is initialized. Track Frame.bool bOK;{// Localization Mode:// 如果当前状态为LOST，则进行重定位跟踪if(mState==LOST){bOK = Relocalization(1);}else{// 如果上一帧在地图中跟踪到足够多的地图点if(!mbVO){// In last frame we tracked enough MapPoints in the map// 且mVelocity不为空且useMotionModel为true,进行低成本的跟踪if(!mVelocity.empty() && useMotionModel){bool _bOK = false;bOK = LightTrackWithMotionModel(_bOK);// TODO: check out!!!}// 否则进行参考关键帧跟踪else{bOK = TrackReferenceKeyFrame();}}// mbVO为true，表明此帧匹配了很少的地图点，既做跟踪又做重定位else{// In last frame we tracked mainly "visual odometry" points.// We compute two camera poses, one from motion model and one doing relocalization.// If relocalization is sucessfull we choose that solution, otherwise we retain// the "visual odometry" solution.// 通过恒速模型进行跟踪的结果bool bOKMM = false;// 通过重定位方法来跟踪的结果bool bOKReloc = false;// 恒速模型中构造的地图点vector<MapPoint*> vpMPsMM;// 在追踪运动模型后发现的外点vector<bool> vbOutMM;// 运动模型得到的位姿cv::Mat TcwMM;// 低成本的跟踪的结果bool lightTracking = false;bool bVO = false;// 且mVelocity不为空且useMotionModel为true,进行低成本的跟踪if(!mVelocity.empty() && useMotionModel){lightTracking = true;//进行低成本的恒速跟踪bOKMM = LightTrackWithMotionModel(bVO); // TODO: check out!!// 将恒速模型跟踪结果暂存到这几个变量中，因为后面重定位会改变这些变量vpMPsMM = mCurrentFrame.mvpMapPoints;vbOutMM = mCurrentFrame.mvbOutlier;TcwMM = mCurrentFrame.mTcw.clone();}// 使用重定位的方法来得到当前帧的位姿bOKReloc = Relocalization(1);//根据前面的恒速模型、重定位结果来更新状态if(bOKMM && !bOKReloc){恒速模型成功、重定位失败，重新使用之前暂存的恒速模型结果mCurrentFrame.SetPose(TcwMM);mCurrentFrame.mvpMapPoints = vpMPsMM;mCurrentFrame.mvbOutlier = vbOutMM;if((lightTracking && bVO) || (!lightTracking && mbVO)){// 更新当前帧的地图点被观测次数for(int i =0; i<mCurrentFrame.N; i++){//如果这个特征点形成了地图点,并且也不是外点的时候if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i]){//增加能观测到该地图点的帧数mCurrentFrame.mvpMapPoints[i]->IncreaseFound();}}}}//有一个成功我们就认为执行成功了bOK = bOKReloc || bOKMM;}}}// 将当前帧的参考关键帧指针设置为mpReferenceKFmCurrentFrame.mpReferenceKF = mpReferenceKF;if(!bOK){//地图中的关键帧数量小于等于5，则低成本跟踪失败if(mpMap->KeyFramesInMap()<=5){cout << "Light Tracking not working..." << endl;return;}}// 如果当前帧的参考关键帧指不存在，重新设置mCurrentFrame.mpReferenceKF = mpReferenceKF;}
}

(2) Tracking::LightTrackWithMotionModel() 低成本的恒速模型跟踪流程

Tracking::LightTrackWithMotionModel()函数的作用是用上一帧地图点来对当前帧进行跟踪。

  1. 更新上一帧的位姿，目的是为了得到更好的地图点位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点。
  2. 根据之前估计的速度，用恒速模型得到当前帧的初始位姿，恒速模型利用上一帧的位姿作为当前帧的位姿的初始值。
  3. 用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次
  4. 利用3D-2D投影关系，优化当前帧位姿 ，上一帧的位姿只是初始值，优化后更加精确
  5. 剔除地图点中外点
  6. 最后判断，如果匹配数大于20，认为跟踪成功，返回true；在正常跟踪下匹配数设置为10。

与正常恒速模型跟踪的对比：低成本的恒速模型跟踪和正常恒速模型跟踪基本相同，代码存在冗余；低成本的恒速模型跟踪比正常恒速模型跟踪会多一个仅定位模式的判断。

/*** @brief 根据恒定速度模型用上一帧地图点来对当前帧进行跟踪* Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点* Step 2：根据之前估计的速度，用恒速模型得到当前帧的初始位姿* Step 3：用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次* Step 4：利用3D-2D投影关系，优化当前帧位姿 * Step 5：剔除地图点中外点* @return 如果匹配数大于20，认为跟踪成功，返回true*/
bool Tracking::LightTrackWithMotionModel(bool &bVO)
{// 最小距离 < 0.9 * 次小距离 匹配成功，检查旋转ORBmatcher matcher(0.9,true);// Update last frame pose according to its reference keyframe// Create "visual odometry" points if in Localization ModeFrame lastFrameBU = mLastFrame;list<MapPoint*> lpTemporalPointsBU = mlpTemporalPoints;// Step 1：更新上一帧的位姿；对于双目或RGB-D相机，还会根据深度值生成临时地图点UpdateLastFrame(); //TODO: check out!// Step 2：根据之前估计的速度，用恒速模型得到当前帧的初始位姿mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);// 清空当前帧的地图点fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL)); //TODO:Checkout// Project points seen in previous frame// 设置特征匹配过程中的搜索半径int th;if(mSensor!=System::STEREO)th=15;    //单目elseth=7;     //双目// Step 3：用上一帧地图点进行投影匹配，如果匹配点不够，则扩大搜索半径再来一次int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);//TODO:Checkout// If few matches, uses a wider window search// 如果匹配点太少，则扩大搜索半径再来一次if(nmatches<20){fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));//TODO:Checkoutnmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR);//TODO:Checkout}// 如果还是不能够获得足够的匹配点,那么就认为跟踪失败if(nmatches<20)return false;// Optimize frame pose with all matches// Step 4：利用3D-2D投影关系，优化当前帧位姿Optimizer::PoseOptimization(&mCurrentFrame);// Discard outliers// Step 5：剔除地图点中外点int nmatchesMap = 0;for(int i =0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i]){if(mCurrentFrame.mvbOutlier[i]){// 如果优化后判断某个地图点是外点，清除它的所有关系MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);mCurrentFrame.mvbOutlier[i]=false;pMP->mbTrackInView = false;pMP->mnLastFrameSeen = mCurrentFrame.mnId;nmatches--;}else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)// 累加成功匹配到的地图点数目nmatchesMap++;}}mLastFrame = lastFrameBU;mlpTemporalPoints = lpTemporalPointsBU;// 如果成功追踪的地图点非常少, 那么这里的bVO标志就会置位bVO = nmatchesMap<10;// Step 6：匹配超过20个点就认为跟踪成功return nmatches>20;}

5.4 DynaSLAM的正常跟踪

        DynaSLAM的正常跟踪 Tracking::Track() 与ORB-SLAM2的跟踪函数相同，跟踪部分主要包括两个阶段，第一个阶段包括三种跟踪方法：用参考关键帧来跟踪、恒速模型跟踪、重定位跟踪，它们的目的是保证能够“跟的上”，但估计出来的位姿可能没那么准确。第二个阶段是局部地图跟踪，将当前帧的局部关键帧对应的局部地图点投影到该帧，得到更多的特征点匹配关系，对第一阶段的位姿再次优化得到相对准确的位姿。

        关于参考关键帧来跟踪、恒速模型跟踪、重定位跟踪、局部地图跟踪可以参考其他博主的解析。

/** @brief Main tracking function. It is independent of the input sensor.** track包含两部分：估计运动、跟踪局部地图* * Step 1：初始化* Step 2：跟踪* Step 3：记录位姿信息，用于轨迹复现*/
void Tracking::Track()
{// track包含两部分：估计运动、跟踪局部地图// mState为tracking的状态，包括 SYSTME_NOT_READY, NO_IMAGE_YET, NOT_INITIALIZED, OK, LOST// 如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态if(mState==NO_IMAGES_YET){mState = NOT_INITIALIZED;}// mLastProcessedState 存储了Tracking最新的状态，用于FrameDrawer中的绘制mLastProcessedState=mState;// Get Map Mutex -> Map cannot be changed// 地图更新时加锁。保证地图不会发生变化// 疑问:这样子会不会影响地图的实时更新?// 回答：主要耗时在构造帧中特征点的提取和匹配部分,在那个时候地图是没有被上锁的,有足够的时间更新地图unique_lock<mutex> lock(mpMap->mMutexMapUpdate);// Step 1：地图初始化if(mState==NOT_INITIALIZED){if(mSensor==System::STEREO || mSensor==System::RGBD)//双目RGBD相机的初始化共用一个函数StereoInitialization();else//单目初始化MonocularInitialization();//更新帧绘制器中存储的最新状态mpFrameDrawer->Update(this);//这个状态量在上面的初始化函数中被更新if(mState!=OK)return;}else{// System is initialized. Track Frame.// bOK为临时变量，用于表示每个函数是否执行成功bool bOK;// Initial camera pose estimation using motion model or relocalization (if tracking is lost)// mbOnlyTracking等于false表示正常SLAM模式（定位+地图更新），mbOnlyTracking等于true表示仅定位模式// tracking 类构造时默认为false。在viewer中有个开关ActivateLocalizationMode，可以控制是否开启mbOnlyTrackingif(!mbOnlyTracking){// Local Mapping is activated. This is the normal behaviour, unless// you explicitly activate the "only tracking" mode.// Step 2：跟踪进入正常SLAM模式，有地图更新// 是否正常跟踪if(mState==OK){// Local Mapping might have changed some MapPoints tracked in last frame// Step 2.1 检查并更新上一帧被替换的MapPoints// 局部建图线程则可能会对原有的地图点进行替换.在这里进行检查CheckReplacedInLastFrame();// Step 2.2 运动模型是空的或刚完成重定位，跟踪参考关键帧；否则恒速模型跟踪// 第一个条件,如果运动模型为空,说明是刚初始化开始，或者已经跟丢了// 第二个条件,如果当前帧紧紧地跟着在重定位的帧的后面，我们将重定位帧来恢复位姿// mnLastRelocFrameId 上一次重定位的那一帧if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2){// 用最近的关键帧来跟踪当前的普通帧// 通过BoW的方式在参考帧中找当前帧特征点的匹配点// 优化每个特征点都对应3D点重投影误差即可得到位姿bOK = TrackReferenceKeyFrame();}else{// 用最近的普通帧来跟踪当前的普通帧// 根据恒速模型设定当前帧的初始位姿// 通过投影的方式在参考帧中找当前帧特征点的匹配点// 优化每个特征点所对应3D点的投影误差即可得到位姿bOK = TrackWithMotionModel();if(!bOK)//根据恒速模型失败了，只能根据参考关键帧来跟踪bOK = TrackReferenceKeyFrame();}}else{// 如果跟踪状态不成功,那么就只能重定位了// BOW搜索，EPnP求解位姿bOK = Relocalization();}}else        {// Localization Mode: Local Mapping is deactivated// Step 2：只进行跟踪tracking，局部地图不工作if(mState==LOST){// Step 2.1 如果跟丢了，只能重定位bOK = Relocalization();}else    {// mbVO是mbOnlyTracking为true时的才有的一个变量// mbVO为false表示此帧匹配了很多的MapPoints，跟踪很正常 (注意有点反直觉)// mbVO为true表明此帧匹配了很少的MapPoints，少于10个，要跪的节奏if(!mbVO){// Step 2.2 如果跟踪正常，使用恒速模型 或 参考关键帧跟踪// In last frame we tracked enough MapPoints in the mapif(!mVelocity.empty()){bOK = TrackWithMotionModel();// ? 为了和前面模式统一，这个地方是不是应该加上// if(!bOK)//    bOK = TrackReferenceKeyFrame();}else{// 如果恒速模型不被满足,那么就只能够通过参考关键帧来定位bOK = TrackReferenceKeyFrame();}}else{// In last frame we tracked mainly "visual odometry" points.// We compute two camera poses, one from motion model and one doing relocalization.// If relocalization is sucessfull we choose that solution, otherwise we retain// the "visual odometry" solution.// mbVO为true，表明此帧匹配了很少（小于10）的地图点，要跪的节奏，既做跟踪又做重定位//MM=Motion Model,通过运动模型进行跟踪的结果bool bOKMM = false;//通过重定位方法来跟踪的结果bool bOKReloc = false;//运动模型中构造的地图点vector<MapPoint*> vpMPsMM;//在追踪运动模型后发现的外点vector<bool> vbOutMM;//运动模型得到的位姿cv::Mat TcwMM;// Step 2.3 当运动模型有效的时候,根据运动模型计算位姿if(!mVelocity.empty()){bOKMM = TrackWithMotionModel();// 将恒速模型跟踪结果暂存到这几个变量中，因为后面重定位会改变这些变量vpMPsMM = mCurrentFrame.mvpMapPoints;vbOutMM = mCurrentFrame.mvbOutlier;TcwMM = mCurrentFrame.mTcw.clone();}// Step 2.4 使用重定位的方法来得到当前帧的位姿bOKReloc = Relocalization();// Step 2.5 根据前面的恒速模型、重定位结果来更新状态if(bOKMM && !bOKReloc){// 恒速模型成功、重定位失败，重新使用之前暂存的恒速模型结果mCurrentFrame.SetPose(TcwMM);mCurrentFrame.mvpMapPoints = vpMPsMM;mCurrentFrame.mvbOutlier = vbOutMM;//? 疑似bug！这段代码是不是重复增加了观测次数？后面 TrackLocalMap 函数中会有这些操作// 如果当前帧匹配的3D点很少，增加当前可视地图点的被观测次数if(mbVO){// 更新当前帧的地图点被观测次数for(int i =0; i<mCurrentFrame.N; i++){//如果这个特征点形成了地图点,并且也不是外点的时候if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i]){//增加能观测到该地图点的帧数mCurrentFrame.mvpMapPoints[i]->IncreaseFound();}}}}else if(bOKReloc){// 只要重定位成功整个跟踪过程正常进行（重定位与跟踪，更相信重定位）mbVO = false;}//有一个成功我们就认为执行成功了bOK = bOKReloc || bOKMM;}}}// 将最新的关键帧作为当前帧的参考关键帧mCurrentFrame.mpReferenceKF = mpReferenceKF;// If we have an initial estimation of the camera pose and matching. Track the local map.// Step 3：在跟踪得到当前帧初始姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿// 前面只是跟踪一帧得到初始位姿，这里搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化if(!mbOnlyTracking){if(bOK)bOK = TrackLocalMap();}else{// mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve// a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes// the camera we will use the local map again.// 重定位成功if(bOK && !mbVO)bOK = TrackLocalMap();}//根据上面的操作来判断是否追踪成功if(bOK)mState = OK;elsemState=LOST;// Step 4：更新显示线程中的图像、特征点、地图点等信息mpFrameDrawer->Update(this);// If tracking were good, check if we insert a keyframe//只有在成功追踪时才考虑生成关键帧的问题if(bOK){// Update motion model// Step 5：跟踪成功，更新恒速运动模型if(!mLastFrame.mTcw.empty()){// 更新恒速运动模型 TrackWithMotionModel 中的mVelocitycv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));// mVelocity = Tcl = Tcw * Twl,表示上一帧到当前帧的变换， 其中 Twl = LastTwcmVelocity = mCurrentFrame.mTcw*LastTwc; }else//否则速度为空mVelocity = cv::Mat();//更新显示中的位姿mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);// Clean VO matches// Step 6：清除观测不到的地图点   for(int i=0; i<mCurrentFrame.N; i++){MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];if(pMP)if(pMP->Observations()<1){mCurrentFrame.mvbOutlier[i] = false;mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);}}// Delete temporal MapPoints// Step 7：清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd）// 步骤6中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除// 临时地图点仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend =  mlpTemporalPoints.end(); lit!=lend; lit++){MapPoint* pMP = *lit;delete pMP;}// 这里不仅仅是清除mlpTemporalPoints，通过delete pMP还删除了指针指向的MapPoint// 不能够直接执行这个是因为其中存储的都是指针,之前的操作都是为了避免内存泄露mlpTemporalPoints.clear();// Check if we need to insert a new keyframe// Step 8：检测并插入关键帧，对于双目或RGB-D会产生新的地图点if(NeedNewKeyFrame())CreateNewKeyFrame();// We allow points with high innovation (considererd outliers by the Huber Function)// pass to the new keyframe, so that bundle adjustment will finally decide// if they are outliers or not. We don't want next frame to estimate its position// with those points so we discard them in the frame.// 作者这里说允许在BA中被Huber核函数判断为外点的传入新的关键帧中，让后续的BA来审判他们是不是真正的外点// 但是估计下一帧位姿的时候我们不想用这些外点，所以删掉//  Step 9 删除那些在bundle adjustment中检测为outlier的地图点for(int i=0; i<mCurrentFrame.N;i++){// 这里第一个条件还要执行判断是因为, 前面的操作中可能删除了其中的地图点if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);}}// Reset if the camera get lost soon after initialization// Step 10 如果初始化后不久就跟踪失败，并且relocation也没有搞定，只能重新Resetif(mState==LOST){//如果地图中的关键帧信息过少的话,直接重新进行初始化了if(mpMap->KeyFramesInMap()<=5){cout << "Track lost soon after initialisation, reseting..." << endl;mpSystem->Reset();return;}}//确保已经设置了参考关键帧if(!mCurrentFrame.mpReferenceKF)mCurrentFrame.mpReferenceKF = mpReferenceKF;// 保存上一帧的数据,当前帧变上一帧mLastFrame = Frame(mCurrentFrame);}// Store frame pose information to retrieve the complete camera trajectory afterwards.// Step 11：记录位姿信息，用于最后保存所有的轨迹if(!mCurrentFrame.mTcw.empty()){// 计算相对姿态Tcr = Tcw * Twr, Twr = Trw^-1cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();//保存各种状态mlRelativeFramePoses.push_back(Tcr);mlpReferences.push_back(mpReferenceKF);mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);mlbLost.push_back(mState==LOST);}else{// This can happen if tracking is lost// 如果跟踪失败，则相对位姿使用上一次值mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());mlpReferences.push_back(mlpReferences.back());mlFrameTimes.push_back(mlFrameTimes.back());mlbLost.push_back(mState==LOST);}}// Tracking

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.hqwc.cn/news/22278.html

如若内容造成侵权/违法违规/事实不符，请联系编程知识网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

SpringBoot2+Vue2实战(十八)修改密码

SpringBoot2+Vue2实战(十八)修改密码

一、修改密码&#xff1a; Header.vue <el-dropdown-item style"font-size: 14px; padding: 5px 0"><router-link to"/password" style"text-decoration: none">修改密码</router-link></el-dropdown-item> router/i…
阅读更多...
Kafka 深度剖析

Kafka 深度剖析

1、应用场景 1.1 kafka场景 Kafka最初是由LinkedIn公司采用Scala语言开发&#xff0c;基于ZooKeeper&#xff0c;现在已经捐献给了Apache基金会。目前Kafka已经定位为一个分布式流式处理平台&#xff0c;它以 高吞吐、可持久化、可水平扩展、支持流处理等多种特性而被广泛应用…
阅读更多...
牛客HJ99 - 自守数【暴力 + 换位取模】

牛客HJ99 - 自守数【暴力 + 换位取模】

原题传送门 原题描述 首先我们来看一下原题是怎么描述的&#xff0c;题面很简单&#xff0c;输入n&#xff0c;然后让我们去统计从1 ~ n之间的自守数有几个&#xff0c;那什么是【自守数】呢&#xff0c;上面也说到了&#xff0c;即一个数在平方之后该数的尾数等于该数自身的自…
阅读更多...
ES6新特性基础总结

ES6新特性基础总结

目录 简介 node.js安装 babel转码器 let命令 const命令 解构对象 字符串扩展 新增方法&#xff1a; 数组扩展 扩展运算符 伪数组 对象扩展 属性的简介表示法 属性名表达式 对象的扩展运算符 函数扩展 箭头函数 使用注意 Set数据结构 基本用法 size属性 特有方法 P…
阅读更多...
用于分析脉冲类信号的二阶瞬态提取变换研究（Matlab代码实现）

用于分析脉冲类信号的二阶瞬态提取变换研究（Matlab代码实现）

&#x1f4a5;&#x1f4a5;&#x1f49e;&#x1f49e;欢迎来到本博客❤️❤️&#x1f4a5;&#x1f4a5; &#x1f3c6;博主优势&#xff1a;&#x1f31e;&#x1f31e;&#x1f31e;博客内容尽量做到思维缜密&#xff0c;逻辑清晰&#xff0c;为了方便读者。 ⛳️座右铭&a…
阅读更多...
RabbitMQ知识掌握 【进阶篇】

RabbitMQ知识掌握 【进阶篇】

一、如何保证消息的可靠性 &#x1f349; 1.保证消息的可靠性投递 &#x1f95d; 在生产环境中由于一些不明原因&#xff0c;导致 rabbitmq 重启&#xff0c;在 RabbitMQ 重启期间生产者消息投递失败&#xff0c;导致消息丢失&#xff0c;需要手动处理和恢复。于是&#xff0…
阅读更多...
微软亚洲研究院推出AI编译器界“工业重金属四部曲”

微软亚洲研究院推出AI编译器界“工业重金属四部曲”

编者按&#xff1a;编译器在传统计算科学中一直是一个重要的研究课题。在人工智能技术快速发展和广泛应用的今天&#xff0c;人工智能模型需要部署在多样化的计算机硬件架构上。同时&#xff0c;训练和部署大型人工智能模型时又对硬件性能有着更高的要求&#xff0c;有时还需根…
阅读更多...
7.10蓝桥杯刷题

7.10蓝桥杯刷题

public class _求阶乘和 {public static void main(String[] args) {// 根据已有的知识 可以知道的是&#xff0c;现在要求s的末尾九位数字&#xff0c;已知的是39之后的阶乘他的后九位都是0;//所以不需要计算到2023的阶乘//一个数求出来的阶乘想要末尾有0//数中必须要有2和5&a…
阅读更多...
高压放大器到底有什么作用

高压放大器到底有什么作用

高压放大器是一种重要的电子元器件&#xff0c;其作用是将信号放大到更高的电压水平&#xff0c;以便供给需要高电压的负载使用。高压放大器被广泛应用于通讯设备、医疗仪器、仿真模拟、气体激光、光学器件等领域。下面安泰电子将详细介绍高压放大器的作用以及其在各领域中的应…
阅读更多...
VUE2基础-Vue实例

VUE2基础-Vue实例

Vue 实例 创建一个 Vue 实例 每个 Vue 应用都是通过用 Vue 函数创建一个新的 Vue 实例开始的&#xff1a; var vm new Vue({// 选项 }) 虽然没有完全遵循 MVVM 模型&#xff0c;但是 Vue 的设计也受到了它的启发。因此在文档中经常会使用 vm (ViewModel 的缩写) 这个变量名…
阅读更多...
抖音seo矩阵系统源码搭建技术+二开开源代码定制部署

抖音seo矩阵系统源码搭建技术+二开开源代码定制部署

抖音SEO源码是指将抖音平台上的视频资源进行筛选、排序等操作&#xff0c;进而提升其在搜索排名中的权重&#xff0c;从而让更多的用户能够发现并观看到这些视频资源。而抖音SEO矩阵系统源码则是指通过建立一个分析系统&#xff0c;分析抖音中的用户、视频、标签等数据&#xf…
阅读更多...
Spring 核心与设计思想

Spring 核心与设计思想

文章目录 1.Spring 是什么&#xff1f;2.什么是容器3.什么是IoC3.1 传统程序开发3.2 IoC程序开发3.3 IoC再理解 4.认识DI 1.Spring 是什么&#xff1f; Spring框架是一个轻量级的企业开发的一站式解决方案&#xff0c;可以基于Spring解决Java EE 开发中的所有问题。 ⽤⼀句大白…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023