bool findCorrectPose(Mat_<double> E, vector<KeyPoint> key1, vector<KeyPoint> key2, Mat_<double> K,Mat_<double> Kinv,Matx34d& out,vector<Point3d>& cloudOut) {
Matx34d Peye(1.0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 1.0, 0);
//decompose E to P' , HZ (9.19)
cv::SVD svd(E, SVD::MODIFY_A);
Mat svd_u = svd.u;
Mat svd_vt = svd.vt;
Mat svd_w = svd.w;
Matx33d W(0, -1, 0, //HZ 9.13
1, 0, 0, 0, 0, 1);
/////////////////////////////////////////////////////
//newly added must check if correct
Matx33d Z(0, 1, 0, //HZ 9.13
-1, 0, 0, 0, 0, 0);
Mat_<double> S = svd_u * Mat(Z) * svd_u.t();
///////////////////////////////////
Mat_<double> R = svd_u * Mat(W) * svd_vt; //HZ 9.19
Mat_<double> R1 = svd_u * Mat(W).t() * svd_vt; //HZ 9.19
///////////////////newly added according to mit implementation////////
if(determinant(R)<0)
R = -R;
if(determinant(R1)<0)
R1 = -R1;
/////////////////////////////////////
Mat_<double> t = svd_u.col(2); // u3
Mat_<double> tneg = -1 * svd_u.col(2); // -u3
cv::SVD rSVD(R);
R = rSVD.u * Mat::eye(3, 3, CV_64F) * rSVD.vt;
double myscale = trace(rSVD.w)[0] / 3;
t = t / myscale;
cv::SVD r1SVD(R1);
R1 = r1SVD.u * Mat::eye(3, 3, CV_64F) * r1SVD.vt;
double myscale1 = trace(r1SVD.w)[0] / 3;
tneg = tneg / myscale1;
if (!CheckCoherentRotation(R) || !CheckCoherentRotation(R1)) {
out = 0;
return false;
}
//save the correct pointclouds and Relativepose
Matx34d TempPose,poseHolder;
vector<Point3d> pointCloud,pointCloudMax;
createPoseFromRotationTranslation(R, t, out);
double max = -1,temp = 0.0;
max = getPointCloudAndPointsInFront(Peye, out, key1, key2, K, K.inv(), pointCloud);
pointCloudMax = pointCloud;
poseHolder = Matx34d(out);
pointCloud.clear();
out = TempPose;
createPoseFromRotationTranslation(R, tneg, out);
temp = getPointCloudAndPointsInFront(Peye, out, key1, key2, K, K.inv(), pointCloud);
if(temp>max) {
pointCloudMax = pointCloud;
poseHolder = Matx34d(out);
max= temp;
}
pointCloud.clear();
out = TempPose;
createPoseFromRotationTranslation(R1, t, out);
temp = getPointCloudAndPointsInFront(Peye, out, key1, key2, K, K.inv(), pointCloud);
if(temp>max) {
pointCloudMax = pointCloud;
poseHolder = Matx34d(out);
max= temp;
}
pointCloud.clear();
out = TempPose;
createPoseFromRotationTranslation(R1, tneg, out);
temp = getPointCloudAndPointsInFront(Peye, out, key1, key2, K, K.inv(), pointCloud);
if(temp>max) {
pointCloudMax = pointCloud;
poseHolder = Matx34d(out);
max= temp;
}
out = poseHolder;
cloudOut = pointCloudMax;
return true;
}
int main(int argc, char **argv) {
try {
ParseArgs(argc, argv);
int num_cameras = static_cast<int>(imgs.size());
if (num_cameras < 1)
throw runtime_error("Need at least one camera");
// Find features
cout << "Finding features...\n";
Ptr<detail::FeaturesFinder> features_finder = features_finder_creator->Create();
for (int i = 0; i < num_cameras; ++i) {
int64 t = getTickCount();
cout << "Finding features in '" << img_names[i] << "'... ";
Ptr<detail::ImageFeatures> features = new detail::ImageFeatures();
(*features_finder)(imgs[i], *features);
features_collection[i] = features;
cout << "#features = " << features_collection.find(i)->second->keypoints.size()
<< ", time = " << (getTickCount() - t) / getTickFrequency() << " sec\n";
}
// Match all pairs
cout << "Matching pairs... ";
MatchesCollection matches_collection;
Ptr<detail::FeaturesMatcher> matcher = features_matcher_creator.Create();
FeaturesCollection::iterator from_iter = features_collection.begin();
FeaturesCollection::iterator from_next_iter = from_iter; ++from_next_iter;
FeaturesCollection::iterator to_iter;
for (; from_next_iter != features_collection.end(); from_iter = from_next_iter++) {
for (to_iter = from_next_iter; to_iter != features_collection.end(); ++to_iter) {
cout << "(" << from_iter->first << "->" << to_iter->first << ") ";
detail::MatchesInfo mi;
(*matcher)(*(from_iter->second), *(to_iter->second), mi);
matches_collection[make_pair(from_iter->first, to_iter->first)]
= new vector<DMatch>(mi.matches);
}
}
cout << endl;
// Estimate homographies
HomographiesP2 Hs;
HomographiesP2 good_Hs;
vector<Mat> Hs_from_0;
RelativeConfidences rel_confs;
Mat keypoints1, keypoints2;
cout << "Estimating Hs...\n";
for (int from = 0; from < num_cameras - 1; ++from) {
for (int to = from + 1; to < num_cameras; ++to) {
const vector<DMatch> &matches = *(matches_collection.find(make_pair(from, to))->second);
cout << "Estimating H between '" << img_names[from] << "' and '" << img_names[to]
<< "'... #matches = " << matches.size();
if (static_cast<int>(matches.size()) < min_num_matches) {
cout << ", not enough matches\n";
continue;
}
ExtractMatchedKeypoints(*(features_collection.find(from)->second),
*(features_collection.find(to)->second),
matches, keypoints1, keypoints2);
vector<uchar> inliers_mask;
Mat_<double> H = findHomography(keypoints1.reshape(2), keypoints2.reshape(2),
inliers_mask, RANSAC, H_est_thresh);
if (H.empty()) {
cout << ", can't estimate H\n";
continue;
}
Ptr<vector<DMatch> > inliers = new vector<DMatch>();
for (size_t i = 0; i < matches.size(); ++i)
if (inliers_mask[i])
inliers->push_back(matches[i]);
cout << ", #inliers = " << inliers->size();
double rms_err = 0;
for (size_t i = 0; i < matches.size(); ++i) {
const Point2d &kp1 = keypoints1.at<Point2d>(0, i);
const Point2d &kp2 = keypoints2.at<Point2d>(0, i);
double x = H(0, 0) * kp1.x + H(0, 1) * kp1.y + H(0, 2);
double y = H(1, 0) * kp1.x + H(1, 1) * kp1.y + H(1, 2);
double z = H(2, 0) * kp1.x + H(2, 1) * kp1.y + H(2, 2);
x /= z; y /= z;
rms_err += (kp2.x - x) * (kp2.x - x) + (kp2.y - y) * (kp2.y - y);
}
rms_err = sqrt(rms_err / matches.size());
cout << ", RMS err = " << rms_err;
// See "Automatic Panoramic Image Stitching using Invariant Features"
// by Matthew Brown and David G. Lowe, IJCV 2007 for the explanation
double confidence = inliers->size() / (8 + 0.3 * matches.size()) - 1;
rel_confs[make_pair(from, to)] = confidence;
cout << ", conf = " << confidence;
cout << endl;
Hs[make_pair(from, to)] = H;
matches_collection[make_pair(from, to)] = inliers;
if (confidence > 0)
good_Hs[make_pair(from, to)] = H;
if (from == 0)
Hs_from_0.push_back(H);
}
}
// Linear calibration
if (K_init.empty()) {
cout << "Linear calibrating...\n";
if (lin_est_skew)
K_init = CalibRotationalCameraLinear(good_Hs);
else
K_init = CalibRotationalCameraLinearNoSkew(good_Hs);
cout << "K_init =\n" << K_init << endl;
}
// Non-linear refinement
cout << "Refining camera...\n";
if (Hs_from_0.size() != num_cameras - 1) {
stringstream msg;
msg << "Refinement requires Hs between first and all other images, "
<< "but only " << Hs_from_0.size() << " were/was found";
throw runtime_error(msg.str());
}
map<int, Mat> Rs;
Rs[0] = Mat::eye(3, 3, CV_64F);
for (int i = 1; i < num_cameras; ++i)
Rs[i] = K_init.inv() * Hs_from_0[i - 1] * K_init;
Mat_<double> K_refined = K_init.clone();
if (refine_skew)
RefineRigidCamera(K_refined, Rs, features_collection, matches_collection);
else {
K_refined(0, 1) = 0;
RefineRigidCamera(K_refined, Rs, features_collection, matches_collection,
REFINE_FLAG_K_ALL & ~REFINE_FLAG_K_SKEW);
}
cout << "K_refined =\n" << K_refined << endl;
cout << "SUMMARY\n";
cout << "K_init =\n" << K_init << endl;
cout << "K_refined =\n" << K_refined << endl;
}
catch (const exception &e) {
cout << "Error: " << e.what() << endl;
}
return 0;
}