| 3455 | |
| 3456 | |
| 3457 | void Tracking::UpdateLocalKeyFrames() |
| 3458 | { |
| 3459 | // Each map point vote for the keyframes in which it has been observed |
| 3460 | map<KeyFrame*,int> keyframeCounter; |
| 3461 | if(!mpAtlas->isImuInitialized() || (mCurrentFrame.mnId<mnLastRelocFrameId+2)) |
| 3462 | { |
| 3463 | for(int i=0; i<mCurrentFrame.N; i++) |
| 3464 | { |
| 3465 | MapPoint* pMP = mCurrentFrame.mvpMapPoints[i]; |
| 3466 | if(pMP) |
| 3467 | { |
| 3468 | if(!pMP->isBad()) |
| 3469 | { |
| 3470 | const map<KeyFrame*,tuple<int,int>> observations = pMP->GetObservations(); |
| 3471 | for(map<KeyFrame*,tuple<int,int>>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++) |
| 3472 | keyframeCounter[it->first]++; |
| 3473 | } |
| 3474 | else |
| 3475 | { |
| 3476 | mCurrentFrame.mvpMapPoints[i]=NULL; |
| 3477 | } |
| 3478 | } |
| 3479 | } |
| 3480 | } |
| 3481 | else |
| 3482 | { |
| 3483 | for(int i=0; i<mLastFrame.N; i++) |
| 3484 | { |
| 3485 | // Using lastframe since current frame has not matches yet |
| 3486 | if(mLastFrame.mvpMapPoints[i]) |
| 3487 | { |
| 3488 | MapPoint* pMP = mLastFrame.mvpMapPoints[i]; |
| 3489 | if(!pMP) |
| 3490 | continue; |
| 3491 | if(!pMP->isBad()) |
| 3492 | { |
| 3493 | const map<KeyFrame*,tuple<int,int>> observations = pMP->GetObservations(); |
| 3494 | for(map<KeyFrame*,tuple<int,int>>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++) |
| 3495 | keyframeCounter[it->first]++; |
| 3496 | } |
| 3497 | else |
| 3498 | { |
| 3499 | // MODIFICATION |
| 3500 | mLastFrame.mvpMapPoints[i]=NULL; |
| 3501 | } |
| 3502 | } |
| 3503 | } |
| 3504 | } |
| 3505 | |
| 3506 | |
| 3507 | int max=0; |
| 3508 | KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL); |
| 3509 | |
| 3510 | mvpLocalKeyFrames.clear(); |
| 3511 | mvpLocalKeyFrames.reserve(3*keyframeCounter.size()); |
| 3512 | |
| 3513 | // All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points |
| 3514 | for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++) |
nothing calls this directly
no test coverage detected