#include <opencv2/core/utility.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
#include "opencv2/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include <iostream>
#include <cstring>
#include <cmath>
#include <limits>
#include <fstream>
#include "opencv2/core.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/features2d.hpp"
using namespace cv;
using namespace std;
int main( int argc, char** argv ){
Mat frame;
//Mat flow;
//std::string video = argv[1];
//VideoCapture cap(video);
VideoCapture cap("farn.mp4");
if(!cap.isOpened())
{
cout << "Could not read video file" << endl;
return 1;
}
cap >> frame;
cv::Mat image_prev, image_next, image_prev_gr, frame_gr, img_feat, img_keypoints, H;
cv::Mat K= cv::Mat::zeros(3,3,CV_8U);
vector<Mat> R, T, N;
K.at<float>(0,0)=1;
K.at<float>(1,0)=0;
K.at<float>(2,0)=0;
K.at<float>(0,1)=0;
K.at<float>(1,1)=1;
K.at<float>(2,1)=0;
K.at<float>(0,2)=0;
K.at<float>(1,2)=0;
K.at<float>(2,2)=1;
std::vector<cv::Point2f> features_prev, features_next;
cv::Point2f p1, p2;
std::vector<cv::KeyPoint> feat_prev, feat_next;
int m;
const std::vector<int> & KeyPointIndex1 = std::vector<int>();
const std::vector<int> & KeyPointIndex2 = std::vector<int>();
vector<double> af(6), S(6);
double alpha, sx, sy, hx, tx, ty, alpha1, sx1, sy1, hx1, tx1, ty1, alpha2, sx2, sy2, hx2, tx2, ty2;
cvtColor(frame, frame_gr, cv::COLOR_RGB2GRAY);
int minHessian = 600;
Ptr<ORB> detector = ORB::create(9);
detector->detect( frame_gr, feat_next );
//ofstream myFile;
//myFile.open("affin.csv");
for(;;)
{
KeyPoint::convert(feat_prev, features_prev, KeyPointIndex1);
KeyPoint::convert(feat_next, features_next, KeyPointIndex2);
for(int i = 1; i <= 2; i++)
{
image_prev = frame.clone();
features_prev = features_next;
cap >> frame;
cvtColor(frame, frame_gr, cv::COLOR_RGB2GRAY);
cvtColor(image_prev, image_prev_gr, cv::COLOR_RGB2GRAY);
vector<uchar> status;
vector<float> err;
calcOpticalFlowPyrLK(
image_prev_gr, frame_gr,
features_prev,
features_next,
status,
err
);
/*int n = features_next.size();
double x1[n];
double y1[n];
double x2[n];
double y2[n];*/
img_feat = frame.clone();
for (size_t i = 0; i < features_next.size(); ++i)
{
circle( img_feat, features_next[i], 5, Scalar(0, 255, 0), 3 );
}
imshow("Tracking", img_feat);
if(waitKey(1)==27)break;
}
H = findHomography(features_prev, features_next, CV_LMEDS);
/*H.at<float>(0,0)=0.5;
H.at<float>(1,0)=0.1;
H.at<float>(2,0)=0.1;
H.at<float>(0,1)=0.1;
H.at<float>(1,1)=0.5;
H.at<float>(2,1)=0.1;
H.at<float>(0,2)=0.1;
H.at<float>(1,2)=0.1;
H.at<float>(2,2)=0.5;*/
//decomposeProjectionMatrix(InputArray projMatrix, OutputArray cameraMatrix, OutputArray rotMatrix, OutputArray transVect, OutputArray rotMatrixX=noArray(), OutputArray rotMatrixY=noArray(), OutputArray rotMatrixZ=noArray(), OutputArray eulerAngles=noArray() )
int solutions = decomposeHomographyMat (H, K, R, T, N);
cout << H.at<float>(0,0) <<", "<< H.at<float>(1,0) <<", "<< H.at<float>(2,0) << endl;
//cout << R[0] << endl;
//myFile << alpha1 << "," << tx1 << "," << ty1 << "," << hx << "," << sx1 << "," << sy1 <<endl;
/*solvePnPRansac ( InputArray objectPoints,
InputArray imagePoints,
InputArray cameraMatrix,
InputArray distCoeffs,
OutputArray rvec,
OutputArray tvec,
bool useExtrinsicGuess = false,
int iterationsCount = 100,
float reprojectionError = 8.0,
double confidence = 0.99,
OutputArray inliers = noArray(),
int flags = SOLVEPNP_ITERATIVE
) */
/*for (int i = 0; i < solutions; i++)
{
//double factor_d1 = 1.0 / d_inv1;
Mat rvec_decomp;
Rodrigues(R[i], rvec_decomp);
cout << "Solution " << i << ":" << endl;
cout << "rvec from homography decomposition: " << rvec_decomp.t() << endl;
//cout << "rvec from camera displacement: " << rvec_1to2.t() << endl;
cout << "tvec from homography decomposition: " << T[i].t() << endl; //<< " and scaled by d: " << factor_d1 * T[i].t() << endl;
//cout << "tvec from camera displacement: " << t_1to2.t() << endl;
cout << "plane normal from homography decomposition: " << N[i].t() << endl;
//cout << "plane normal at camera 1 pose: " << normal1.t() << endl << endl;
}*/
detector->detect( frame_gr, feat_next );
if(waitKey(1)==27)break;
}
//myFile.close();
return 0;
}