| 53 | |
| 54 | namespace ORB_SLAM3 { |
| 55 | MLPnPsolver::MLPnPsolver(const Frame &F, const vector<MapPoint *> &vpMapPointMatches): |
| 56 | mnInliersi(0), mnIterations(0), mnBestInliers(0), N(0), mpCamera(F.mpCamera){ |
| 57 | mvpMapPointMatches = vpMapPointMatches; |
| 58 | mvBearingVecs.reserve(F.mvpMapPoints.size()); |
| 59 | mvP2D.reserve(F.mvpMapPoints.size()); |
| 60 | mvSigma2.reserve(F.mvpMapPoints.size()); |
| 61 | mvP3Dw.reserve(F.mvpMapPoints.size()); |
| 62 | mvKeyPointIndices.reserve(F.mvpMapPoints.size()); |
| 63 | mvAllIndices.reserve(F.mvpMapPoints.size()); |
| 64 | |
| 65 | int idx = 0; |
| 66 | for(size_t i = 0, iend = mvpMapPointMatches.size(); i < iend; i++){ |
| 67 | MapPoint* pMP = vpMapPointMatches[i]; |
| 68 | |
| 69 | if(pMP){ |
| 70 | if(!pMP -> isBad()){ |
| 71 | if(i >= F.mvKeysUn.size()) continue; |
| 72 | const cv::KeyPoint &kp = F.mvKeysUn[i]; |
| 73 | |
| 74 | mvP2D.push_back(kp.pt); |
| 75 | mvSigma2.push_back(F.mvLevelSigma2[kp.octave]); |
| 76 | |
| 77 | //Bearing vector should be normalized |
| 78 | cv::Point3f cv_br = mpCamera->unproject(kp.pt); |
| 79 | cv_br /= cv_br.z; |
| 80 | bearingVector_t br(cv_br.x,cv_br.y,cv_br.z); |
| 81 | mvBearingVecs.push_back(br); |
| 82 | |
| 83 | //3D coordinates |
| 84 | Eigen::Matrix<float,3,1> posEig = pMP -> GetWorldPos(); |
| 85 | point_t pos(posEig(0),posEig(1),posEig(2)); |
| 86 | mvP3Dw.push_back(pos); |
| 87 | |
| 88 | mvKeyPointIndices.push_back(i); |
| 89 | mvAllIndices.push_back(idx); |
| 90 | |
| 91 | idx++; |
| 92 | } |
| 93 | } |
| 94 | } |
| 95 | |
| 96 | SetRansacParameters(); |
| 97 | } |
| 98 | |
| 99 | //RANSAC methods |
| 100 | bool MLPnPsolver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers, Eigen::Matrix4f &Tout){ |
nothing calls this directly
no test coverage detected