#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;
  
  };