#ifndef TRIANGULATION_H
#define TRIANGULATION_H

#include <stdio.h>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d.hpp>

#define NOMINMAX ///< needed for OpenCV functionning

#ifdef _WIN32
#include <windows.h>   // required by 1394camapi.h
#include <strsafe.h>   // required by 1394camapi.h
#endif

#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <list>
#include <set>

#include <cstdlib>
#include <cmath>

//#include <core/core.hpp>
#include "transformations.h"

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h> 
#include <pcl/io/io.h>
#include <pcl/visualization/pcl_plotter.h>

#undef max

class Triangulator 
{

public:

	Triangulator();
	Triangulator(int method);

    //! Possible triangulation methods :
    enum { LINEAR_LS, LINEAR_EIGEN, ITERATIVE_LS, ITERATIVE_EIGEN, DIRECT_AFFINE};

	void setTriangulationMethod(int method);

	double getReprojectionError();

    void triangulate(const cv::Mat & W,
                     const cv::Mat & P,
                     pcl::PointCloud<pcl::PointXYZ> & pointCloud);

    /*void triangulatePoints(const std::vector<cv::KeyPoint>& pt_set1,
						   const std::vector<cv::KeyPoint>& pt_set2,
						   const cv::Mat&Kinv,
						   const cv::Matx34d& P,
						   const cv::Matx34d& P1,
						   std::vector<pcl::PointXYZ>& pointcloud);*/

    pcl::PointCloud<pcl::PointXYZ> triangulatePoints_LinearLS(const cv::Mat & Wf,  // Full measurement matrix 2i x p
                           const cv::Mat & Pm  // Camera matrices 3i x 4
                           ); // 4 x p

    void triangulateAffine(const cv::Mat & W,
                           const cv::Mat & P,
                           pcl::PointCloud<pcl::PointXYZ> & pointCloud);

private:
	int triangulationMethod; 
	double reprojectionError;

	//cv::Mat_<double> linearLS( cv::Point3d u, cv::Matx34d P, cv::Point3d u1, cv::Matx34d P1);

	//cv::Mat_<double> linearEigen( cv::Point3d u, cv::Matx34d P, cv::Point3d u1, cv::Matx34d P1);

	//cv::Mat_<double> iterativeLSandEigen( cv::Point3d u, cv::Matx34d P, cv::Point3d u1, cv::Matx34d P1);
};
///@}
#endif