| 3341 | } |
| 3342 | |
| 3343 | void Tracking::SearchLocalPoints() |
| 3344 | { |
| 3345 | // Do not search map points already matched |
| 3346 | for(vector<MapPoint*>::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++) |
| 3347 | { |
| 3348 | MapPoint* pMP = *vit; |
| 3349 | if(pMP) |
| 3350 | { |
| 3351 | if(pMP->isBad()) |
| 3352 | { |
| 3353 | *vit = static_cast<MapPoint*>(NULL); |
| 3354 | } |
| 3355 | else |
| 3356 | { |
| 3357 | pMP->IncreaseVisible(); |
| 3358 | pMP->mnLastFrameSeen = mCurrentFrame.mnId; |
| 3359 | pMP->mbTrackInView = false; |
| 3360 | pMP->mbTrackInViewR = false; |
| 3361 | } |
| 3362 | } |
| 3363 | } |
| 3364 | |
| 3365 | int nToMatch=0; |
| 3366 | |
| 3367 | // Project points in frame and check its visibility |
| 3368 | for(vector<MapPoint*>::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++) |
| 3369 | { |
| 3370 | MapPoint* pMP = *vit; |
| 3371 | |
| 3372 | if(pMP->mnLastFrameSeen == mCurrentFrame.mnId) |
| 3373 | continue; |
| 3374 | if(pMP->isBad()) |
| 3375 | continue; |
| 3376 | // Project (this fills MapPoint variables for matching) |
| 3377 | if(mCurrentFrame.isInFrustum(pMP,0.5)) |
| 3378 | { |
| 3379 | pMP->IncreaseVisible(); |
| 3380 | nToMatch++; |
| 3381 | } |
| 3382 | if(pMP->mbTrackInView) |
| 3383 | { |
| 3384 | mCurrentFrame.mmProjectPoints[pMP->mnId] = cv::Point2f(pMP->mTrackProjX, pMP->mTrackProjY); |
| 3385 | } |
| 3386 | } |
| 3387 | |
| 3388 | if(nToMatch>0) |
| 3389 | { |
| 3390 | ORBmatcher matcher(0.8); |
| 3391 | int th = 1; |
| 3392 | if(mSensor==System::RGBD || mSensor==System::IMU_RGBD) |
| 3393 | th=3; |
| 3394 | if(mpAtlas->isImuInitialized()) |
| 3395 | { |
| 3396 | if(mpAtlas->GetCurrentMap()->GetIniertialBA2()) |
| 3397 | th=2; |
| 3398 | else |
| 3399 | th=6; |
| 3400 | } |
nothing calls this directly
no test coverage detected