| 735 | |
| 736 | |
| 737 | void TwoViewReconstruction::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, Eigen::Matrix3f &T) |
| 738 | { |
| 739 | float meanX = 0; |
| 740 | float meanY = 0; |
| 741 | const int N = vKeys.size(); |
| 742 | |
| 743 | vNormalizedPoints.resize(N); |
| 744 | |
| 745 | for(int i=0; i<N; i++) |
| 746 | { |
| 747 | meanX += vKeys[i].pt.x; |
| 748 | meanY += vKeys[i].pt.y; |
| 749 | } |
| 750 | |
| 751 | meanX = meanX/N; |
| 752 | meanY = meanY/N; |
| 753 | |
| 754 | float meanDevX = 0; |
| 755 | float meanDevY = 0; |
| 756 | |
| 757 | for(int i=0; i<N; i++) |
| 758 | { |
| 759 | vNormalizedPoints[i].x = vKeys[i].pt.x - meanX; |
| 760 | vNormalizedPoints[i].y = vKeys[i].pt.y - meanY; |
| 761 | |
| 762 | meanDevX += fabs(vNormalizedPoints[i].x); |
| 763 | meanDevY += fabs(vNormalizedPoints[i].y); |
| 764 | } |
| 765 | |
| 766 | meanDevX = meanDevX/N; |
| 767 | meanDevY = meanDevY/N; |
| 768 | |
| 769 | float sX = 1.0/meanDevX; |
| 770 | float sY = 1.0/meanDevY; |
| 771 | |
| 772 | for(int i=0; i<N; i++) |
| 773 | { |
| 774 | vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX; |
| 775 | vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY; |
| 776 | } |
| 777 | |
| 778 | T.setZero(); |
| 779 | T(0,0) = sX; |
| 780 | T(1,1) = sY; |
| 781 | T(0,2) = -meanX*sX; |
| 782 | T(1,2) = -meanY*sY; |
| 783 | T(2,2) = 1.f; |
| 784 | } |
| 785 | |
| 786 | int TwoViewReconstruction::CheckRT(const Eigen::Matrix3f &R, const Eigen::Vector3f &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2, |
| 787 | const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers, |