| 473 | } |
| 474 | |
| 475 | bool TwoViewReconstruction::ReconstructF(vector<bool> &vbMatchesInliers, Eigen::Matrix3f &F21, Eigen::Matrix3f &K, |
| 476 | Sophus::SE3f &T21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated) |
| 477 | { |
| 478 | int N=0; |
| 479 | for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++) |
| 480 | if(vbMatchesInliers[i]) |
| 481 | N++; |
| 482 | |
| 483 | // Compute Essential Matrix from Fundamental Matrix |
| 484 | Eigen::Matrix3f E21 = K.transpose() * F21 * K; |
| 485 | |
| 486 | Eigen::Matrix3f R1, R2; |
| 487 | Eigen::Vector3f t; |
| 488 | |
| 489 | // Recover the 4 motion hypotheses |
| 490 | DecomposeE(E21,R1,R2,t); |
| 491 | |
| 492 | Eigen::Vector3f t1 = t; |
| 493 | Eigen::Vector3f t2 = -t; |
| 494 | |
| 495 | // Reconstruct with the 4 hyphoteses and check |
| 496 | vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4; |
| 497 | vector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4; |
| 498 | float parallax1,parallax2, parallax3, parallax4; |
| 499 | |
| 500 | int nGood1 = CheckRT(R1,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D1, 4.0*mSigma2, vbTriangulated1, parallax1); |
| 501 | int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2); |
| 502 | int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3); |
| 503 | int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4); |
| 504 | |
| 505 | int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4))); |
| 506 | |
| 507 | int nMinGood = max(static_cast<int>(0.9*N),minTriangulated); |
| 508 | |
| 509 | int nsimilar = 0; |
| 510 | if(nGood1>0.7*maxGood) |
| 511 | nsimilar++; |
| 512 | if(nGood2>0.7*maxGood) |
| 513 | nsimilar++; |
| 514 | if(nGood3>0.7*maxGood) |
| 515 | nsimilar++; |
| 516 | if(nGood4>0.7*maxGood) |
| 517 | nsimilar++; |
| 518 | |
| 519 | // If there is not a clear winner or not enough triangulated points reject initialization |
| 520 | if(maxGood<nMinGood || nsimilar>1) |
| 521 | { |
| 522 | return false; |
| 523 | } |
| 524 | |
| 525 | // If best reconstruction has enough parallax initialize |
| 526 | if(maxGood==nGood1) |
| 527 | { |
| 528 | if(parallax1>minParallax) |
| 529 | { |
| 530 | vP3D = vP3D1; |
| 531 | vbTriangulated = vbTriangulated1; |
| 532 | |