目录
1.函数作用
2.步骤
3.code
4.函数解释
4.1 利用参考关键帧更新上一帧在世界坐标系下的位姿
4.2 对于双目或rgbd相机,为上一帧生成新的临时地图点
1.函数作用
更新上一帧位姿,在上一帧中生成临时地图点。
单目情况:只计算了上一帧的世界坐标系位姿
双目和rgbd情况:选取有有深度值的并且没有被选为地图点的点生成新的临时地图点,提高跟踪鲁棒性
2.步骤
Step 1:利用参考关键帧更新上一帧在世界坐标系下的位姿
Step 2:对于双目或rgbd相机,为上一帧生成新的临时地图点
Step 2.1:得到上一帧中具有有效深度值的特征点(不一定是地图点)
Step 2.2:从中找出不是地图点的部分
Step 2.3:需要创建的点,包装为地图点。只是为了提高双目和RGBD的跟踪成功率,并没有添加复杂属性,因为后面会扔掉
Step 2.4:如果地图点质量不好,停止创建地图点
3.code
void Tracking::UpdateLastFrame() { // Update pose according to reference keyframe // Step 1:利用参考关键帧更新上一帧在世界坐标系下的位姿 // 上一普通帧的参考关键帧,注意这里用的是参考关键帧(位姿准)而不是上上一帧的普通帧 KeyFrame* pRef = mLastFrame.mpReferenceKF; // ref_keyframe 到 lastframe的位姿变换 cv::Mat Tlr = mlRelativeFramePoses.back(); // 将上一帧的世界坐标系下的位姿计算出来 // l:last, r:reference, w:world // Tlw = Tlr*Trw mLastFrame.SetPose(Tlr*pRef->GetPose()); // 如果上一帧为关键帧,或者单目的情况,则退出 if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR) return; // Step 2:对于双目或rgbd相机,为上一帧生成新的临时地图点 // 注意这些地图点只是用来跟踪,不加入到地图中,跟踪完后会删除 // Create "visual odometry" MapPoints // We sort points according to their measured depth by the stereo/RGB-D sensor // Step 2.1:得到上一帧中具有有效深度值的特征点(不一定是地图点) vector<pair<float,int> > vDepthIdx; vDepthIdx.reserve(mLastFrame.N); for(int i=0; i<mLastFrame.N;i++) { float z = mLastFrame.mvDepth[i]; if(z>0) { // vDepthIdx第一个元素是某个点的深度,第二个元素是对应的特征点id vDepthIdx.push_back(make_pair(z,i)); } } // 如果上一帧中没有有效深度的点,那么就直接退出 if(vDepthIdx.empty()) return; // 按照深度从小到大排序 sort(vDepthIdx.begin(),vDepthIdx.end()); // We insert all close points (depth<mThDepth) // If less than 100 close points, we insert the 100 closest ones. // Step 2.2:从中找出不是地图点的部分 int nPoints = 0; for(size_t j=0; j<vDepthIdx.size();j++) { int i = vDepthIdx[j].second; bool bCreateNew = false; // 如果这个点对应在上一帧中的地图点没有,或者创建后就没有被观测到,那么就生成一个临时的地图点 MapPoint* pMP = mLastFrame.mvpMapPoints[i]; if(!pMP) bCreateNew = true; else if(pMP->Observations()<1) { // 地图点被创建后就没有被观测,认为不靠谱,也需要重新创建 bCreateNew = true; } if(bCreateNew) { // Step 2.3:需要创建的点,包装为地图点。只是为了提高双目和RGBD的跟踪成功率,并没有添加复杂属性,因为后面会扔掉 // 反投影到世界坐标系中 cv::Mat x3D = mLastFrame.UnprojectStereo(i); MapPoint* pNewMP = new MapPoint( x3D, // 世界坐标系坐标 mpMap, // 跟踪的全局地图 &mLastFrame, // 存在这个特征点的帧(上一帧) i); // 特征点id // 加入上一帧的地图点中 mLastFrame.mvpMapPoints[i]=pNewMP; // 标记为临时添加的MapPoint,之后在CreateNewKeyFrame之前会全部删除 mlpTemporalPoints.push_back(pNewMP); nPoints++; } else { // 因为从近到远排序,记录其中不需要创建地图点的个数 nPoints++; } // Step 2.4:如果地图点质量不好,停止创建地图点 // 停止新增临时地图点必须同时满足以下条件: // 1、当前的点的深度已经超过了设定的深度阈值(35倍基线) // 2、nPoints已经超过100个点,说明距离比较远了,可能不准确,停掉退出 if(vDepthIdx[j].first>mThDepth && nPoints>100) break; } }
4.函数解释
4.1 利用参考关键帧更新上一帧在世界坐标系下的位姿
如上图,我们获得上一普通帧mLastFrame的参考关键帧pRef,获得ref_keyframe 到 lastframe的位姿变换Tlr,用Tlw = Tlr*Trw获得lastframe的位姿。
如果上一帧为关键帧,或者单目的情况,则退出。
4.2 对于双目或rgbd相机,为上一帧生成新的临时地图点
注意这些地图点只是用来跟踪,不加入到地图中,跟踪完后会删除。
遍历上一帧LastFrame中具有有效深度值的特征点(不一定是地图点),记录特征点的深度和序号到vDepthIdx容器中。接着按照深度从小到大排序。
遍历具有有效深度值的特征点vDepthIdx,用变量i记录是该特征点LastFrame中的哪个索引,如果这个点:
①对应在上一帧LastFrame中的地图点没有(!pMP)
②或者创建后就没有被观测到(pMP->Observations()<1)
那么就生成一个临时的地图点
将这个特征点反投影到世界坐标系中得到3D点x3D,并包装成地图点pNewMP,加入上一帧LastFrame的地图点mvpMapPoints中,标记为临时添加的MapPointmlpTemporalPoints.,之后在CreateNewKeyFrame之前会全部删除。将标记nPoints++。
如果地图上有地图点我们也需要将nPoints++。
直到满足下列条件我们停止新增LastFrame的地图点:
①当前的点的深度已经超过了设定的深度阈值(35倍基线)(我们之前把地图点的深度从小到大排序)
②nPoints已经超过100个点,说明距离比较远了,可能不准确,停掉退出