#include <stdio.h>
#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp> 
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "optimization.h"
#include "FundMatFitting.hpp"



using namespace std;

class Autocalib
{

public:

vector<vector<cv::Point2d> > findFeaturePoint(vector<cv::Mat> img,double qualityLevel ); // paired measurement matrix
vector<vector<cv::Point2d> > getMeasureVector(vector<vector<cv::Point2d> > MatchVector); // dense measurement matrix i.e. without zeros
vector<vector<cv::Point2d> > searchAllMatch(vector<vector<cv::Point2d> > Vector);	// remove zeros
cv::Mat vectorPoint2Mat(vector<vector<cv::Point2d> > Vector);

vector<cv::Mat> estimatFundMatVector(vector<vector<cv::Point2d> > MeasureVector);

cv::Mat plotStereoWithEpilines(cv::Mat img1,cv::Mat img2,cv::Mat F,vector<cv::Point2d> pts1,vector<cv::Point2d> pts2);

void showEpipolarLines(vector<cv::Mat> image, vector<vector<cv::Point2d> > measureVector, vector<cv::Mat> fundMatrix);
//void showRectificaEpipolarLines(vector<Mat> image, vector<vector<Point2d> > MeasureMatrix, vector<Mat> fundMatrix);

vector <cv::Mat> estimatParameter(cv::Mat measureMatrix);

private:
cv::Mat _W;

};