| 33 | |
| 34 | |
| 35 | Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector<MapPoint *> &vpMatched12, const bool bFixScale, |
| 36 | vector<KeyFrame*> vpKeyFrameMatchedMP): |
| 37 | mnIterations(0), mnBestInliers(0), mbFixScale(bFixScale), |
| 38 | pCamera1(pKF1->mpCamera), pCamera2(pKF2->mpCamera) |
| 39 | { |
| 40 | bool bDifferentKFs = false; |
| 41 | if(vpKeyFrameMatchedMP.empty()) |
| 42 | { |
| 43 | bDifferentKFs = true; |
| 44 | vpKeyFrameMatchedMP = vector<KeyFrame*>(vpMatched12.size(), pKF2); |
| 45 | } |
| 46 | |
| 47 | mpKF1 = pKF1; |
| 48 | mpKF2 = pKF2; |
| 49 | |
| 50 | vector<MapPoint*> vpKeyFrameMP1 = pKF1->GetMapPointMatches(); |
| 51 | |
| 52 | mN1 = vpMatched12.size(); |
| 53 | |
| 54 | mvpMapPoints1.reserve(mN1); |
| 55 | mvpMapPoints2.reserve(mN1); |
| 56 | mvpMatches12 = vpMatched12; |
| 57 | mvnIndices1.reserve(mN1); |
| 58 | mvX3Dc1.reserve(mN1); |
| 59 | mvX3Dc2.reserve(mN1); |
| 60 | |
| 61 | Eigen::Matrix3f Rcw1 = pKF1->GetRotation(); |
| 62 | Eigen::Vector3f tcw1 = pKF1->GetTranslation(); |
| 63 | Eigen::Matrix3f Rcw2 = pKF2->GetRotation(); |
| 64 | Eigen::Vector3f tcw2 = pKF2->GetTranslation(); |
| 65 | |
| 66 | mvAllIndices.reserve(mN1); |
| 67 | |
| 68 | size_t idx=0; |
| 69 | |
| 70 | KeyFrame* pKFm = pKF2; //Default variable |
| 71 | for(int i1=0; i1<mN1; i1++) |
| 72 | { |
| 73 | if(vpMatched12[i1]) |
| 74 | { |
| 75 | MapPoint* pMP1 = vpKeyFrameMP1[i1]; |
| 76 | MapPoint* pMP2 = vpMatched12[i1]; |
| 77 | |
| 78 | if(!pMP1) |
| 79 | continue; |
| 80 | |
| 81 | if(pMP1->isBad() || pMP2->isBad()) |
| 82 | continue; |
| 83 | |
| 84 | if(bDifferentKFs) |
| 85 | pKFm = vpKeyFrameMatchedMP[i1]; |
| 86 | |
| 87 | int indexKF1 = get<0>(pMP1->GetIndexInKeyFrame(pKF1)); |
| 88 | int indexKF2 = get<0>(pMP2->GetIndexInKeyFrame(pKFm)); |
| 89 | |
| 90 | if(indexKF1<0 || indexKF2<0) |
| 91 | continue; |
| 92 |
nothing calls this directly
no test coverage detected