ORB-SLAM2 ---- Tracking::TrackWithMotionModel函数
创始人
2024-03-23 03:23:39
0

目录

1.函数作用

2.步骤 

3.code 

4.函数解释 

4.1 更新上一帧的位姿;对于双目或RGB-D相机,还会根据深度值生成临时地图点

4.2 根据之前估计的速度,用恒速模型得到当前帧的初始位姿。

4.3 用上一帧地图点进行投影匹配,如果匹配点不够,则扩大搜索半径再来一次 

4.4 利用3D-2D投影关系,优化当前帧位姿 

4.5 剔除地图点中外点


1.函数作用

        用最近的普通帧来跟踪当前的普通帧。根据恒速模型设定当前帧的初始位姿,通过投影的方式在参考帧中找当前帧特征点的匹配点,优化每个特征点所对应3D点的投影误差即可得到位姿。

2.步骤 

 * @brief 根据恒定速度模型用上一帧地图点来对当前帧进行跟踪
 * Step 1:更新上一帧的位姿;对于双目或RGB-D相机,还会根据深度值生成临时地图点
 * Step 2:根据上一帧特征点对应地图点进行投影匹配
 * Step 3:优化当前帧位姿
 * Step 4:剔除地图点中外点
 * @return 如果匹配数大于10,认为跟踪成功,返回true

3.code 

bool Tracking::TrackWithMotionModel()
{// 最小距离 < 0.9*次小距离 匹配成功,检查旋转ORBmatcher matcher(0.9,true);// Update last frame pose according to its reference keyframe// Create "visual odometry" points// Step 1:更新上一帧的位姿;对于双目或RGB-D相机,还会根据深度值生成临时地图点UpdateLastFrame();// Step 2:根据之前估计的速度,用恒速模型得到当前帧的初始位姿。mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);// 清空当前帧的地图点fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL));// 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);// If few matches, uses a wider window search// 如果匹配点太少,则扩大搜索半径再来一次if(nmatches<20){fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast(NULL));nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR); // 2*th}// 如果还是不能够获得足够的匹配点,那么就认为跟踪失败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(NULL);mCurrentFrame.mvbOutlier[i]=false;pMP->mbTrackInView = false;pMP->mnLastFrameSeen = mCurrentFrame.mnId;nmatches--;}else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)// 累加成功匹配到的地图点数目nmatchesMap++;}}    if(mbOnlyTracking){// 纯定位模式下:如果成功追踪的地图点非常少,那么这里的mbVO标志就会置位mbVO = nmatchesMap<10;return nmatches>20;}// Step 6:匹配超过10个点就认为跟踪成功return nmatchesMap>=10;
}

4.函数解释 

4.1 更新上一帧的位姿;对于双目或RGB-D相机,还会根据深度值生成临时地图点

ORB-SLAM2 ---- Tracking::UpdateLastFrame函数解析icon-default.png?t=M85Bhttps://blog.csdn.net/qq_41694024/article/details/128192577        通过这一步,我们计算了LastFrame的位姿并且对于双目或RGB-D相机来说,我们新增了一些地图点。

4.2 根据之前估计的速度,用恒速模型得到当前帧的初始位姿。

         当前帧本身是没有位姿的,恒速模型设置当前帧的位姿为速度mVelocity * 上一帧的位姿得到当前帧的初始位姿。

        我们看速度mVelocity 的更新:

                // 更新恒速运动模型 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; }

        它其实是上一帧到当前帧的变换,它不断在更新,因此“上一帧到当前帧的变换”mVelocity (lastFrame的上一帧到lastFrame的变换) * 上一帧的位姿(lastFrame的位姿),即我们假设相同两帧的变化幅度不大,用LastFrame的LastFrame到LastFrame的变换来代替LastFrame到CurrentFrame的变化矩阵,但这仅仅是个初始值。

        随后清空当前帧CurrentFrame的地图点,根据相机类型选择匹配半径。

4.3 用上一帧地图点进行投影匹配,如果匹配点不够,则扩大搜索半径再来一次 

        我们用上一帧地图点进行投影匹配,如果匹配点不够,则扩大搜索半径再来一次。

        如果匹配点太少,则扩大搜索半径再来一次。

        如果还是不能够获得足够的匹配点,那么就认为跟踪失败。

4.4 利用3D-2D投影关系,优化当前帧位姿 

4.5 剔除地图点中外点

        如果我们经过优化后确定当前地图点是外点,则清除它的所有关系:

        ①将当前帧的地地图点mvpMapPoints[i]的内存清空

        ②设置当前地图点的外点的标记mvbOutlier为NULL。属于外点的特征点标记,在 Optimizer::PoseOptimization 使用了。

        ③将标记mbTrackInView设置为false

mbTrackInView==false的点有几种:
 a 已经和当前帧经过匹配(TrackReferenceKeyFrame,TrackWithMotionModel)但在优化过程中认为是外点
b 已经和当前帧经过匹配且为内点,这类点也不需要再进行投影 
c 不在当前相机视野中的点(即未通过isInFrustum判断)

        ④将标记mnLastFrameSeen设置为false

mnLastFrameSeen==mCurrentFrame.mnId的点有几种:
a 已经和当前帧经过匹配(TrackReferenceKeyFrame,TrackWithMotionModel)但在优化过程中认为是外点
b 已经和当前帧经过匹配且为内点,这类点也不需要再进行投影

相关内容

热门资讯

保存时出现了1个错误,导致这篇... 当保存文章时出现错误时,可以通过以下步骤解决问题:查看错误信息:查看错误提示信息可以帮助我们了解具体...
汇川伺服电机位置控制模式参数配... 1. 基本控制参数设置 1)设置位置控制模式   2)绝对值位置线性模...
不能访问光猫的的管理页面 光猫是现代家庭宽带网络的重要组成部分,它可以提供高速稳定的网络连接。但是,有时候我们会遇到不能访问光...
表格中数据未显示 当表格中的数据未显示时,可能是由于以下几个原因导致的:HTML代码问题:检查表格的HTML代码是否正...
本地主机上的图像未显示 问题描述:在本地主机上显示图像时,图像未能正常显示。解决方法:以下是一些可能的解决方法,具体取决于问...
表格列调整大小出现问题 问题描述:表格列调整大小出现问题,无法正常调整列宽。解决方法:检查表格的布局方式是否正确。确保表格使...
不一致的条件格式 要解决不一致的条件格式问题,可以按照以下步骤进行:确定条件格式的规则:首先,需要明确条件格式的规则是...
Android|无法访问或保存... 这个问题可能是由于权限设置不正确导致的。您需要在应用程序清单文件中添加以下代码来请求适当的权限:此外...
【NI Multisim 14...   目录 序言 一、工具栏 🍊1.“标准”工具栏 🍊 2.视图工具...
银河麒麟V10SP1高级服务器... 银河麒麟高级服务器操作系统简介: 银河麒麟高级服务器操作系统V10是针对企业级关键业务...