| 569 | } |
| 570 | |
| 571 | bool TwoViewReconstruction::ReconstructH(vector<bool> &vbMatchesInliers, Eigen::Matrix3f &H21, Eigen::Matrix3f &K, |
| 572 | Sophus::SE3f &T21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated) |
| 573 | { |
| 574 | int N=0; |
| 575 | for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++) |
| 576 | if(vbMatchesInliers[i]) |
| 577 | N++; |
| 578 | |
| 579 | // We recover 8 motion hypotheses using the method of Faugeras et al. |
| 580 | // Motion and structure from motion in a piecewise planar environment. |
| 581 | // International Journal of Pattern Recognition and Artificial Intelligence, 1988 |
| 582 | Eigen::Matrix3f invK = K.inverse(); |
| 583 | Eigen::Matrix3f A = invK * H21 * K; |
| 584 | |
| 585 | Eigen::JacobiSVD<Eigen::Matrix3f> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); |
| 586 | Eigen::Matrix3f U = svd.matrixU(); |
| 587 | Eigen::Matrix3f V = svd.matrixV(); |
| 588 | Eigen::Matrix3f Vt = V.transpose(); |
| 589 | Eigen::Vector3f w = svd.singularValues(); |
| 590 | |
| 591 | float s = U.determinant() * Vt.determinant(); |
| 592 | |
| 593 | float d1 = w(0); |
| 594 | float d2 = w(1); |
| 595 | float d3 = w(2); |
| 596 | |
| 597 | if(d1/d2<1.00001 || d2/d3<1.00001) |
| 598 | { |
| 599 | return false; |
| 600 | } |
| 601 | |
| 602 | vector<Eigen::Matrix3f> vR; |
| 603 | vector<Eigen::Vector3f> vt, vn; |
| 604 | vR.reserve(8); |
| 605 | vt.reserve(8); |
| 606 | vn.reserve(8); |
| 607 | |
| 608 | //n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1 |
| 609 | float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3)); |
| 610 | float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3)); |
| 611 | float x1[] = {aux1,aux1,-aux1,-aux1}; |
| 612 | float x3[] = {aux3,-aux3,aux3,-aux3}; |
| 613 | |
| 614 | //case d'=d2 |
| 615 | float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2); |
| 616 | |
| 617 | float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2); |
| 618 | float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta}; |
| 619 | |
| 620 | for(int i=0; i<4; i++) |
| 621 | { |
| 622 | Eigen::Matrix3f Rp; |
| 623 | Rp.setZero(); |
| 624 | Rp(0,0) = ctheta; |
| 625 | Rp(0,2) = -stheta[i]; |
| 626 | Rp(1,1) = 1.f; |
| 627 | Rp(2,0) = stheta[i]; |
| 628 | Rp(2,2) = ctheta; |