void computePoseDifference(Mat img1, Mat img2, CommandArgs args, Mat k, Mat& dist_coefficients, double& worldScale, Mat& R, Mat& t, Mat& img_matches)
{
cout << "%===============================================%" << endl;
Mat camera_matrix = k.clone();
if (args.resize_factor > 1)
{
resize(img1, img1, Size(img1.cols / args.resize_factor,
img1.rows / args.resize_factor)); // make smaller for performance and displayablity
resize(img2, img2, Size(img2.cols / args.resize_factor,
img2.rows / args.resize_factor));
// scale matrix down according to changed resolution
camera_matrix = camera_matrix / args.resize_factor;
camera_matrix.at<double>(2,2) = 1;
}
Mat K1, K2;
K1 = K2 = camera_matrix;
if (img1.rows > img1.cols) // it is assumed the camera has been calibrated in landscape mode, so undistortion must also be performed in landscape orientation, or the camera matrix must be modified (fx,fy and cx,cy need to be exchanged)
{
swap(K1.at<double>(0,0), K1.at<double>(1,1));
swap(K1.at<double>(0,2), K1.at<double>(1,2));
}
if (img2.rows > img2.cols)
{
swap(K2.at<double>(0,0), K2.at<double>(1,1));
swap(K2.at<double>(0,2), K2.at<double>(1,2));
}
// Feature detection + extraction
vector<KeyPoint> KeyPoints_1, KeyPoints_2;
Mat descriptors_1, descriptors_2;
Ptr<Feature2D> feat_detector;
if (args.detector == DETECTOR_KAZE)
{
feat_detector = AKAZE::create(args.detector_data.upright ? AKAZE::DESCRIPTOR_MLDB_UPRIGHT : AKAZE::DESCRIPTOR_MLDB,
args.detector_data.descriptor_size,
args.detector_data.descriptor_channels,
args.detector_data.threshold,
args.detector_data.nOctaves,
args.detector_data.nOctaveLayersAkaze);
} else if (args.detector == DETECTOR_SURF)
{
feat_detector = xfeatures2d::SURF::create(args.detector_data.minHessian,
args.detector_data.nOctaves, args.detector_data.nOctaveLayersAkaze, args.detector_data.extended, args.detector_data.upright);
} else if (args.detector == DETECTOR_SIFT)
{
feat_detector = xfeatures2d::SIFT::create(args.detector_data.nFeatures,
args.detector_data.nOctaveLayersSift, args.detector_data.contrastThreshold, args.detector_data.sigma);
}
feat_detector->detectAndCompute(img1, noArray(), KeyPoints_1, descriptors_1);
feat_detector->detectAndCompute(img2, noArray(), KeyPoints_2, descriptors_2);
cout << "Number of feature points (img1, img2): " << "(" << KeyPoints_1.size() << ", " << KeyPoints_2.size() << ")" << endl;
// Find correspondences
BFMatcher matcher;
vector<DMatch> matches;
if (args.use_ratio_test)
{
if (args.detector == DETECTOR_KAZE)
matcher = BFMatcher(NORM_HAMMING, false);
else matcher = BFMatcher(NORM_L2, false);
vector<vector<DMatch>> match_candidates;
const float ratio = args.ratio;
matcher.knnMatch(descriptors_1, descriptors_2, match_candidates, 2);
for (int i = 0; i < match_candidates.size(); i++)
if (match_candidates[i][0].distance < ratio * match_candidates[i][1].distance)
matches.push_back(match_candidates[i][0]);
cout << "Number of matches passing ratio test: " << matches.size() << endl;
} else
{
if (args.detector == DETECTOR_KAZE)
matcher = BFMatcher(NORM_HAMMING, true);
else matcher = BFMatcher(NORM_L2, true);
matcher.match(descriptors_1, descriptors_2, matches);
cout << "Number of matching feature points: " << matches.size() << endl;
}
// Convert correspondences to vectors
vector<Point2f>imgpts1,imgpts2;
for(unsigned int i = 0; i < matches.size(); i++)
{
imgpts1.push_back(KeyPoints_1[matches[i].queryIdx].pt);
imgpts2.push_back(KeyPoints_2[matches[i].trainIdx].pt);
}
Mat mask; // inlier mask
if (args.undistort)
{
undistortPoints(imgpts1, imgpts1, K1, dist_coefficients, noArray(), K1);
undistortPoints(imgpts2, imgpts2, K2, dist_coefficients, noArray(), K2);
}
double focal = camera_matrix.at<double>(0,0);
Point2d principalPoint(camera_matrix.at<double>(0,2),camera_matrix.at<double>(1,2));
Mat E = findEssentialMat(imgpts1, imgpts2, focal, principalPoint, RANSAC, 0.999, 1, mask);
/* Mat F = camera_matrix.t().inv() * E * camera_matrix.inv(); */
Mat F = findFundamentalMat(imgpts1, imgpts2, CV_FM_RANSAC);
correctMatches(F, imgpts1, imgpts2, imgpts1, imgpts2);
cout << "Reprojection error:\n " << computeReprojectionError(imgpts1, imgpts2, mask, F) << endl;
int inliers = recoverPose(E, imgpts1, imgpts2, R, t, focal, principalPoint, mask);
cout << "Matches used for pose recovery:\n " << inliers << endl;
/* Mat R1, R2, ProjMat1, ProjMat2, Q; */
/* stereoRectify(camera_matrix, dist_coefficients, camera_matrix, dist_coefficients, img1.size(), R, t, R1, R2, ProjMat1, ProjMat2, Q); */
/* cout << "P1=" << ProjMat1 << endl; */
/* cout << "P2=" << ProjMat2 << endl; */
/* cout << "Q=" << Q << endl; */
Mat mtxR, mtxQ;
Mat Qx, Qy, Qz;
Vec3d angles = RQDecomp3x3(R, mtxR, mtxQ, Qx, Qy, Qz);
/* cout << "Qx:\n " << Qx << endl; */
/* cout << "Qy:\n " << Qy << endl; */
/* cout << "Qz:\n " << Qz << endl; */
cout << "Translation:\n " << t.t() << endl;
cout << "Euler angles [x y z] in degrees:\n " << angles.t() << endl;
if (args.epilines)
{
drawEpilines(Mat(imgpts1), 1, F, img2);
drawEpilines(Mat(imgpts2), 2, F, img1);
}
drawMatches(img1, KeyPoints_1, img2, KeyPoints_2, // draw only inliers given by mask
matches, img_matches, Scalar::all(-1), Scalar::all(-1), mask);
vector<Point2f> imgpts1_masked, imgpts2_masked;
for (int i = 0; i < imgpts1.size(); i++)
{
if (mask.at<uchar>(i,0) == 1)
{
imgpts1_masked.push_back(imgpts1[i]);
imgpts2_masked.push_back(imgpts2[i]);
}
}
Mat pnts4D;
Mat P1 = camera_matrix * Mat::eye(3, 4, CV_64FC1), P2;
Mat p2[2] = { R, t };
hconcat(p2, 2, P2);
P2 = camera_matrix * P2;
#define USE_OPENCV_TRIANGULATION
#ifndef USE_OPENCV_TRIANGULATION // strangely, both methods yield identical results
vector<Point3d> homogPoints1, homogPoints2;
for (int i = 0; i < imgpts1_masked.size(); i++)
{
Point2f currentPoint1 = imgpts1_masked[i];
homogPoints1.push_back(Point3d(currentPoint1.x, currentPoint1.y, 1));
Point2f currentPoint2 = imgpts2_masked[i];
homogPoints2.push_back(Point3d(currentPoint2.x, currentPoint2.y, 1));
}
Mat dehomogenized(imgpts1_masked.size(), 3, CV_64FC1);
for (int i = 0; i < imgpts1_masked.size(); i++)
{
Mat_<double> triangulatedPoint = IterativeLinearLSTriangulation(homogPoints1[i], P1, homogPoints2[i], P2);
Mat r = triangulatedPoint.t();
r.colRange(0,3).copyTo(dehomogenized.row(i)); // directly assigning to dehomogenized.row(i) compiles but does nothing, wtf?
}
#else
triangulatePoints(P1, P2, imgpts1_masked, imgpts2_masked, pnts4D);
pnts4D = pnts4D.t();
Mat dehomogenized;
convertPointsFromHomogeneous(pnts4D, dehomogenized);
dehomogenized = dehomogenized.reshape(1); // instead of 3 channels and 1 col, we want 1 channel and 3 cols
#endif
double mDist = 0;
int n = 0;
int pos = 0, neg = 0;
/* Write ply file header */
ofstream ply_file("points.ply", ios_base::trunc);
ply_file <<
"ply\n"
"format ascii 1.0\n"
"element vertex " << dehomogenized.rows << "\n"
"property float x\n"
"property float y\n"
"property float z\n"
"property uchar red\n"
"property uchar green\n"
"property uchar blue\n"
"end_header" << endl;
Mat_<double> row;
for (int i = 0; i < dehomogenized.rows; i++)
{
row = dehomogenized.row(i);
double d = row(2);
if (d > 0)
{
pos++;
mDist += norm(row);
n++;
/* float startx=imgpts1_masked[i].x - 1, starty=imgpts1_masked[i].y - 1, endx=imgpts1_masked[i].x + 1, endy=imgpts1_masked[i].y + 1; */
/* cout << "startx,endx = " << startx << "," << endx << endl; */
/* cout << "starty,endy = " << starty << "," << endy << endl; */
Vec3b rgb = img1.at<Vec3b>(imgpts1_masked[i].x, imgpts1_masked[i].y);
ply_file << row(0) << " " << row(1) << " " << row(2) << " " << (int)rgb[2] << " " << (int)rgb[1] << " " << (int)rgb[0] << "\n";
} else
{
neg++;
ply_file << 0 << " " << 0 << " " << 0 << " " << 0 << " " << 0 << " " << 0 << "\n";
}
}
ply_file.close();
mDist /= n;
worldScale = mDist;
cout << "Mean distance of " << n << " points to camera:\n " << mDist << " (dehomogenized)" << endl;
cout << "pos=" << pos << ", neg=" << neg << endl;
/* char filename[100]; */
/* sprintf(filename, "mat_1%d", i+1); */
/* Ptr<Formatter> formatter = Formatter::get(Formatter::FMT_CSV); */
/* Ptr<Formatted> formatted = formatter->format(dehomogenized); */
/* ofstream file(filename, ios_base::trunc); */
/* file << formatted << endl; */
/* Removed until cmake has been fathomed */
/* vector< Point3d > points3D; */
/* vector< vector< Point2d > > pointsImg; */
/* int NPOINTS=dehomogenized.rows; // number of 3d points */
/* int NCAMS=2; // number of cameras */
/* points3D.resize(NPOINTS); */
/* for (int i = 0; i < NPOINTS; i++) */
/* { */
/* points3D[i] = Point3d(dehomogenized.at<double>(i,0), */
/* dehomogenized.at<double>(i,1), */
/* dehomogenized.at<double>(i,2) */
/* ); */
/* } */
/* // fill image projections */
/* vector<vector<int> > visibility(2, vector<int>(NPOINTS, 1)); */
/* vector<Mat> camera_matrices(2, camera_matrix); */
/* vector<Mat> Rs(2); */
/* Rodrigues(Mat::eye(3, 3, CV_64FC1), Rs[0]); */
/* Rodrigues(R, Rs[0]); */
/* vector<Mat> Ts = { Mat::zeros(3,1, CV_64FC1), t }; */
/* vector<Mat> dist_coefficientss(2, dist_coefficients); */
/* pointsImg.resize(NCAMS); */
/* for(int i=0; i<NCAMS; i++) pointsImg[i].resize(NPOINTS); */
/* for (int i = 0; i < NPOINTS; i++) */
/* { */
/* pointsImg[0][i] = Point2d(imgpts1_masked[i].x, imgpts1_masked[i].y); */
/* pointsImg[1][i] = Point2d(imgpts2_masked[i].x, imgpts2_masked[i].y); */
/* } */
/* cvsba::Sba sba; */
/* sba.run(points3D, pointsImg, visibility, camera_matrices, Rs, Ts, dist_coefficientss); */
/* cout<<"Initial error="<<sba.getInitialReprjError()<<". "<< */
/* "Final error="<<sba.getFinalReprjError()<<endl; */
cout << "%===============================================%" << endl;
}
