// Copyright 2010-2014, D. E. Shaw Research.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt )

#ifndef BOOST_RANDOM_DETAIL_MULHILO_HPP
#define BOOST_RANDOM_DETAIL_MULHILO_HPP

#include <limits>
#include <boost/cstdint.hpp>
#include <boost/integer.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/static_assert.hpp>

namespace boost{
namespace random{
namespace detail{

// First, we implement hilo multiplication with "half words".  This is
// the "reference implementation" which should be correct for any
// binary unsigned integral UINT with an even number of bits.  In
// practice, we try to avoid this implementation because it is so slow
// (4 multiplies plus about a dozen xor, or, +, shift, mask and
// compare operations.
template <typename Uint>
inline Uint
mulhilo_halfword(Uint a, Uint b, Uint& hip){
    BOOST_STATIC_ASSERT(std::numeric_limits<Uint>::is_specialized &&
                        std::numeric_limits<Uint>::is_integer &&
                        !std::numeric_limits<Uint>::is_signed &&
                        std::numeric_limits<Uint>::radix == 2 &&
                        std::numeric_limits<Uint>::digits%2 == 0);
    const unsigned WHALF = std::numeric_limits<Uint>::digits/2;
    const Uint LOMASK = ((Uint)(~(Uint)0)) >> WHALF;
    Uint lo = a*b;
    Uint ahi = a>>WHALF;
    Uint alo = a& LOMASK;
    Uint bhi = b>>WHALF;
    Uint blo = b& LOMASK;

    Uint ahbl = ahi*blo;
    Uint albh = alo*bhi;

    Uint ahbl_albh = ((ahbl&LOMASK) + (albh&LOMASK));
    Uint hi = (ahi*bhi) + (ahbl>>WHALF) +  (albh>>WHALF);
    hi += ahbl_albh >> WHALF;
    /* carry from the sum with alo*blo */
    hi += ((lo >> WHALF) < (ahbl_albh&LOMASK));
    hip = hi;
    return lo;
}

// We can formulate a much faster implementation if we can use
// integers of twice the width of Uint (e.g., DblUint).  Such types
// are not always available (e.g., when Uint is uintmax_t), but when
// they are, we find that modern compilers (gcc, MSVC, Intel) pattern
// match the structure of the mulhilo below and turn it into an
// optimized instruction sequence, e.g., mulw or mull.
//
// However, the alternative implementation, which we want to use
// when there *is* a DblUint would be ambiguoous without some enable_if
// hackery.  To support that, we need a has_double_width type trait.
//
// N.B.  It should be possible to do this with some SFINAE wrapped
// around an instantiation of uint_t<2*W>::least.  My attempts to do
// so ran into the problem described in
// https://svn.boost.org/trac/boost/ticket/6169 from Nov 23, 2011
// (still open in April 2014).  So instead just check that twice the
// number of digits in Uint is less than or equal to the number of
// digits in uintmax_t.  FWIW, there is currently (as of 1.53)
// a BOOST_STATIC_ASSERT in integer.hpp that insists on a very
// similar condition:  Bits <= sizeof(boost::uintmax_t)*CHAR_BIT.

template <typename Uint>
class has_double_width{
public:
    static const bool value = std::numeric_limits<Uint>::is_specialized &&
        std::numeric_limits<Uint>::is_integer &&
        !std::numeric_limits<Uint>::is_signed &&
        std::numeric_limits<Uint>::radix == 2 &&
        2*std::numeric_limits<Uint>::digits <= std::numeric_limits< boost::uintmax_t >::digits;
};

// mulhilo using double-width DblUint
template <typename Uint>
inline typename boost::enable_if_c<has_double_width<Uint>::value, Uint>::type
mulhilo(Uint a, Uint b, Uint& hip){
    typedef typename uint_t<2*std::numeric_limits<Uint>::digits>::least DblUint;
    DblUint product = ((DblUint)a)*((DblUint)b);
    hip = product>>std::numeric_limits<Uint>::digits;
    return (Uint)product;
}

// When there is no DblUint and there are no specializations that use
// machine-specific intrinsics (below), fall back to mulhilo_halfword.
template <typename Uint>
inline typename boost::enable_if_c<!has_double_width<Uint>::value, Uint>::type
mulhilo(Uint a, Uint b, Uint& hip){
    return mulhilo_halfword(a, b, hip);
}

// Every ISA I know (x86, ppc, arm, CUDA) has an instruction that
// gives the hi word of the product of two uintmax_t's FAR more
// quickly and succinctly than a call to mulhilo_halfword.  Without
// them, philox<N, uintmax_t> would be impractically slow.
// Unfortunately, they require compiler-and-hardware-specific
// intrinsics or asm statements.
//
// FIXME - add more special cases here, e.g., MSVC intrinsics and
// asm for PowerPC and ARM.
#if defined(__GNUC__) && defined(__x86_64__)
template <>
inline uint64_t
mulhilo(uint64_t ax, uint64_t b, uint64_t& hip){
    uint64_t dx;
    __asm__("\n\t"
        "mulq %2\n\t"
        : "=a"(ax), "=d"(dx)
        : "r"(b), "0"(ax)
        );
    hip = dx;
    return ax;
}
#endif

} // namespace detail
} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_DETAIL_MULHILO_HPP


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

#include "aslam/calibration/functions/IncompleteGammaPFunction.h"

#include <boost/math/special_functions/gamma.hpp>

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

    IncompleteGammaPFunction::IncompleteGammaPFunction(double alpha) :
        mAlpha(alpha) {
    }

    IncompleteGammaPFunction::IncompleteGammaPFunction(const
        IncompleteGammaPFunction& other) :
        mAlpha(other.mAlpha) {
    }

    IncompleteGammaPFunction& IncompleteGammaPFunction::operator = (const
        IncompleteGammaPFunction& other) {
      if (this != &other) {
        mAlpha = other.mAlpha;
      }
      return *this;
    }

    IncompleteGammaPFunction::~IncompleteGammaPFunction() {
    }

/******************************************************************************/
/* Stream operations                                                          */
/******************************************************************************/

    void IncompleteGammaPFunction::read(std::istream& stream) {
    }

    void IncompleteGammaPFunction::write(std::ostream& stream) const {
      stream << "alpha: " << mAlpha;
    }

    void IncompleteGammaPFunction::read(std::ifstream& stream) {
    }

    void IncompleteGammaPFunction::write(std::ofstream& stream) const {
    }

/******************************************************************************/
/* Accessors                                                                  */
/******************************************************************************/

    double IncompleteGammaPFunction::getValue(const VariableType& argument)
        const {
      return boost::math::gamma_p(mAlpha, argument);
    }

    double IncompleteGammaPFunction::getAlpha() const {
      return mAlpha;
    }

    void IncompleteGammaPFunction::setAlpha(double alpha) {
      mAlpha = alpha;
    }

  }
}


// Bring in my package's API, which is what I'm testing

#include <Eigen/Core>
// Bring in gtest
#include <gtest/gtest.h>
#include <boost/cstdint.hpp>
#include <sm/eigen/gtest.hpp>

#include <sm/eigen/matrix_sqrt.hpp>

TEST(EigenMatrixSqrtTest, testMatrixSqrt) {
    for (int i = 0; i < 100; ++i) {
        Eigen::MatrixXd R = sm::eigen::randomCovariance<3>();
        Eigen::MatrixXd sqrtR;
        sm::eigen::computeMatrixSqrt(R, sqrtR);
        Eigen::MatrixXd reconstructedR = sqrtR * sqrtR.transpose();
        ASSERT_DOUBLE_MX_EQ(R, reconstructedR, 1e-9,
                            "Reconstructing the matrix from the square root in iteration " << i);
    }
}


/*****************************************************************************
*   ExploringSfMWithOpenCV
******************************************************************************
*   by Roy Shilkrot, 5th Dec 2012
*   http://www.morethantechnical.com/
******************************************************************************
*   Ch4 of the book "Mastering OpenCV with Practical Computer Vision Projects"
*   Copyright Packt Publishing 2012.
*   http://www.packtpub.com/cool-projects-with-opencv/book
*****************************************************************************/

#include "FindCameraMatrices.h"
#include "Triangulation.h"

#include <vector>
#include <iostream>

#include <opencv2/calib3d/calib3d.hpp>

using namespace cv;
using namespace std;

#ifdef USE_EIGEN
#include <Eigen/Eigen>
#endif

#define DECOMPOSE_SVD

#ifndef CV_PCA_DATA_AS_ROW
#define CV_PCA_DATA_AS_ROW 0
#endif

void DecomposeEssentialUsingHorn90(double _E[9], double _R1[9], double _R2[9], double _t1[3], double _t2[3]) {
	//from : http://people.csail.mit.edu/bkph/articles/Essential.pdf
#ifdef USE_EIGEN
	using namespace Eigen;

	Matrix3d E = Map<Matrix<double,3,3,RowMajor> >(_E);
	Matrix3d EEt = E * E.transpose();
	Vector3d e0e1 = E.col(0).cross(E.col(1)),e1e2 = E.col(1).cross(E.col(2)),e2e0 = E.col(2).cross(E.col(2));
	Vector3d b1,b2;

#if 1
	//Method 1
	Matrix3d bbt = 0.5 * EEt.trace() * Matrix3d::Identity() - EEt; //Horn90 (12)
	Vector3d bbt_diag = bbt.diagonal();
	if (bbt_diag(0) > bbt_diag(1) && bbt_diag(0) > bbt_diag(2)) {
		b1 = bbt.row(0) / sqrt(bbt_diag(0));
		b2 = -b1;
	} else if (bbt_diag(1) > bbt_diag(0) && bbt_diag(1) > bbt_diag(2)) {
		b1 = bbt.row(1) / sqrt(bbt_diag(1));
		b2 = -b1;
	} else {
		b1 = bbt.row(2) / sqrt(bbt_diag(2));
		b2 = -b1;
	}
#else
	//Method 2
	if (e0e1.norm() > e1e2.norm() && e0e1.norm() > e2e0.norm()) {
		b1 = e0e1.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	} else if (e1e2.norm() > e0e1.norm() && e1e2.norm() > e2e0.norm()) {
		b1 = e1e2.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	} else {
		b1 = e2e0.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
		b2 = -b1;
	}
#endif

	//Horn90 (19)
	Matrix3d cofactors; cofactors.col(0) = e1e2; cofactors.col(1) = e2e0; cofactors.col(2) = e0e1;
	cofactors.transposeInPlace();

	//B = [b]_x , see Horn90 (6) and http://en.wikipedia.org/wiki/Cross_product#Conversion_to_matrix_multiplication
	Matrix3d B1; B1 <<	0,-b1(2),b1(1),
						b1(2),0,-b1(0),
						-b1(1),b1(0),0;
	Matrix3d B2; B2 <<	0,-b2(2),b2(1),
						b2(2),0,-b2(0),
						-b2(1),b2(0),0;

	Map<Matrix<double,3,3,RowMajor> > R1(_R1),R2(_R2);

	//Horn90 (24)
	R1 = (cofactors.transpose() - B1*E) / b1.dot(b1);
	R2 = (cofactors.transpose() - B2*E) / b2.dot(b2);
	Map<Vector3d> t1(_t1),t2(_t2);
	t1 = b1; t2 = b2;

	cout << "Horn90 provided " << endl << R1 << endl << "and" << endl << R2 << endl;
#endif
}

bool CheckCoherentRotation(cv::Mat_<double>& R) {

	if(fabsf(determinant(R))-1.0 > 1e-07) {
		cerr << "det(R) != +-1.0, this is not a rotation matrix" << endl;
		return false;
	}

	return true;
}

Mat GetFundamentalMat(const vector<KeyPoint>& imgpts1,
					   const vector<KeyPoint>& imgpts2,
					   vector<KeyPoint>& imgpts1_good,
					   vector<KeyPoint>& imgpts2_good,
					   vector<DMatch>& matches
#ifdef __SFM__DEBUG__
					  ,const Mat& img_1,
					  const Mat& img_2
#endif
					  )
{
	//Try to eliminate keypoints based on the fundamental matrix
	//(although this is not the proper way to do this)
	vector<uchar> status(imgpts1.size());

#ifdef __SFM__DEBUG__
	std::vector< DMatch > good_matches_;
	std::vector<KeyPoint> keypoints_1, keypoints_2;
#endif
	//	undistortPoints(imgpts1, imgpts1, cam_matrix, distortion_coeff);
	//	undistortPoints(imgpts2, imgpts2, cam_matrix, distortion_coeff);
	//
	imgpts1_good.clear(); imgpts2_good.clear();

	vector<KeyPoint> imgpts1_tmp;
	vector<KeyPoint> imgpts2_tmp;
	if (matches.size() <= 0) {
		//points already aligned...
		imgpts1_tmp = imgpts1;
		imgpts2_tmp = imgpts2;
	} else {
		GetAlignedPointsFromMatch(imgpts1, imgpts2, matches, imgpts1_tmp, imgpts2_tmp);
	}

	Mat F;
	{
		vector<Point2f> pts1,pts2;
		KeyPointsToPoints(imgpts1_tmp, pts1);
		KeyPointsToPoints(imgpts2_tmp, pts2);
#ifdef __SFM__DEBUG__
		cout << "pts1 " << pts1.size() << " (orig pts " << imgpts1_tmp.size() << ")" << endl;
		cout << "pts2 " << pts2.size() << " (orig pts " << imgpts2_tmp.size() << ")" << endl;
#endif
		double minVal,maxVal;
		cv::minMaxIdx(pts1,&minVal,&maxVal);
		F = findFundamentalMat(pts1, pts2, FM_RANSAC, 0.006 * maxVal, 0.99, status); //threshold from [Snavely07 4.1]
	}

	vector<DMatch> new_matches;
	cout << "F keeping " << countNonZero(status) << " / " << status.size() << endl;
	for (unsigned int i=0; i<status.size(); i++) {
		if (status[i])
		{
			imgpts1_good.push_back(imgpts1_tmp[i]);
			imgpts2_good.push_back(imgpts2_tmp[i]);

			if (matches.size() <= 0) { //points already aligned...
				new_matches.push_back(DMatch(matches[i].queryIdx,matches[i].trainIdx,matches[i].distance));
			} else {
				new_matches.push_back(matches[i]);
			}

#ifdef __SFM__DEBUG__
			good_matches_.push_back(DMatch(imgpts1_good.size()-1,imgpts1_good.size()-1,1.0));
			keypoints_1.push_back(imgpts1_tmp[i]);
			keypoints_2.push_back(imgpts2_tmp[i]);
#endif
		}
	}

	cout << matches.size() << " matches before, " << new_matches.size() << " new matches after Fundamental Matrix\n";
	matches = new_matches; //keep only those points who survived the fundamental matrix

#if 0
	//-- Draw only "good" matches
#ifdef __SFM__DEBUG__
	if(!img_1.empty() && !img_2.empty()) {
		vector<Point2f> i_pts,j_pts;
		Mat img_orig_matches;
		{ //draw original features in red
			vector<uchar> vstatus(imgpts1_tmp.size(),1);
			vector<float> verror(imgpts1_tmp.size(),1.0);
			img_1.copyTo(img_orig_matches);
			KeyPointsToPoints(imgpts1_tmp, i_pts);
			KeyPointsToPoints(imgpts2_tmp, j_pts);
			drawArrows(img_orig_matches, i_pts, j_pts, vstatus, verror, Scalar(0,0,255));
		}
		{ //superimpose filtered features in green
			vector<uchar> vstatus(imgpts1_good.size(),1);
			vector<float> verror(imgpts1_good.size(),1.0);
			i_pts.resize(imgpts1_good.size());
			j_pts.resize(imgpts2_good.size());
			KeyPointsToPoints(imgpts1_good, i_pts);
			KeyPointsToPoints(imgpts2_good, j_pts);
			drawArrows(img_orig_matches, i_pts, j_pts, vstatus, verror, Scalar(0,255,0));
			imshow( "Filtered Matches", img_orig_matches );
		}
		int c = waitKey(0);
		if (c=='s') {
			imwrite("fundamental_mat_matches.png", img_orig_matches);
		}
		destroyWindow("Filtered Matches");
	}
#endif
#endif

	return F;
}

void TakeSVDOfE(Mat_<double>& E, Mat& svd_u, Mat& svd_vt, Mat& svd_w) {
#if 1
	//Using OpenCV's SVD
	SVD svd(E,SVD::MODIFY_A);
	svd_u = svd.u;
	svd_vt = svd.vt;
	svd_w = svd.w;
#else
	//Using Eigen's SVD
	cout << "Eigen3 SVD..\n";
	Eigen::Matrix3f  e = Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor> >((double*)E.data).cast<float>();
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(e, Eigen::ComputeThinU | Eigen::ComputeThinV);
	Eigen::MatrixXf Esvd_u = svd.matrixU();
	Eigen::MatrixXf Esvd_v = svd.matrixV();
	svd_u = (Mat_<double>(3,3) << Esvd_u(0,0), Esvd_u(0,1), Esvd_u(0,2),
						  Esvd_u(1,0), Esvd_u(1,1), Esvd_u(1,2),
						  Esvd_u(2,0), Esvd_u(2,1), Esvd_u(2,2));
	Mat_<double> svd_v = (Mat_<double>(3,3) << Esvd_v(0,0), Esvd_v(0,1), Esvd_v(0,2),
						  Esvd_v(1,0), Esvd_v(1,1), Esvd_v(1,2),
						  Esvd_v(2,0), Esvd_v(2,1), Esvd_v(2,2));
	svd_vt = svd_v.t();
	svd_w = (Mat_<double>(1,3) << svd.singularValues()[0] , svd.singularValues()[1] , svd.singularValues()[2]);
#endif

	cout << "----------------------- SVD ------------------------\n";
	cout << "U:\n"<<svd_u<<"\nW:\n"<<svd_w<<"\nVt:\n"<<svd_vt<<endl;
	cout << "----------------------------------------------------\n";
}

bool TestTriangulation(const vector<CloudPoint>& pcloud, const Matx34d& P, vector<uchar>& status) {
	vector<Point3d> pcloud_pt3d = CloudPointsToPoints(pcloud);
	vector<Point3d> pcloud_pt3d_projected(pcloud_pt3d.size());

	Matx44d P4x4 = Matx44d::eye();
	for(int i=0;i<12;i++) P4x4.val[i] = P.val[i];

	perspectiveTransform(pcloud_pt3d, pcloud_pt3d_projected, P4x4);

	status.resize(pcloud.size(),0);
	for (int i=0; i<pcloud.size(); i++) {
		status[i] = (pcloud_pt3d_projected[i].z > 0) ? 1 : 0;
	}
	int count = countNonZero(status);

	double percentage = ((double)count / (double)pcloud.size());
	cout << count << "/" << pcloud.size() << " = " << percentage*100.0 << "% are in front of camera" << endl;
	if(percentage < 0.75)
		return false; //less than 75% of the points are in front of the camera

	//check for coplanarity of points
	if(false) //not
	{
		cv::Mat_<double> cldm(pcloud.size(),3);
		for(unsigned int i=0;i<pcloud.size();i++) {
			cldm.row(i)(0) = pcloud[i].pt.x;
			cldm.row(i)(1) = pcloud[i].pt.y;
			cldm.row(i)(2) = pcloud[i].pt.z;
		}
		cv::Mat_<double> mean;
		cv::PCA pca(cldm,mean,CV_PCA_DATA_AS_ROW);

		int num_inliers = 0;
		cv::Vec3d nrm = pca.eigenvectors.row(2); nrm = nrm / norm(nrm);
		cv::Vec3d x0 = pca.mean;
		double p_to_plane_thresh = sqrt(pca.eigenvalues.at<double>(2));

		for (int i=0; i<pcloud.size(); i++) {
			Vec3d w = Vec3d(pcloud[i].pt) - x0;
			double D = fabs(nrm.dot(w));
			if(D < p_to_plane_thresh) num_inliers++;
		}

		cout << num_inliers << "/" << pcloud.size() << " are coplanar" << endl;
		if((double)num_inliers / (double)(pcloud.size()) > 0.85)
			return false;
	}

	return true;
}

bool DecomposeEtoRandT(
	Mat_<double>& E,
	Mat_<double>& R1,
	Mat_<double>& R2,
	Mat_<double>& t1,
	Mat_<double>& t2)
{
#ifdef DECOMPOSE_SVD
	//Using HZ E decomposition
	Mat svd_u, svd_vt, svd_w;
	TakeSVDOfE(E,svd_u,svd_vt,svd_w);

	//check if first and second singular values are the same (as they should be)
	double singular_values_ratio = fabsf(svd_w.at<double>(0) / svd_w.at<double>(1));
	if(singular_values_ratio>1.0) singular_values_ratio = 1.0/singular_values_ratio; // flip ratio to keep it [0,1]
	if (singular_values_ratio < 0.7) {
		cout << "singular values are too far apart\n";
		return false;
	}

	Matx33d W(0,-1,0,	//HZ 9.13
		1,0,0,
		0,0,1);
	Matx33d Wt(0,1,0,
		-1,0,0,
		0,0,1);
	R1 = svd_u * Mat(W) * svd_vt; //HZ 9.19
	R2 = svd_u * Mat(Wt) * svd_vt; //HZ 9.19
	t1 = svd_u.col(2); //u3
	t2 = -svd_u.col(2); //u3
#else
	//Using Horn E decomposition
	DecomposeEssentialUsingHorn90(E[0],R1[0],R2[0],t1[0],t2[0]);
#endif
	return true;
}

bool FindCameraMatrices(const Mat& K,
						const Mat& Kinv,
						const Mat& distcoeff,
						const vector<KeyPoint>& imgpts1,
						const vector<KeyPoint>& imgpts2,
						vector<KeyPoint>& imgpts1_good,
						vector<KeyPoint>& imgpts2_good,
						Matx34d& P,
						Matx34d& P1,
						vector<DMatch>& matches,
						vector<CloudPoint>& outCloud
#ifdef __SFM__DEBUG__
						,const Mat& img_1,
						const Mat& img_2
#endif
						)
{
	//Find camera matrices
	{
		cout << "Find camera matrices...";
		double t = getTickCount();

		Mat F = GetFundamentalMat(imgpts1,imgpts2,imgpts1_good,imgpts2_good,matches
#ifdef __SFM__DEBUG__
								  ,img_1,img_2
#endif
								  );
		if(matches.size() < 100) { // || ((double)imgpts1_good.size() / (double)imgpts1.size()) < 0.25
			cerr << "not enough inliers after F matrix" << endl;
			return false;
		}

		//Essential matrix: compute then extract cameras [R|t]
		Mat_<double> E = K.t() * F * K; //according to HZ (9.12)

		//according to http://en.wikipedia.org/wiki/Essential_matrix#Properties_of_the_essential_matrix
		if(fabsf(determinant(E)) > 1e-07) {
			cout << "det(E) != 0 : " << determinant(E) << "\n";
			P1 = 0;
			return false;
		}

		Mat_<double> R1(3,3);
		Mat_<double> R2(3,3);
		Mat_<double> t1(1,3);
		Mat_<double> t2(1,3);

		//decompose E to P' , HZ (9.19)
		{
			if (!DecomposeEtoRandT(E,R1,R2,t1,t2)) return false;

			if(determinant(R1)+1.0 < 1e-09) {
				//according to http://en.wikipedia.org/wiki/Essential_matrix#Showing_that_it_is_valid
				cout << "det(R) == -1 ["<<determinant(R1)<<"]: flip E's sign" << endl;
				E = -E;
				DecomposeEtoRandT(E,R1,R2,t1,t2);
			}
			if (!CheckCoherentRotation(R1)) {
				cout << "resulting rotation is not coherent\n";
				P1 = 0;
				return false;
			}

			P1 = Matx34d(R1(0,0),	R1(0,1),	R1(0,2),	t1(0),
						 R1(1,0),	R1(1,1),	R1(1,2),	t1(1),
						 R1(2,0),	R1(2,1),	R1(2,2),	t1(2));
			cout << "Testing P1 " << endl << Mat(P1) << endl;

			vector<CloudPoint> pcloud,pcloud1; vector<KeyPoint> corresp;
			double reproj_error1 = TriangulatePoints(imgpts1_good, imgpts2_good, K, Kinv, distcoeff, P, P1, pcloud, corresp);
			double reproj_error2 = TriangulatePoints(imgpts2_good, imgpts1_good, K, Kinv, distcoeff, P1, P, pcloud1, corresp);
			vector<uchar> tmp_status;
			//check if pointa are triangulated --in front-- of cameras for all 4 ambiguations
			if (!TestTriangulation(pcloud,P1,tmp_status) || !TestTriangulation(pcloud1,P,tmp_status) || reproj_error1 > 100.0 || reproj_error2 > 100.0) {
				P1 = Matx34d(R1(0,0),	R1(0,1),	R1(0,2),	t2(0),
							 R1(1,0),	R1(1,1),	R1(1,2),	t2(1),
							 R1(2,0),	R1(2,1),	R1(2,2),	t2(2));
				cout << "Testing P1 "<< endl << Mat(P1) << endl;

				pcloud.clear(); pcloud1.clear(); corresp.clear();
				reproj_error1 = TriangulatePoints(imgpts1_good, imgpts2_good, K, Kinv, distcoeff, P, P1, pcloud, corresp);
				reproj_error2 = TriangulatePoints(imgpts2_good, imgpts1_good, K, Kinv, distcoeff, P1, P, pcloud1, corresp);

				if (!TestTriangulation(pcloud,P1,tmp_status) || !TestTriangulation(pcloud1,P,tmp_status) || reproj_error1 > 100.0 || reproj_error2 > 100.0) {
					if (!CheckCoherentRotation(R2)) {
						cout << "resulting rotation is not coherent\n";
						P1 = 0;
						return false;
					}

					P1 = Matx34d(R2(0,0),	R2(0,1),	R2(0,2),	t1(0),
								 R2(1,0),	R2(1,1),	R2(1,2),	t1(1),
								 R2(2,0),	R2(2,1),	R2(2,2),	t1(2));
					cout << "Testing P1 "<< endl << Mat(P1) << endl;

					pcloud.clear(); pcloud1.clear(); corresp.clear();
					reproj_error1 = TriangulatePoints(imgpts1_good, imgpts2_good, K, Kinv, distcoeff, P, P1, pcloud, corresp);
					reproj_error2 = TriangulatePoints(imgpts2_good, imgpts1_good, K, Kinv, distcoeff, P1, P, pcloud1, corresp);

					if (!TestTriangulation(pcloud,P1,tmp_status) || !TestTriangulation(pcloud1,P,tmp_status) || reproj_error1 > 100.0 || reproj_error2 > 100.0) {
						P1 = Matx34d(R2(0,0),	R2(0,1),	R2(0,2),	t2(0),
									 R2(1,0),	R2(1,1),	R2(1,2),	t2(1),
									 R2(2,0),	R2(2,1),	R2(2,2),	t2(2));
						cout << "Testing P1 "<< endl << Mat(P1) << endl;

						pcloud.clear(); pcloud1.clear(); corresp.clear();
						reproj_error1 = TriangulatePoints(imgpts1_good, imgpts2_good, K, Kinv, distcoeff, P, P1, pcloud, corresp);
						reproj_error2 = TriangulatePoints(imgpts2_good, imgpts1_good, K, Kinv, distcoeff, P1, P, pcloud1, corresp);

						if (!TestTriangulation(pcloud,P1,tmp_status) || !TestTriangulation(pcloud1,P,tmp_status) || reproj_error1 > 100.0 || reproj_error2 > 100.0) {
							cout << "Shit." << endl;
							return false;
						}
					}
				}
			}
			for (unsigned int i=0; i<pcloud.size(); i++) {
				outCloud.push_back(pcloud[i]);
			}
		}

		t = ((double)getTickCount() - t)/getTickFrequency();
		cout << "Done. (" << t <<"s)"<< endl;
	}
	return true;
}


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::example::location_scale.cpp                        //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <algorithm>
#include <iterator>
#include <numeric>
#include <cmath>
#include <string>
#include <stdexcept>
#include <limits>

#include <boost/mpl/apply.hpp>
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <boost/range.hpp>

#include <boost/math/tools/precision.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

#include <boost/statistics/detail/distribution_common/functor/log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_common/distributions/reference/wrapper.hpp>

#include <boost/statistics/detail/distribution_toolkit/meta/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/students_t/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/include.hpp>
#include <boost/statistics/detail/distribution_toolkit/map_pdf/include.hpp>

#include <libs/statistics/detail/distribution_toolkit/example/distribution_function.h>

void example_distribution_function(std::ostream& out){

        out << "-> example_distribution_function" << std::endl;

        using namespace boost;
        using namespace math;
        using namespace statistics::detail;
        namespace dist = distribution;

        // Types
        typedef double                                          	val_;
        typedef boost::mt19937                                  	urng_;
        typedef boost::normal_distribution<val_>                	rnd_;
        typedef math::students_t_distribution<val_>             	stud_;
        typedef dist::toolkit::location_scale_distribution<stud_>   ls_stud_;
        typedef boost::variate_generator<urng_&,rnd_>           	vg_;
        typedef boost::numeric::bounds<val_> 						bounds_;

        // Constants
        const unsigned df   = 10;
        const val_ mu       = 10;
        const val_ sigma    = 2;
        const val_ x        = 2.132;

        // Initialization
        urng_ urng;

        stud_ stud(df);
        ls_stud_ ls_stud(mu, sigma, stud);

		out << "testing error handling : " << std::endl;

        try{
        	static const val_ inf = std::numeric_limits<val_>::infinity();
			ls_stud_ ls_stud(inf,sigma,stud);
        }catch(std::exception& e){
        	out << e.what() << std::endl;
		}

		try{
			ls_stud_(mu,-1.0,stud);
        }catch(std::exception& e){
        	out << e.what() << std::endl;
        }

        struct float_{

            static bool equal(const val_& a, const val_& b){
                static val_ e = boost::math::tools::epsilon<val_>();;
                return fabs(a-b)< e;
            }

        };
        {   // product_pdf
            typedef dist::toolkit::product_pdf<stud_,ls_stud_>     prod_dist_;
            prod_dist_ prod_dist(stud,ls_stud);
            prod_dist_ prod_dist2(stud,ls_stud);
            prod_dist = prod_dist2; // check assignment
            BOOST_ASSERT(
                float_::equal(
                    log_unnormalized_pdf(prod_dist,x),
                    log_unnormalized_pdf(prod_dist2,x)
                )
            );
            BOOST_ASSERT(
                float_::equal(
                    log_unnormalized_pdf(prod_dist.first(),x)
                     + log_unnormalized_pdf(prod_dist.second(),x),
                    log_unnormalized_pdf(prod_dist,x)
                )
            );

        }
        {   // inverse_pdf
            typedef dist::toolkit::inverse_pdf<stud_> inv_dist_;
            inv_dist_ inv_dist(stud);
            BOOST_ASSERT(
                float_::equal(
                    -log_unnormalized_pdf(inv_dist.distribution(),x),
                    log_unnormalized_pdf(inv_dist,x)
                )
            );
        }
        {   // ratio_pdf + fun_wrap
            typedef dist::toolkit::meta_ratio_pdf<stud_,ls_stud_>  	mf_;
            typedef mf_::type ratio_dist_;
            ratio_dist_ ratio_dist = mf_::call(stud,ls_stud);
            BOOST_ASSERT(
                float_::equal(
                    // + sign because inherits from inverse
                    log_unnormalized_pdf(ratio_dist.first(),x)
                        + log_unnormalized_pdf(ratio_dist.second(),x),
                    log_unnormalized_pdf(ratio_dist,x)
                )
            );
        }
    out << "<-" << std::endl;
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <vector>
#include <utility>

#include <boost/graph/adjacency_list.hpp>

/*
  Sample Output

  0 <--
  1 <-- 0
  2 <-- 1
  3 <-- 1
  4 <-- 2  3

 */

int main(int , char* [])
{
  using namespace boost;
  using namespace std;
  using namespace boost;

  typedef adjacency_list<listS,vecS,bidirectionalS> Graph;
  const int num_vertices = 5;
  Graph g(num_vertices);

  add_edge(0, 1, g);
  add_edge(1, 2, g);
  add_edge(1, 3, g);
  add_edge(2, 4, g);
  add_edge(3, 4, g);

  boost::graph_traits<Graph>::vertex_iterator i, end;
  boost::graph_traits<Graph>::in_edge_iterator ei, edge_end;

  for(boost::tie(i,end) = vertices(g); i != end; ++i) {
    cout << *i << " <-- ";
    for (boost::tie(ei,edge_end) = in_edges(*i, g); ei != edge_end; ++ei)
      cout << source(*ei, g) << "  ";
    cout << endl;
  }
  return 0;
}


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */

/*
 * Implement a NON-LINEAR range angle observer for many filter schemes
 * The model parameters allow model sizes and the non-linearity.
 * This provides an excellent vehicle to test each scheme. Use for regression testing
 */

#include "BayesFilter/allFilters.hpp"
#include "BayesFilter/schemeFlt.hpp"
#include "Test/random.hpp"
#include "angle.hpp"
#include <cmath>
#include <iostream>
#include <boost/numeric/ublas/io.hpp>
#include <boost/format.hpp>
#include <boost/limits.hpp>

using namespace Bayesian_filter;
using namespace Bayesian_filter_matrix;
using namespace angleArith;

const std::size_t NX = 2+1;			// State dimension (x,y) and some more to show noise coupling or singular X
const std::size_t NQ = 2;			// State dimension of noise
const std::size_t NZ = 2+2;			// Observation dimension (r,a) and some empty dummies
const std::size_t NS = 1000;			// Number of samples for a sampled representation

const bool RA_MODEL = true;		// Use Range angle NON-linear model (requires normalising angle)
const bool NOISE_MODEL = true;		// Add noise to truth model
const bool TRUTH_STATIONARY = false;// Truth model setup
const Float INIT_XY[2] = {1.,-0.2};	// XY initial position
const Float TARGET[2] = {-11.,0.};	// XY position of target

const Float RANGE_NOISE = NOISE_MODEL ? Float(0.1) : Float(1e-6);
const Float ANGLE_NOISE = NOISE_MODEL ? Float(5. * angle<Float>::Deg2Rad) : Float(1e-6);
const Float Z_CORRELATION = Float(0e-1);	// (Un)Correlated observation model

const Float X_NOISE = Float(0.05);		// predict model
const Float Y_NOISE = Float(0.09);
const Float XY_NOISE_COUPLING = Float(0.05);
const Float Q_NOISE = Float(1.0);		// Noise in addition Q terms
const Float G_COUPLING = Float(1.0);	// Coupling in addition G terms

const Float INIT_X_NOISE = Float(0.07);
const Float INIT_Y_NOISE = Float(0.10);
const Float INIT_XY_NOISE_CORRELATION = Float(0.4);
const Float INIT_2_NOISE = Float(0.09);	// Use zero for singular X
const Float INIT_2_NOISE_CORRELATION = Float(0.5);

// Square
template <class scalar>
inline scalar sqr(scalar x)
{
	return x*x;
}

class Rtheta_random : public Bayesian_filter_test::Boost_random, public SIR_random
/*
 * Random numbers for filters from Boost
 */
{
public:
	Float normal (const Float mean, const Float sigma)
	{
		Float f = Boost_random::normal (mean, sigma);
		return f;
	}
	void normal (DenseVec& v)
	{
		Boost_random::normal (v);
	}
	void uniform_01 (DenseVec& v)
	{
		Boost_random::uniform_01 (v);
	}
} Random, Random2;

/*
 * Linear predict model
 *  x static and y tending to x
 *  Correlated additive noise
 */
class pred_model : public Sampled_LiInAd_predict_model
{
public:
	pred_model();
};

pred_model::pred_model () :
	Sampled_LiInAd_predict_model(NX,NQ, Random2)
// Construct constant model
{
	// Build Fx, Identity all except active partition
	FM::identity (Fx);
	Fx(0,0) = Float(1);
	Fx(0,1) = Float(0);
	Fx(1,0) = Float(0.1);
	Fx(1,1) = Float(0.9);

	// Build q,G, Test addition parts using coupled noise
	q = ublas::scalar_vector<Float>(NQ, Q_NOISE);
	q[0] = sqr(X_NOISE);
	q[1] = sqr(Y_NOISE);
	G = ublas::scalar_matrix<Float>(NX,NQ, G_COUPLING);
	G(0,0) = 1;
	G(0,1) = XY_NOISE_COUPLING;
	G(1,0) = XY_NOISE_COUPLING;
	G(1,1) = 1;

	// Build inverse Fx, Identity all except active partition
	DenseColMatrix denseinvFx (Fx.size1(), Fx.size2());
	Information_root_scheme::inverse_Fx (denseinvFx, Fx);
	inv.Fx = denseinvFx;
}

/*
 * Observation model: gradient linearised versions
 *  Linearisation state must be set before use
 *  Both uncorrelated and correlated representation
 *  Correlated representation is just the uncorrelated adapted so the
 *  correlation term are added.
 */
#ifndef MODEL_LINEAR
	typedef General_LzUnAd_observe_model Observe_un_special;
	typedef General_LzCoAd_observe_model Observe_co_special;
#else
	typedef General_LiUnAd_observe_model Observe_un_special;
	typedef General_LiCoAd_observe_model Observe_co_special;
#endif

class uobs_model : public Observe_un_special
{
public:
	uobs_model();
	const Vec& h(const Vec& x) const;
	void normalise (Vec& z_denorm, const Vec& z_from) const;
	void state (const Vec& x);
private:
	mutable Vec z_pred;
};

class cobs_model : public Observe_co_special
{
public:
	cobs_model(uobs_model& u);
	const Vec& h(const Vec& x) const;
	void normalise (Vec& z_denorm, const Vec& z_from) const;
	void state (const Vec& x);
private:
	uobs_model& uobs;
};

uobs_model::uobs_model() :
	Observe_un_special(NX,NZ),
	z_pred(NZ)
{
	// Observation covariance uncorrelated
	Zv = ublas::scalar_vector<Float>(Zv.size(), Float(1));
	Zv[0] = sqr(RANGE_NOISE);
	Zv[1] = (RA_MODEL ? sqr(ANGLE_NOISE): sqr(RANGE_NOISE));
}

cobs_model::cobs_model(uobs_model& u) :
	Observe_co_special(NX,NZ),
	uobs(u)
{
	FM::identity (Z);
	// Create the correlation in Z
	Z(0,0) = Float(uobs.Zv[0]); Z(1,1) = Float(uobs.Zv[1]);	// ISSUE mixed type proxy assignment
	Z(1,0) = Float(Z(0,1) = sqrt(Z(0,0))*sqrt(Z(1,1))*Z_CORRELATION);
}

void uobs_model::state (const Vec& x)
{
	Float dx = TARGET[0] - x[0];
	Float dy = TARGET[1] - x[1];

	Hx.clear();
	if (RA_MODEL) {
		Float distSq = dx*dx + dy*dy;
		Float dist = sqrt (distSq);
		Hx(0,0) = -dx / dist;
		Hx(0,1) = -dy / dist;
		Hx(1,0) = +dy / distSq;
		Hx(1,1) = -dx / distSq;
	}
	else {
		Hx(0,0) = -1;
		Hx(0,1) = 0;
		Hx(1,0) = 0;
		Hx(1,1) = -1;
	}
}

void cobs_model::state (const FM::Vec& x)
{
	uobs.state (x);
	Hx = uobs.Hx;
}

const Vec& uobs_model::h (const Vec& x) const
{
	Float dx = TARGET[0] - x[0];
	Float dy = TARGET[1] - x[1];

	z_pred.clear();
	if (RA_MODEL) {
		Float distSq = dx*dx + dy*dy;
		Float dist = sqrt (distSq);

		z_pred[0] = dist;
		z_pred[1] = std::atan2 (dy, dx);
	}
	else {
		z_pred[0] = dx;
		z_pred[1] = dy;
	}
	return z_pred;
}

const Vec& cobs_model::h (const Vec& x) const
{
	return uobs.h(x);
}

void uobs_model::normalise (Vec& z_denorm, const Vec& z_from) const
{
	if (RA_MODEL) {
		z_denorm[1] = angle<Float>(z_denorm[1]).from (z_from[1]);
	}
}

void cobs_model::normalise (Vec& z_denorm, const Vec& z_from) const
{
	uobs.normalise(z_denorm, z_from);
}

/*
 * A dynamic system with noise, or a fixed additive noise
 */
class walk : private pred_model
{
public:
	walk (const Vec start, const bool fixed = false);
	void predict ();
	Vec x, x_pred, rootq;
private:
	bool m_fixed;
	Vec m_base;
};

walk::walk (const Vec start, const bool fixed) : x(NX), x_pred(NX), rootq(NQ), m_base(NX)
{
	m_fixed = fixed;
	m_base = start;
	x = m_base;
	for (Vec::const_iterator qi = q.begin(); qi != q.end(); ++qi) {
		rootq[qi.index()] = std::sqrt(*qi);
	}
}

void walk::predict ()
{
						// Correlated additive random noise
	DenseVec n(rootq.size()), nc(x.size());
	::Random.normal (n);		// independant zero mean normal
								// multiply elements by std dev
	for (DenseVec::iterator ni = n.begin(); ni != n.end(); ++ni) {
		*ni *= rootq[ni.index()];
	}
	nc = prod(G,n);				// correlate

	if (m_fixed) {				// Randomize based on assumed noise
		x = m_base + nc;
	}
	else {
		x_pred = f(x);
		x = x_pred;
		noalias(x) += nc;
	}
}

// Special for Information scheme using Linrz predict
class Information_linrz_scheme : public Information_scheme
{
public:
	Information_linrz_scheme (std::size_t x_size, std::size_t z_initialsize = 0) :
		Kalman_state_filter (x_size),
		Information_state_filter (x_size),
		Information_scheme (x_size, z_initialsize)
	{}
	Float predict (Linrz_predict_model& f)
	// Enforce use of Linrz predict
	{
		return Information_scheme::predict (f);
	}
};

// Filter_scheme Information_linrz_scheme specialisation
namespace Bayesian_filter {
template <>
Filter_scheme<Information_linrz_scheme>::Filter_scheme(std::size_t x_size, std::size_t q_maxsize, std::size_t z_initialsize) :
	Kalman_state_filter (x_size),
	Information_state_filter (x_size),
	Information_linrz_scheme (x_size, z_initialsize)
{}
}

/*
 * Filter under test. Initialised for state and covariance
 */
template <class TestScheme>
class Filter
{
public:
	Filter (const Vec& x_init, const SymMatrix& X_init);
	Filter_scheme<TestScheme> ts;
	template <class P>
	void predict (P& pmodel)
	{
		ts.predict (pmodel);
	}
	template <class O>
	void observe (O& omodel, const Vec& z)
	{
		ts.observe (omodel, z);
	}
	void update ()
	{
		ts.update ();
	}
	const Vec& x()
	{	return ts.x;
	}
	const SymMatrix& X()
	{	return ts.X;
	}
	void dump_state()
	{	// output any additional state variables
	}
};

template <class TestScheme>
Filter<TestScheme>::Filter (const Vec& x_init, const SymMatrix& X_init) :
	ts (x_init.size(), NQ, NZ)
{
	ts.init_kalman (x_init, X_init);
}

// Specialise for SIR_kalman
template <>
Filter<SIR_kalman_scheme>::Filter (const Vec& x_init, const SymMatrix& X_init) :
	ts (x_init.size(), NS, ::Random2)
{
	ts.init_kalman (x_init, X_init);
}

// dump_state specialisations
template <>
void Filter<Information_root_scheme>::dump_state()
{	// output any additional state variables
	std::cout << ts.r << ts.R << std::endl;
}

template <>
void Filter<Unscented_scheme>::dump_state()
{	// output any additional state variables
	std::cout << ts.XX << std::endl;
}

/*
 * Compare Two filters
 */

template<class Tf1, class Tf2>
class CCompare
{
public:
	CCompare (const Vec x_init, const SymMatrix X_init, unsigned nIterations);
private:
	void doIt (unsigned nIterations);
	// Attributes
	Tf1 f1;
	Tf2 f2;
	walk truth;
	pred_model f;
	uobs_model uh;
	cobs_model ch;

	// Implementation
	void dumpCompare ();

	Vec ztrue, z;

	DenseVec f1_xpred, f2_xpred;
	DenseSymMatrix f1_Xpred;
	DenseSymMatrix f2_Xpred;
};

template<class Tf1, class Tf2>
CCompare<Tf1,Tf2>::CCompare (const Vec x_init, const SymMatrix X_init, unsigned nIterations) :
	f1(x_init, X_init),
	f2(x_init, X_init),
	truth (x_init, TRUTH_STATIONARY),
	uh(),
	ch(uh),
	ztrue(NZ), z(NZ),
	f1_xpred (NX),
	f2_xpred (NX),
	f1_Xpred (NX, NX),
	f2_Xpred (NX, NX)
{
	// Initialises test variables
	z.clear();

	f1_xpred.clear();
	f2_xpred.clear();
	f1_Xpred.clear();
	f2_Xpred.clear();

	doIt (nIterations);
}

template<class Tf1, class Tf2>
void CCompare<Tf1, Tf2>::dumpCompare ()
{
	// Additional Scheme state
	//f1.dump_state(); f2.dump_state();

	Float zx, zy;
	if (RA_MODEL) {
		zx = truth.x[0] + z[0] * cos (z[1]);
		zy = truth.x[1] + z[0] * sin (z[1]);
	}
	else {
		zx = truth.x[0] + z[0];
		zy = truth.x[1] + z[1];
	}

	// Comparison
	{
		using std::cout; using std::endl;
		using boost::format;
		const Vec& f1x = f1.x(); const SymMatrix& f1X = f1.X();
		const Vec& f2x = f2.x(); const SymMatrix& f2X = f2.X();

		// Comparison and truth line
		//	x(0)diff, x(1)diff, truth.x(0), truth.x(1), zx, zy)
		cout << format("*%11.4g %11.4g * %10.3f %10.3f  %10.3f %10.3f")
			 	% (f1x[0]-f2x[0]) % (f1x[1]-f2x[1])
			 	% Float(truth.x[0]) % Float(truth.x[1]) % zx % zy << endl;	// ISSUE mixed type proxy assignment

		format state(" %11.4g %11.4g * %10.3f %10.3f");
		format covariance(" %12.4e %12.4e %12.4e");

		// Filter f1 performance
		//		x[0]err, x[1]err, x[0], x[1],  Xpred*3, X*3
		cout << state % (f1x[0]-truth.x[0]) % (f1x[1]-truth.x[1])
				% f1x[0] % f1x[1] << endl;
		cout << covariance % f1_Xpred(0,0) % f1_Xpred(1,1) % f1_Xpred(1,0);
		cout << covariance % f1X(0,0) % f1X(1,1) % f1X(1,0) << endl;
		// Filter f2 performance
		//		x[0]err, x[1]err, x[0], x[1], x[01]dist,  Xpred*3, X*3
		cout << state % (f2x[0]-truth.x[0]) % (f2x[1]-truth.x[1])
				% f2x[0] % f2x[1] << endl;
		cout << covariance % f2_Xpred(0,0) % f2_Xpred(1,1) % f2_Xpred(1,0);
		cout << covariance % f2X(0,0) % f2X(1,1) % f2X(1,0) << endl;
	}
}

template<class Tf1, class Tf2>
void CCompare<Tf1, Tf2>::doIt (unsigned nIterations)
{
	// Update the filter x,X representation
	f1.update (); f2.update ();
	z = ztrue = uh.h(truth.x);
	dumpCompare();

	for (unsigned i = 0; i < nIterations; i++ ) {
		truth.predict ();		// Predict truth model
		f1.predict (f);			// Predict filters
		f2.predict (f);

		// Update the filter
		f1.update (); f2.update ();

		f1_xpred = f1.x(); f2_xpred = f2.x();
		f1_Xpred = f1.X(); f2_Xpred = f2.X();

		// Observation, true and Randomize based on an uncorrelated noise model
		ztrue = uh.h(truth.x);
		if (NOISE_MODEL) {
			z[0] = Random.normal (ztrue[0], sqrt(uh.Zv[0]) );
			z[1] = Random.normal (ztrue[1], sqrt(uh.Zv[1]) );
		}
		else
			z = ztrue;

		// Observe using model linearised about filter state estimate
		if (Z_CORRELATION == 0)
		{
			uh.state(f1.x()); f1.observe (uh, z);
			uh.state(f2.x()); f2.observe (uh, z);
		}
		else
		{
			ch.state(f1.x()); f1.observe (ch, z);
			ch.state(f2.x()); f2.observe (ch, z);
		}

		// Update the filter
		f1.update (); f2.update ();

		dumpCompare();
		// DEBUG char c;std::cin>>c;
	}
}

int main()
{
	// Other things I might want to test
	extern void other_tests();
	other_tests();

	// Use a know sequence for comparisons between systems
	Random.seed();
	Random2.seed();

	// Setup the test filters
		// Cartesian start position (in meters)
	Vec x_init (NX);
	x_init.clear();
 	x_init[0] = INIT_XY[0];
	x_init[1] = INIT_XY[1];
		// Initial state covariance, correlated
	SymMatrix X_init (NX, NX);
	FM::identity (X_init);
	X_init(0,0) = sqr(INIT_X_NOISE);
	X_init(1,1) = sqr(INIT_Y_NOISE);
	X_init(1,0) = X_init(0,1) = INIT_X_NOISE*INIT_Y_NOISE*INIT_XY_NOISE_CORRELATION;
	// Additional state correlation is useful for testing
		X_init(2,2) = sqr(INIT_2_NOISE);
	X_init(2,0) = X_init(0,2) = INIT_X_NOISE*INIT_2_NOISE*INIT_2_NOISE_CORRELATION;
	X_init(2,1) = X_init(1,2) = INIT_Y_NOISE*INIT_2_NOISE*INIT_2_NOISE_CORRELATION;

	// Initialise and do the comparison
	std::cout << "udfilter, ufilter " << "RA_MODEL:" << RA_MODEL << " NOISE_MODEL:" << NOISE_MODEL << " TRUTH_STATIONARY:" << TRUTH_STATIONARY << std::endl;
	Random.seed();
	CCompare<Filter<UD_scheme>, Filter<Unscented_scheme> > test1(x_init, X_init, 4);
	std::cout << std::endl;

	std::cout << "cfilter, ifilter " << "RA_MODEL:" << RA_MODEL << " NOISE_MODEL:" << NOISE_MODEL << " TRUTH_STATIONARY:" << TRUTH_STATIONARY << std::endl;
	Random.seed();
	CCompare<Filter<Covariance_scheme>, Filter<Information_linrz_scheme> > test2(x_init, X_init, 4);
	std::cout << std::endl;

	std::cout << "irfilter, ilfilter " << "RA_MODEL:" << RA_MODEL << " NOISE_MODEL:" << NOISE_MODEL << " TRUTH_STATIONARY:" << TRUTH_STATIONARY << std::endl;
	Random.seed();
	CCompare<Filter<Information_root_scheme>, Filter<Information_scheme> > test3(x_init, X_init, 4);
	std::cout << std::endl;

	std::cout << "sfilter, itfilter " << "RA_MODEL:" << RA_MODEL << " NOISE_MODEL:" << NOISE_MODEL << " TRUTH_STATIONARY:" << TRUTH_STATIONARY << std::endl;
	Random.seed();
	CCompare<Filter<SIR_kalman_scheme>, Filter<Iterated_covariance_scheme> > test4(x_init, X_init, 4);
	std::cout << std::endl;

	return 0;
}


// File: compressed2D.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;

    // CRS matrix
    compressed2D<double>   A(12, 12);

    // Laplace operator discretized on a 3x4 grid
    mat::laplacian_setup(A, 3, 4);
    std::cout << "A is \n" << A;

    // Element access is allowed for reading
    std::cout << "A[3][2] is " << A[3][2] << "\n\n";

    // CCS matrix
    compressed2D<float, mat::parameters<tag::col_major> > B(10, 10);

    // Assign the identity matrix times 3 to B
    B= 3;
    std::cout << "B is \n" << B << "\n";

    return 0;
}


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include <boost/test/unit_test.hpp>

#include "Molassembler/Temple/Optimization/SO3NelderMead.h"

#include <iostream>

using namespace Scine::Molassembler;

BOOST_AUTO_TEST_CASE(SO3NelderMead, *boost::unit_test::label("Temple")) {
  struct EigenValueDecomposition {
    static inline double square(double x) noexcept {
      return x * x;
    }

    double operator() (const Eigen::Matrix3d& rotation) const {
      Eigen::Matrix3d X;
      X <<  5,  2,  1,
            2,  7,  3,
            1,  3, 10;
      Eigen::Matrix3d partiallyDiagonal = rotation * X * rotation.transpose();
      double offDiagonalSquares = 0;
      for(unsigned i = 0; i < 2; ++i) {
        for(unsigned j = i + 1; j < 3; ++j) {
          offDiagonalSquares += square(partiallyDiagonal(i, j)) + square(partiallyDiagonal(j, i));
        }
      }
      return offDiagonalSquares;
    }
  };

  struct NelderMeadChecker {
    bool shouldContinue(unsigned iteration, double lowestValue, double stddev) {
      //std::cout << "Iteration " << iteration << ": " << lowestValue << " +- " << stddev << "\n";
      return iteration < 1000 && lowestValue >= 1e-5 && stddev > 1e-7;
    }
  };

  using OptimizerType = Temple::SO3NelderMead<>;

  { /* Squared distance commutative */
    const auto A = OptimizerType::Manifold::randomRotation();
    const auto B = OptimizerType::Manifold::randomRotation();
    BOOST_CHECK_MESSAGE(
      std::fabs(OptimizerType::Manifold::distanceSquared(A, B) - OptimizerType::Manifold::distanceSquared(B, A)) < 1e-10,
      "Squared distance calculation is not commutative"
    );
  }

  { /* Geodesic interpolation works */
    const Eigen::Matrix3d zPi = Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()).toRotationMatrix();
    const Eigen::Matrix3d zPiHalf = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()).toRotationMatrix();
    BOOST_CHECK_MESSAGE(
      OptimizerType::Manifold::geodesic(zPi, zPiHalf, 0).isApprox(zPiHalf, 1e-4),
      "geodesicExtrapolation(a, b, 0) != b"
    );
    BOOST_CHECK_MESSAGE(
      OptimizerType::Manifold::geodesic(zPi, zPiHalf, 1).isApprox(zPi, 1e-4),
      "geodesicExtrapolation(a, b, 1) != a"
    );

    const Eigen::AngleAxisd interpolation {OptimizerType::Manifold::geodesic(zPiHalf, zPi, 0.5)};
    BOOST_CHECK_MESSAGE(
      interpolation.axis().isApprox(Eigen::Vector3d::UnitZ(), 1e-4),
      "Axis of interpolated rotation is no longer z, but: " << interpolation.axis().transpose()
    );
    BOOST_CHECK_MESSAGE(
      std::fabs(interpolation.angle() - 3 * M_PI / 4) < 1e-4,
      "Angle of interpolated rotation is not 3 pi / 4 (" << (3 * M_PI / 4) << "), but " << interpolation.angle()
    );
  }

  bool pass = false;
  OptimizerType::OptimizationReturnType result;
  const unsigned maxAttempts = 10;
  for(unsigned i = 0; i < maxAttempts; ++i) {
    auto simplexVertices = OptimizerType::randomParameters();
    result = Temple::SO3NelderMead<>::minimize(
      simplexVertices,
      EigenValueDecomposition {},
      NelderMeadChecker {}
    );
    if(std::fabs(result.value) < 1e-2) {
      pass = true;
      break;
    }

    std::cout << "Attempt " << i << " lowest SO(3) minim. value = " << result.value << "\n";
  }

  BOOST_CHECK_MESSAGE(
    pass,
    "SO(3) Nelder-Mead does not find minimization of EigenValueDecomposition problem in three attempts, value is "
    << result.value << " after " << result.iterations
    << " iterations."
  );
}


#include "ode45.hpp"
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Sparse>

using namespace Eigen;
using namespace std;

// Compute the maps Phi and W at time T, for initial data given by u0 and v0.
pair<Vector2d,Matrix2d> PhiAndW(double u0, double v0, double T) {
    auto f = [] (const VectorXd & w) {

        // TODO: the right hand side of the system of ODEs related to Phi and W.

    };

    ode45<Eigen::VectorXd> O(f);
    O.options.rtol = 1e-14;
    O.options.atol = 1e-12;

    // TODO

}

// Apply the Newton method to find initial data giving solutions with period equal to 5.
int main(){

    // TODO

}

Nummerical Methods for CSE/PS13/templates_ps13/LV_template.cpp


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

#include <boost/timer.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <sophus/se3.h>

/* parameters */
const int boarder = 20;         // boundary
const int width = 640;
const int height = 480;

const double fx = 481.2f;
const double fy = -480.0f;
const double cx = 319.5f;
const double cy = 239.5f;

const int ncc_window_size = 2;  // NCC window width half size
const int ncc_area = (2*ncc_window_size + 1) * (2*ncc_window_size + 1);     // NCC window area size

const double min_cov = 0.1f;    // convergence condition: minimum variance
const double max_cov = 10.f;    // divergence  condition: maximum variance

/* functions declaration */
bool readDatasetFiles(
    const std::string& path,
    std::vector<std::string>& color_image_files,
    std::vector<Sophus::SE3>& poses
);

bool update(
    const cv::Mat& ref,
    const cv::Mat& curr,
    const Sophus::SE3& T_c_r,
    cv::Mat& depth,
    cv::Mat& depth_cov
);

bool epipolarSearch(
    const cv::Mat& ref,
    const cv::Mat& curr,
    const Sophus::SE3& T_c_r,
    const Eigen::Vector2d& pt_ref,
    const double& depth_mu,
    const double& depth_cov,
    Eigen::Vector2d& pt_curr
);

bool updateDepthFilter(
    const Eigen::Vector2d& pt_ref,
    const Eigen::Vector2d& pt_curr,
    const Sophus::SE3& T_c_r,
    cv::Mat& depth,
    cv::Mat& depth_cov
);

double computeNCC(
    const cv::Mat& ref,
    const cv::Mat& curr,
    const Eigen::Vector2d& pt_ref,
    const Eigen::Vector2d& pt_curr
);

inline double getBilinearInterpolatedValue(const cv::Mat& image, const Eigen::Vector2d& pt)
{
    uchar* d = & image.data[int(pt.y()) * image.step + int(pt.x())];

    double xx = pt.x() - floor(pt.x());
    double yy = pt.y() - floor(pt.y());

    return ( (1.f-xx) * (1.f-yy) * double(d[0]) +
                xx    * (1.f-yy) * double(d[1]) +
             (1.f-xx) *    yy    * double(d[image.step]) +
                xx    *    yy    * double(d[image.step + 1]) ) / 255.0;
}

/* some tools functions */
void plotDepth(const cv::Mat& depth);

inline Eigen::Vector3d px2cam(const Eigen::Vector2d px)
{
    return Eigen::Vector3d(
        (px.x() - cx) / fx,
        (px.y() - cy) / fy,
        1.0
    );
}

inline Eigen::Vector2d cam2px(const Eigen::Vector3d p_cam)
{
    return Eigen::Vector2d(
        p_cam.x() * fx / p_cam.z() + cx,
        p_cam.y() * fy / p_cam.z() + cy
    );
}

inline bool inside(const Eigen::Vector2d& pt)
{
    return (pt.x() >= boarder && pt.y() >= boarder
        &&  pt.x() + boarder <= width && pt.y() + boarder <= height);
}

void showEpipolarMatch(
    const cv::Mat& ref,
    const cv::Mat& curr,
    const Eigen::Vector2d& px_ref,
    const Eigen::Vector2d& px_curr
);

void showEpipolarLine(
    const cv::Mat& ref,
    const cv::Mat& curr,
    const Eigen::Vector2d& px_ref,
    const Eigen::Vector2d& px_mincurr,
    const Eigen::Vector2d& px_maxcurr
);

int main(int argc, char** argv)
{
    if (argc != 2) {
        std::cout << "Usage: dense_mapping_monocular <path_to_test_dataset>" << std::endl;
        exit(EXIT_FAILURE);
    }

    // read the data form the dataset
    std::vector<std::string> color_image_files;
    std::vector<Sophus::SE3> poses_T_w_c;
    bool ret = readDatasetFiles(argv[1], color_image_files, poses_T_w_c);
    if (ret == false) {
        std::cout << "Reading files failed" << std::endl;
        exit(EXIT_FAILURE);
    }

    std::cout << "read total " << color_image_files.size() << " files." << std::endl;

    // first image
    cv::Mat ref = cv::imread(color_image_files[0], 0);      // garg-scale image
    Sophus::SE3 pose_ref_Twc = poses_T_w_c[0];
    double init_depth = 3.0;
    double init_cov2 = 3.0;
    cv::Mat depth(height, width, CV_64F, init_depth);
    cv::Mat depth_cov(height, width, CV_64F, init_cov2);

    for (int index = 1; index < color_image_files.size(); index ++) {
        std::cout << "*** loop " << index << " ***" << std::endl;
        cv::Mat curr = cv::imread(color_image_files[index], 0);
        if (curr.data == nullptr) continue;
        Sophus::SE3 pose_curr_Twc = poses_T_w_c[index];
        Sophus::SE3 pose_curr_ref = pose_curr_Twc.inverse() * pose_ref_Twc;
        update(ref, curr, pose_curr_ref, depth, depth_cov);
        plotDepth(depth);
        cv::imshow("image", curr);
        cv::waitKey(1);
    }

    std::cout << "estimation returens, saving depth map ..." << std::endl;
    cv::imwrite("depth.png", depth);
    std::cout << "done." << std::endl;

    return 0u;
}

bool readDatasetFiles(const std::string& path, std::vector<std::string>& color_image_files, std::vector<Sophus::SE3>& poses)
{
    std::ifstream fin(path + "/first_200_frames_traj_over_table_input_sequence.txt");
    if (!fin) return false;

    while(!fin.eof()) {
        /* the form of dataset : image's name, tx, ty, tz, qx, qy, qz, qw. NOTE: the transformation matrix is camera to world */
        std::string image_name;
        fin >> image_name;

        double data[7];
        for (double& d : data) fin >> d;

        color_image_files.push_back(path + std::string("/images/") + image_name);
        poses.push_back(
            Sophus::SE3(Sophus::Quaterniond(data[6], data[3], data[4], data[5]),
                        Sophus::Vector3d(data[0], data[1], data[2]))
        );

        if (!fin.good()) break;
    }

    return true;
}

/* update the global depth map */
bool update(const cv::Mat& ref, const cv::Mat& curr, const Sophus::SE3& T_c_r, cv::Mat& depth, cv::Mat& depth_cov)
{
#pragma omp parallel for
    for (int x = boarder; x < width - boarder; x++) {
#pragma omp parallel for
        for (int y = boarder; y < height - boarder; y++) {
            // Traversing every pixel
            if (depth_cov.ptr<double>(y)[x] < min_cov || depth_cov.ptr<double>(y)[x] > max_cov)     // the depth was convergence / divergence
                continue;

            // match the pixel in the epipolar between the reference and the current
            Eigen::Vector2d pt_curr;
            bool ret = epipolarSearch(
                ref,
                curr,
                T_c_r,
                Eigen::Vector2d(x, y),
                depth.ptr<double>(y)[x],
                sqrt(depth_cov.ptr<double>(y)[x]),
                pt_curr
            );

            if (ret == false)   // match false
                continue;

            // 取消该注释以显示匹配
//             showEpipolarMatch(ref, curr, Eigen::Vector2d(x, y), pt_curr);

            // update the depth map
            updateDepthFilter(Eigen::Vector2d(x, y), pt_curr, T_c_r, depth, depth_cov);
        }
    }
    return true;
}

bool epipolarSearch(const cv::Mat& ref, const cv::Mat& curr, const Sophus::SE3& T_c_r, const Eigen::Vector2d& pt_ref, const double& depth_mu, const double& depth_cov, Eigen::Vector2d& pt_curr)
{
    Eigen::Vector3d f_ref = px2cam(pt_ref);
    f_ref.normalize();
    Eigen::Vector3d p_ref = f_ref*depth_mu;

    Eigen::Vector2d px_mean_curr = cam2px(T_c_r * p_ref);           // 按深度均值投影的像素
    double d_min = depth_mu - 3*depth_cov, d_max = depth_mu + 3*depth_cov;
    if (d_min < 0.1) d_min = 0.1;
    Eigen::Vector2d px_min_curr = cam2px(T_c_r * (f_ref * d_min));  // 按最小深度投影的像素
    Eigen::Vector2d px_max_curr = cam2px(T_c_r * (f_ref * d_max));  // 按最大深度投影的像素

    Eigen::Vector2d epipolar_line = px_max_curr - px_min_curr;      // 极线（线段形式）
    Eigen::Vector2d epipolar_direction = epipolar_line;             // 极线方向
    epipolar_direction.normalize();
    double half_length = 0.5 * epipolar_line.norm();                // 极线线段的半长度
    if (half_length > 100) half_length = 100;                       // 我们不希望搜索太多东西

    // if the epipolar line length <= 1.5 pixel, we regard that the mean point is the matching point
    if (half_length <= 1.5) {
        pt_curr = px_mean_curr;
        if (!inside(pt_curr))
            return false;
        return true;
    }

    // 取消此句注释以显示极线（线段）
//     showEpipolarLine(ref, curr, pt_ref, px_min_curr, px_max_curr);

    // 在极线上搜索，以深度均值点为中心，左右各取半长度
    double best_ncc = -1.0;
    Eigen::Vector2d best_px_curr;
    for (double l = -half_length; l <= half_length; l += 0.7){      // l += sqrt(2)
        Eigen::Vector2d px_curr = px_mean_curr + l * epipolar_direction;    // 待匹配点
        if (!inside(px_curr))
            continue;
        // 计算待匹配点与参考帧的 NCC
        double ncc = computeNCC(ref, curr, pt_ref, px_curr);
        if (ncc > best_ncc) {
            best_ncc = ncc;
            best_px_curr = px_curr;
        }
    }
    if (best_ncc < 0.85f)       // 只相信 NCC 很高的匹配
        return false;

    pt_curr = best_px_curr;

    return true;
}

double computeNCC(const cv::Mat& ref, const cv::Mat& curr, const Eigen::Vector2d& pt_ref, const Eigen::Vector2d& pt_curr)
{
    // 零均值-归一化互相关
    // 先算均值
    double mean_ref = 0, mean_curr = 0;
    std::vector<double> values_ref, values_curr;    // 参考帧和当前帧的均值
    for (int x = -ncc_window_size; x <= ncc_window_size; x ++) {
        for (int y = -ncc_window_size; y <= ncc_window_size; y ++) {
            double value_ref = double(ref.ptr<uchar>(int(y + pt_ref.y()))[int(x + pt_ref.x())]) / 255.0;
            mean_ref += value_ref;

//             double value_curr = double(curr.ptr<uchar>(int(y + pt_curr.y()))[int(x + pt_curr.x())]) / 255.0;
            double value_curr = getBilinearInterpolatedValue(curr, pt_curr + Eigen::Vector2d(x, y));
            mean_curr += value_curr;

            values_ref.push_back(value_ref);
            values_curr.push_back(value_curr);
        }
    }

    mean_ref /= ncc_area;
    mean_curr /= ncc_area;

    // compute Zero mean NCC
    double numerator = 0, denominator1 = 0, denominator2 = 0;
    for (int i = 0; i < values_ref.size(); i++) {
        double n = (values_ref[i] - mean_ref) * (values_curr[i] - mean_curr);
        numerator += n;
        denominator1 += (values_ref[i] - mean_ref) * (values_ref[i] - mean_ref);
        denominator2 += (values_curr[i] - mean_curr) * (values_curr[i] - mean_curr);
    }
    return numerator / sqrt(denominator1 * denominator2 + 1e-10);   // 防止分母出现零
}

bool updateDepthFilter(const Eigen::Vector2d& pt_ref, const Eigen::Vector2d& pt_curr, const Sophus::SE3& T_c_r, cv::Mat& depth, cv::Mat& depth_cov)
{
    // 用三角化计算深度
    Sophus::SE3 T_r_c = T_c_r.inverse();
    Eigen::Vector3d f_ref = px2cam(pt_ref);
    f_ref.normalize();
    Eigen::Vector3d f_curr = px2cam(pt_curr);
    f_curr.normalize();

    /**
     * function:
     * d_ref * f_ref = d_cur * (R_r_c * f_cur) + t_r_c
     * => [f_ref^T f_ref, -f_ref^T f_cur] [d_ref] = [f_ref^T t]
     *    [f_cur^T f_ref, -f_cur^T f_cur] [d_cur] = [f_cur^T t]
     * 二阶方程用克莱默法则求解并解之
     */
    Eigen::Vector3d t = T_r_c.translation();
    Eigen::Vector3d f2 = T_r_c.rotation_matrix() * f_curr;
    Eigen::Vector2d b = Eigen::Vector2d(t.dot(f_ref), t.dot(f2));

    double A[4];
    A[0] = f_ref.dot(f_ref);
    A[2] = f_ref.dot(f2);
    A[1] = -A[2];
    A[3] = -f2.dot(f2);

    double d = A[0]*A[3] - A[1]*A[2];
    Eigen::Vector2d lambda_vec = Eigen::Vector2d(
        A[3]*b(0, 0) - A[1]*b(1, 0),
        A[0]*b(1, 0) - A[2]*b(0, 0)) / d;

    Eigen::Vector3d xm = lambda_vec(0, 0) * f_ref;
    Eigen::Vector3d xn = lambda_vec(1, 0) * f2 + t;
    Eigen::Vector3d d_esti = (xm + xn) / 2.0;       // 三角化算得的深度向量
    double depth_estimation = d_esti.norm();        // 深度值

    // 计算不确定性（以一个像素为误差）
    Eigen::Vector3d p = f_ref*depth_estimation;
    Eigen::Vector3d a = p - t;
    double t_norm = t.norm();
    double a_norm = a.norm();
    double alpha = acos(f_ref.dot(t) / t_norm);
    double beta = acos(a.dot(-t) / a_norm / t_norm);
    double beta_prime = beta + atan(1.0 / fx / 2.0) * 2.0;
    double gamma = M_PI - alpha - beta_prime;
    double p_prime = t_norm * sin(beta_prime) / sin(gamma);
    double d_cov = p_prime - depth_estimation;
    double d_cov2 = d_cov * d_cov;

    // 高斯融合
    double mu = depth.ptr<double>(int(pt_ref.y()))[int(pt_ref.x())];
    double sigma2 = depth_cov.ptr<double>(int(pt_ref.y()))[int(pt_ref.x())];

    double mu_fuse = (d_cov2*mu + sigma2*depth_estimation) / (d_cov2 + sigma2);
    double sigma2_fuse = (d_cov2*sigma2) / (d_cov2 + sigma2);

    depth.ptr<double>(int(pt_ref.y()))[int(pt_ref.x())] = mu_fuse;
    depth_cov.ptr<double>(int(pt_ref.y()))[int(pt_ref.x())] = sigma2_fuse;

    return true;
}

void plotDepth(const cv::Mat& depth)
{
    cv::imshow("depth", depth * 0.4);
    cv::waitKey(1);
}

void showEpipolarMatch(const cv::Mat& ref, const cv::Mat& curr, const Eigen::Vector2d& px_ref, const Eigen::Vector2d& px_curr)
{
    cv::Mat ref_show, curr_show;
    cv::cvtColor(ref, ref_show, CV_GRAY2BGR);
    cv::cvtColor(curr, curr_show, CV_GRAY2BGR);

    cv::circle(ref_show, cv::Point2f(px_ref.x(), px_ref.y()), 5, cv::Scalar(0, 0, 250), 2);
    cv::circle(curr_show, cv::Point2f(px_curr.x(), px_curr.y()), 5, cv::Scalar(0, 0, 250), 2);

    cv::imshow("ref", ref_show);
    cv::imshow("curr", curr_show);
    cv::waitKey(1);
}

void showEpipolarLine(const cv::Mat& ref, const cv::Mat& curr, const Eigen::Vector2d& px_ref, const Eigen::Vector2d& px_mincurr, const Eigen::Vector2d& px_maxcurr)
{
    cv::Mat ref_show, curr_show;
    cv::cvtColor(ref, ref_show, CV_GRAY2BGR);
    cv::cvtColor(curr, curr_show, CV_GRAY2BGR);

    cv::circle(ref_show, cv::Point2f(px_ref.x(), px_ref.y()), 5, cv::Scalar(0, 250, 0), 2);
    cv::circle(curr_show, cv::Point2f(px_mincurr.x(), px_mincurr.y()), 5, cv::Scalar(0, 250, 2), 2);
    cv::circle(curr_show, cv::Point2f(px_maxcurr.x(), px_maxcurr.y()), 5, cv::Scalar(0, 250, 2), 2);
    cv::line(curr_show, cv::Point2f(px_mincurr.x(), px_mincurr.y()), cv::Point2f(px_maxcurr.x(), px_maxcurr.y()), cv::Scalar(0, 250, 0), 1);

    cv::imshow("ref", ref_show);
    cv::imshow("curr", curr_show);
    cv::waitKey(1);
}


//
//  Copyright (c) 2013 Vladimir Chalupecky
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to
//  deal in the Software without restriction, including without limitation the
//  rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
//  sell copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
//  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
//  IN THE SOFTWARE.

#ifndef TPS_TPS_HPP
#define TPS_TPS_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Cholesky>

#include <boost/assert.hpp>
#include <boost/type_traits/conditional.hpp>

#include <cmath>
#include <stdexcept>
#include <vector>
#include <iostream>

namespace tps {
namespace detail {

template< int k >
struct Polyharmonic_spline_kernel_odd
{
    double operator()(double r)
    {
        return std::pow(r, k);
    }
};

template< int k >
struct Polyharmonic_spline_kernel_even
{
    double operator()(double r)
    {
        if (r < 1.0)
        {
            return std::pow(r, k - 1) * std::log(std::pow(r, r));
        }

        return std::pow(r, k) * std::log(r);
    }
};

} // namespace detail

template< int k >
struct Polyharmonic_spline_kernel : public boost::conditional < k % 2,
        detail::Polyharmonic_spline_kernel_odd<k>,
        detail::Polyharmonic_spline_kernel_even<k> >::type
{};

template< int Domain_dim, int Range_dim, typename Kernel_function >
class Polyharmonic_spline_transformation
{
public:
    typedef Eigen::Matrix< double, Domain_dim, 1 > Domain_point;
    typedef Eigen::Matrix< double, Range_dim, 1 >  Range_point;

private:
    typedef std::vector< Domain_point > Domain_points_container;
    typedef std::vector< Range_point> Range_points_container;
    typedef Eigen::MatrixXd Matrix;

public:
    template< typename Forward_iterator_1, typename Forward_iterator_2 >
    Polyharmonic_spline_transformation(
            Forward_iterator_1 domain_points_begin,
            Forward_iterator_1 domain_points_end,
            Forward_iterator_2 range_points_begin,
            Forward_iterator_2 range_points_end,
            double max_relative_error = 1.0e-8
    )
        : domain_points_(domain_points_begin, domain_points_end)
        , bending_norm_(0.0)
        , max_relative_error_(max_relative_error)
    {
        if (domain_points_.size() < Domain_dim + 1)
        {
            throw
                std::runtime_error(
                    "Polyharmonic_spline_transformation(): "
                    "Insufficient number of corresponding pairs given"
                );
        }

        assemble_L_matrix();
        set_range_points(range_points_begin, range_points_end);
    }

    Range_point transform(Domain_point p)
    {
        std::size_t N = domain_points_.size();
        std::size_t M = N + 1 + Domain_dim;

        Range_point result = Wa_.block<1, Range_dim>(N, 0).transpose();

        for (std::size_t i = 0; i < Domain_dim; ++i)
        {
            result += p(i) * Wa_.block<1, Range_dim>(N + 1 + i, 0).transpose();
        }

        for (std::size_t i = 0; i < N; ++i)
        {
            if (p != domain_points_[i])
            {
                result += kernel_((domain_points_[i] - p).norm())
                    * Wa_.block<1, Range_dim>(i, 0).transpose();
            }
        }

        return result;
    }

    template < typename Forward_iterator >
    void set_range_points(Forward_iterator begin, Forward_iterator end)
    {
        if (domain_points_.size() != std::distance(begin, end))
        {
            throw
                std::runtime_error(
                    "Polyharmonic_spline_transformation::set_range_points(): "
                    "The number of domain and range points must be the "
                    "same."
                );
        }

        assemble_Vt_matrix(begin, end);
        update_Wa_matrix();
    }

    double integral_bending_norm() const
    {
        return bending_norm_;
    }

private:
    template < typename Forward_iterator >
    void assemble_Vt_matrix(Forward_iterator begin, Forward_iterator end)
    {
        std::size_t N = domain_points_.size();
        std::size_t M = N + 1 + Domain_dim;

        Vt_.resize(M, Range_dim);
        std::size_t i = 0;

        for (Forward_iterator it = begin; it != end; ++it, ++i)
        {
            Vt_.block<1, Range_dim>(i, 0) = *it;
        }

        Vt_.bottomLeftCorner < Domain_dim + 1, Range_dim > ().setZero();
    }

    void assemble_L_matrix()
    {
        std::size_t N = domain_points_.size();
        std::size_t M = N + 1 + Domain_dim;

        L_.resize(M, M);

        for (std::size_t i = 0; i < N; ++i)
        {
            L_(i, i) = 0.0;

            for (std::size_t j = i + 1; j < N; ++j)
            {
                if (domain_points_[i] == domain_points_[j])
                {
                    throw std::runtime_error(
                        "Polyharmonic_spline_transformation::assemble_L_matrix(): "
                        "Degenerate input points");
                }

                double d = (domain_points_[i] - domain_points_[j]).norm();

				static double const minimum_distance_ = 1.0e-6;
                if (d < minimum_distance_)
                {
                    std::cerr <<
                        "Warning: Polyharmonic_spline_transformation::assemble_L_matrix(): "
                        "Input points " << i << " and " << j <<
                        " are too close (dist(i,j) < " << minimum_distance_
                        << ')' << std::endl;
                }

                L_(i, j) = L_(j, i) = kernel_(d);
            }
        }

        for (std::size_t i = 0; i < N; ++i)
        {
            L_.block<1, Domain_dim>(i, N + 1) = domain_points_[i].transpose();
            L_.block<Domain_dim, 1>(N + 1, i) = domain_points_[i];
        }

        L_.col(N).setOnes();
        L_.row(N).setOnes();
        L_.bottomRightCorner < Domain_dim + 1, Domain_dim + 1 > ().setZero();
    }

    void update_Wa_matrix()
    {
        std::size_t N = domain_points_.size();
        std::size_t M = N + 1 + Domain_dim;

        Wa_.resize(M, Range_dim);
        Wa_ = L_.fullPivLu().solve(Vt_);
        double relative_error = (L_ * Wa_ - Vt_).norm() / Vt_.norm();

        if (std::isnan(relative_error))
        {
            throw
                std::runtime_error(
                    "Polyharmonic_spline_transformation::update_Wa_matrix(): "
                    "Cannot define transformation (probably degenerate input "
                    "points)"
                );
        }

        if (relative_error > max_relative_error_)
        {
            throw
                std::runtime_error(
                    "Polyharmonic_spline_transformation::update_Wa_matrix(): "
                    "Relative error too large"
                );
        }

        Matrix bending_norm = Matrix::Zero(1, 1);

        for (std::size_t i = 0; i < Range_dim; ++i)
        {
            bending_norm += Wa_.block(0, i, N, 1).transpose()
                * L_.block(0, 0, N, N)
                * Wa_.block(0, i, N, 1);
        }

        bending_norm_ = bending_norm(0, 0);
    }

    Domain_points_container domain_points_;
    Kernel_function         kernel_;
    Matrix                  L_, Vt_, Wa_;
    double                  bending_norm_, max_relative_error_;
};

template < int Domain_dim, int Range_dim >
class Thin_plate_spline_transformation : public Polyharmonic_spline_transformation<
                                            Domain_dim,
                                            Range_dim,
                                            Polyharmonic_spline_kernel< 2 > >
{
    typedef Polyharmonic_spline_transformation<
        Domain_dim,
        Range_dim,
        Polyharmonic_spline_kernel< 2 > > Base;

public:
    template< typename Forward_iterator_1, typename Forward_iterator_2 >
    Thin_plate_spline_transformation(
            Forward_iterator_1 domain_points_begin,
            Forward_iterator_1 domain_points_end,
            Forward_iterator_2 range_points_begin,
            Forward_iterator_2 range_points_end,
            double max_relative_error = 1.0e-8
    )
        : Base(domain_points_begin, domain_points_end, range_points_begin,
                range_points_end, max_relative_error)
    {}
};

} // namespace tps

#endif // TPS_TPS_HPP


#ifndef MANIFOLDS_FUNCTIONS_TRIG_SIMPLIFICATIONS_HH
#define MANIFOLDS_FUNCTIONS_TRIG_SIMPLIFICATIONS_HH

#include "multiplication.hh"
#include "integral_polynomial.hh"
#include "addition.hh"
#include "division.hh"
#include <boost/mpl/map.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/contains.hpp>

namespace manifolds {

template <>
struct Simplification<Composition<IntegralPolynomial<0, 0, 1>, Cos>,
                      /*mult_cos_cos*/ 0> {
  static auto Combine(Composition<IntegralPolynomial<0, 0, 1>, Cos>) {
    SIMPLIFY_INFO("Simplifying cos**2\n");
    return ComposeRaw(IP<1, 0, -1>(), sin_);
  }
};

typedef boost::mpl::map<
    boost::mpl::pair<Cos, ACos>, boost::mpl::pair<Sin, ASin>,
    boost::mpl::pair<Tan, ATan>, boost::mpl::pair<Cosh, ACosh>,
    boost::mpl::pair<Sinh, ASinh>, boost::mpl::pair<Tanh, ATanh> > RawInverses;

struct AddReverse {
  template <class Map, class Elem> struct apply {
    typedef typename boost::mpl::insert<
        Map, boost::mpl::pair<typename Elem::second,
                              typename Elem::first> >::type type;
  };
};

typedef boost::mpl::fold<RawInverses, RawInverses, AddReverse>::type Inverses;

static_assert(boost::mpl::size<Inverses>::type::value == 12, "");
static_assert(
    boost::is_same<boost::mpl::at<Inverses, ATanh>::type, Tanh>::value, "");
static_assert(
    boost::mpl::contains<Inverses, boost::mpl::pair<Sin, ASin> >::value, "");

template <class F1, class F2>
struct Simplification<
    Composition<F1, F2>, /*com_f_f_inv*/ 0,
    typename std::enable_if<boost::mpl::contains<
        Inverses, boost::mpl::pair<F1, F2> >::type::value>::type> {
  static auto Combine(Composition<F1, F2>) { return IP<0, 1>(); }
};

template <> struct Simplification<Composition<Sin, ACos>, 0> {
  static auto Combine(Composition<Sin, ACos>) {
    SIMPLIFY_INFO("Simplifying Sin(ACos)\n");
    return sqrt_(IP<1, 0, -1>());
  }
};

template <> struct Simplification<Composition<Sin, ATan>, 0> {
  static auto Combine(Composition<Sin, ATan>) {
    SIMPLIFY_INFO("Simplifying Sin(ATan)\n");
    auto hypo = sqrt_(IP<1, 0, 1>());
    return DivideRaw(IP<0, 1>(), hypo);
  }
};

template <> struct Simplification<Composition<Cos, ASin>, 0> {
  static auto Combine(Composition<Cos, ASin>) {
    SIMPLIFY_INFO("Simplifying Cos(ASin)\n");
    return sqrt_(IP<1, 0, -1>());
  }
};

template <> struct Simplification<Composition<Cos, ATan>, 0> {
  static auto Combine(Composition<Cos, ATan>) {
    SIMPLIFY_INFO("Simplifying Cos(ATan)\n");
    auto hypo = sqrt_(IP<1, 0, 1>());
    return DivideRaw(IP<1>(), hypo);
  }
};

template <> struct Simplification<Composition<Tan, ASin>, 0> {
  static auto Combine(Composition<Tan, ASin>) {
    SIMPLIFY_INFO("Simplifying Tan(ASin)\n");
    return DivideRaw(IP<0, 1>(), IP<1, 0, -1>());
  }
};

template <> struct Simplification<Composition<Tan, ACos>, 0> {
  static auto Combine(Composition<Tan, ACos>) {
    SIMPLIFY_INFO("Simplifying Tan(ACos)\n");
    return DivideRaw(IP<1>(), IP<1, 0, -1>());
  }
};

template <IPInt_t i, IPInt_t... cs> struct is_all_but_last_0 {
  static const bool value = i == 0 && is_all_but_last_0<cs...>::value;
  typedef bool_<value> type;
};

template <IPInt_t i> struct is_all_but_last_0<i> {
  static const bool value = true;
  typedef bool_<value> type;
};

template <IPInt_t i> using ic = std::integral_constant<IPInt_t, i>;

template <IPInt_t... coeffs>
struct Simplification<
    Composition<Sin, IntegralPolynomial<coeffs...> >, 0,
    typename std::enable_if<is_all_but_last_0<coeffs...>::value &&(
        last<ic<coeffs>...>::type::value < 0)>::type> {

  static auto Combine(Composition<Sin, IntegralPolynomial<coeffs...> >) {
    SIMPLIFY_INFO("Simplifying Sin(-n*x^m)\n");
    return Negative(sin_(Negative(IntegralPolynomial<coeffs...>())));
  }
};

template <IPInt_t... coeffs>
struct Simplification<
    Composition<Sin, IntegralPolynomial<coeffs...> >, 0,
    typename std::enable_if<
        is_all_but_last_0<coeffs...>::value &&(sizeof...(coeffs) > 1) &&
        (last<ic<coeffs>...>::type::value > 1)>::type> {
  static const IPInt_t c = last<ic<coeffs>...>::type::value;
  static const IPInt_t left = c / 2;
  static const IPInt_t right = c - left;

  template <std::size_t... indices>
  static auto Combine(std::integer_sequence<std::size_t, indices...>) {
    SIMPLIFY_INFO("Simplifying Sin(n*x^m)\n");
    return std::make_pair(IP<(indices * 0)..., left>(),
                          IP<(indices * 0)..., right>());
  }

  static auto Combine(Composition<Sin, IntegralPolynomial<coeffs...> >) {
    SIMPLIFY_INFO("Simplifying Sin(n*x^m)\n");
    auto subs = Combine(std::make_index_sequence<sizeof...(coeffs) - 1>());
    auto left = subs.first;
    auto right = subs.second;
    return AddRaw(MultiplyRaw(sin_(left), cos_(right)),
                  MultiplyRaw(cos_(left), sin_(right)));
  }
};

template <IPInt_t... coeffs>
struct Simplification<
    Composition<Cos, IntegralPolynomial<coeffs...> >, 0,
    typename std::enable_if<is_all_but_last_0<coeffs...>::value &&(
        last<ic<coeffs>...>::type::value < 0)>::type> {

  static auto Combine(Composition<Cos, IntegralPolynomial<coeffs...> >) {
    SIMPLIFY_INFO("Simplifying Cos(-n*x^m)\n");
    return cos_(Negative(IntegralPolynomial<coeffs...>()));
  }
};

template <IPInt_t... coeffs>
struct Simplification<
    Composition<Cos, IntegralPolynomial<coeffs...> >, 0,
    typename std::enable_if<
        is_all_but_last_0<coeffs...>::value &&(sizeof...(coeffs) > 1) &&
        (last<ic<coeffs>...>::type::value > 1)>::type> {
  static const IPInt_t c = last<ic<coeffs>...>::type::value;
  static const IPInt_t left = c / 2;
  static const IPInt_t right = c - left;

  template <std::size_t... indices>
  static auto Combine(std::integer_sequence<std::size_t, indices...>) {
    return std::make_pair(IP<(indices * 0)..., left>(),
                          IP<(indices * 0)..., right>());
  }

  static auto Combine(Composition<Cos, IntegralPolynomial<coeffs...> >) {
    SIMPLIFY_INFO("Simplifying Cos(n*x^m)\n");
    auto subs = Combine(std::make_index_sequence<sizeof...(coeffs) - 1>());
    auto left = subs.first;
    auto right = subs.second;
    return SubRaw(MultiplyRaw(cos_(left), cos_(right)),
                  MultiplyRaw(sin_(left), sin_(right)));
  }
};

template <IPInt_t... coeffs> struct SplitPoly {
  template <int i>
  using ith = typename nth<i, std::integral_constant<IPInt_t, coeffs>...>::type;

  template <int i, int DUMMY = 0> struct NonZero {
    static const int index = sizeof...(coeffs) - 1 - i;
    static const int value =
        ith<index>::value == 0 ? NonZero<i - 1, 0>::value : i;
  };

  template <int DUMMY> struct NonZero<-1, DUMMY> {
    static const int value = -1;
  };

  static const int first = NonZero<sizeof...(coeffs) - 1>::value;
  static_assert(first != -1, "");

  template <class> struct LeftPoly;

  template <std::size_t... indices>
  struct LeftPoly<std::integer_sequence<std::size_t, indices...> > {
    typedef IntegralPolynomial<ith<indices>::value...> type;
  };

  template <class> struct RightPoly;

  template <std::size_t... indices>
  struct RightPoly<std::integer_sequence<std::size_t, indices...> > {
    typedef std::tuple<std::integral_constant<IPInt_t, coeffs>...> tup;
    typedef std::integral_constant<IPInt_t, 0> zero;
    typedef decltype(replace_element<first>(tup(), zero())) tup_z;
    typedef IntegralPolynomial<
        std::tuple_element<indices, tup_z>::type::value...> type;
  };

  typedef boost::mpl::pair<
      typename LeftPoly<std::make_index_sequence<first + 1> >::type,
      typename RightPoly<std::make_index_sequence<sizeof...(coeffs)> >::type>
  type;
};

template <IPInt_t... coeffs>
struct Simplification<
    Composition<Sin, IntegralPolynomial<coeffs...> >, 0,
    typename std::enable_if<!is_all_but_last_0<coeffs...>::value>::type> {
  typedef typename SplitPoly<coeffs...>::type Splits;
  typedef typename Splits::first Left;
  typedef typename Splits::second Right;

  static auto Combine(Composition<Sin, IntegralPolynomial<coeffs...> >) {
    SIMPLIFY_INFO("Simplifying Sin(poly + poly)\n");
    return AddRaw(MultiplyRaw(Sin {}(Left{}), Cos {}(Right{})),
                  MultiplyRaw(Cos {}(Left{}), Sin {}(Right{})));
  }
};

template <IPInt_t... coeffs>
struct Simplification<
    Composition<Cos, IntegralPolynomial<coeffs...> >, 0,
    typename std::enable_if<!is_all_but_last_0<coeffs...>::value>::type> {
  typedef typename SplitPoly<coeffs...>::type Splits;
  typedef typename Splits::first Left;
  typedef typename Splits::second Right;

  static auto Combine(Composition<Cos, IntegralPolynomial<coeffs...> >) {
    SIMPLIFY_INFO("Simplifying Cos(poly + poly)\n");
    return SubRaw(MultiplyRaw(Cos {}(Left{}), Cos {}(Right{})),
                  MultiplyRaw(Sin {}(Left{}), Sin {}(Right{})));
  }
};

template <IPInt_t... coeffs>
struct Simplification<
    Composition<Tan, IntegralPolynomial<coeffs...> >, 0,
    typename std::enable_if<is_all_but_last_0<coeffs...>::value &&(
        last<ic<coeffs>...>::type::value < 0)>::type> {
  static auto Combine(Composition<Tan, IntegralPolynomial<coeffs...> >) {
    return Negative(tan_(Negative(IP<coeffs...>())));
  }
};

template <IPInt_t... coeffs>
struct Simplification<
    Composition<Tan, IntegralPolynomial<coeffs...> >, 1,
    typename std::enable_if<is_all_but_last_0<coeffs...>::value &&(
        last<ic<coeffs>...>::type::value > 1)>::type> {
  template <class T, std::size_t... indices>
  static auto Process(T, std::integer_sequence<std::size_t, indices...>) {
    return IP<std::tuple_element<indices, T>::type::value...>();
  }
  static auto Combine(Composition<Tan, IntegralPolynomial<coeffs...> >) {
    static const IPInt_t last_c = last<ic<coeffs>...>::type::value;
    static const int last_i = sizeof...(coeffs) - 1;
    static const IPInt_t left = last_c / 2;
    static const IPInt_t right = last_c - left;
    typedef std::tuple<std::integral_constant<IPInt_t, coeffs>...> tup;
    typedef std::integral_constant<IPInt_t, left> Left;
    typedef std::integral_constant<IPInt_t, right> Right;
    std::make_index_sequence<sizeof...(coeffs)> indices;
    auto left_p = Process(replace_element<last_i>(tup(), Left()), indices);
    auto right_p = Process(replace_element<last_i>(tup(), Right()), indices);
    return DivideRaw(AddRaw(tan_(left_p), tan_(right_p)),
                     IP<1, 1>()(MultiplyRaw(tan_(left_p), tan_(right_p))));
  }
};

template <IPInt_t... coeffs>
struct Simplification<
    Composition<Tan, IntegralPolynomial<coeffs...> >, 0,
    typename std::enable_if<!is_all_but_last_0<coeffs...>::value>::type> {
  static auto Combine(Composition<Tan, IntegralPolynomial<coeffs...> >) {
    typedef typename SplitPoly<coeffs...>::type Pair;
    typename Pair::first left;
    typename Pair::second right;
    return DivideRaw(AddRaw(tan_(left), tan_(right)),
                     IP<1, 1>()(MultiplyRaw(tan_(left), tan_(right))));
  }
};

template <IPInt_t... coeffs>
struct Simplification<Composition<IntegralPolynomial<coeffs...>, Sqrt>, 0,
                      typename std::enable_if<(sizeof...(coeffs) > 2)>::type> {
  template <int i>
  using ith = typename nth<i, std::integral_constant<IPInt_t, coeffs>...>::type;

  template <int, class, class> struct Process;

  template <int i, IPInt_t... evens, IPInt_t... odds>
  struct Process<i, std::integer_sequence<IPInt_t, evens...>,
                 std::integer_sequence<IPInt_t, odds...> > {
    typedef typename std::conditional<
        i % 2 == 0, std::integer_sequence<IPInt_t, ith<i>::value, evens...>,
        std::integer_sequence<IPInt_t, evens...> >::type nes;

    typedef typename std::conditional<
        i % 2 == 1, std::integer_sequence<IPInt_t, ith<i>::value, odds...>,
        std::integer_sequence<IPInt_t, odds...> >::type nos;

    typedef typename Process<i - 1, nes, nos>::type type;
  };

  template <IPInt_t... evens, IPInt_t... odds>
  struct Process<-1, std::integer_sequence<IPInt_t, evens...>,
                 std::integer_sequence<IPInt_t, odds...> > {
    typedef boost::mpl::pair<IntegralPolynomial<evens...>,
                             IntegralPolynomial<odds...> > type;
  };

  static auto Combine(Composition<IntegralPolynomial<coeffs...>, Sqrt>) {
    typedef typename Process<sizeof...(coeffs) - 1,
                             std::integer_sequence<IPInt_t>,
                             std::integer_sequence<IPInt_t> >::type Pair;
    typedef typename Pair::first Evens;
    typedef typename Pair::second Odds;
    return AddRaw(Evens(), MultiplyRaw(Odds(), sqrt_));
  }
};
}

#endif


#include <math.h>
#include <iostream>
#include <armadillo>
#include <vector>
#include <complex>
#include <fftw3.h>
#include "mkl.h"

#include "tensor.h"
using namespace std;

#include "time.h"
double gettime(){
    struct timeval tv;
    gettimeofday(&tv,NULL);
    return tv.tv_sec*1000+tv.tv_usec/1000.0; //time:s
};

using namespace std;
using namespace arma;

int main() {
    double t0,t1;
    int I=100;
    int R=0.2*I;
    Tensor<float> a(I,I,I);
    for (int i = 0; i < a.n1; ++i) {
        for (int j = 0; j < a.n2; ++j) {
            for(int k=0; k<a.n3; ++k) {
                a(i,j,k) = randu();
            }
        }
    }
    cout << sizeof(a(1,1,1)) << endl;
//    Tensor<double> g(1,1,1);
//    mat u1(2,1);
//    mat u2(2,1);
//    mat u3(2,1);
//    tuckercore<double> A{g,u1,u2,u3};
//
    cout << "time:" <<endl;
    t0=gettime();
        HOSVD(a,R,R,R);
//    cp_als(a,R);
    t1=gettime();

    cout << "time:" <<t1-t0 <<endl;
//    Tensor<double> b(2,3,5),d(5,5,5), z(2,3,4),t(5,5,5);
//    cout<<z(0,1,2)<<endl;
//
//    z=z.zeros(2,3,4);
//    cout<<z(0,1,2)+123<<endl;
//
//    int *c=getsize(b);
//    cout<<c[0]<<endl; //tensor大小
//    cout<<sizeof(a)<<endl;
//    cout<<norm(a)<<endl;
//
//    cout << "Hello, World!" << endl;
//    cout<<t(1,2,3)<<endl;
//    cout<<norm(a)<<endl;

//    mat m1 = ten2mat(a,1);
//    cout << m1 << endl;
//
//    mat m2 = ten2mat(a,2);
//    cout << m2 << endl;
//
//    mat m3 = ten2mat(a,3);
//    cout << m3 << endl;

//    cout<<slice(a,0,2)<<endl;
//slice
//    cout<<fiber(a,1,2,2)<<endl;
    return 0;

}


#include "DarkART/Radial_Integrator.hpp"

#include <cmath>
#include <functional>

#include <boost/math/quadrature/gauss_kronrod.hpp>

#include "libphysica/Integration.hpp"
#include "libphysica/Natural_Units.hpp"
#include "libphysica/Utilities.hpp"

#include "DarkART/Special_Functions.hpp"

namespace DarkART
{

using namespace libphysica::natural_units;

double Radial_Integrator::Radial_Integral_Adaptive(unsigned int integral_index, double k_final, double q, int l_final, int L)
{
	std::function<double(double)> integrand = [this, integral_index, L, q, k_final, l_final](double r) {
		switch(integral_index)
		{
			case 1:
				return r * r * initial_state.Radial_Wavefunction(r) * final_state->Radial_Wavefunction(r, k_final, l_final) * Spherical_Bessel_jL(L, q * r);
			case 2:
				return r * r * initial_state.Radial_Wavefunction_Derivative(r) * final_state->Radial_Wavefunction(r, k_final, l_final) * Spherical_Bessel_jL(L, q * r);
			case 3:
				return r * initial_state.Radial_Wavefunction(r) * final_state->Radial_Wavefunction(r, k_final, l_final) * Spherical_Bessel_jL(L, q * r);
			default:
				std::cerr << "Radial_Integrator::Radial_Integral_Adaptive(): Integral I_" << integral_index << " not defined." << std::endl;
				std::exit(EXIT_FAILURE);
		}
	};
	// Integrate stepwise
	double stepsize	 = 0.5 * Bohr_Radius;
	double integral	 = 0.0;
	double epsilon_1 = 1.0, epsilon_2 = 1.0;
	double tolerance = 1.0e-6;
	unsigned int i;
	for(i = 0; epsilon_1 > tolerance || epsilon_2 > tolerance; i++)
	{
		epsilon_2				= epsilon_1;
		double new_contribution = boost::math::quadrature::gauss_kronrod<double, 41>::integrate(integrand, i * stepsize, (i + 1) * stepsize, 5, 1e-9);

		integral += new_contribution;
		epsilon_1 = std::fabs(new_contribution / integral);
	}
	return integral;
}

void Radial_Integrator::Tabulate_Initial_Wavefunction()
{
	for(unsigned int ri = 0; ri < r_points; ri++)
	{
		initial_radial_wavefunction_list[ri]			= initial_state.Radial_Wavefunction(r_values_and_weights[ri][0]);
		initial_radial_wavefunction_derivative_list[ri] = initial_state.Radial_Wavefunction_Derivative(r_values_and_weights[ri][0]);
	}
}

void Radial_Integrator::Tabulate_Final_Wavefunction(int l_final, int ki)
{
	if(l_final >= l_final_max_max)
	{
		std::cerr << "Error in Radial_Integrator::Tabulate_Final_Wavefunction(): l_final_max_max = " << l_final_max_max << " exceeded." << std::endl;
		std::exit(EXIT_FAILURE);
	}
	for(int li = l_final_max[ki] + 1; li <= l_final; li++)
		for(unsigned int ri = 0; ri < r_points; ri++)
			final_radial_wavefunction_list[ki][li][ri] = final_state->Radial_Wavefunction(r_values_and_weights[ri][0], k_grid[ki], li);
	l_final_max[ki] = l_final;
}

void Radial_Integrator::Tabulate_Bessel_Function(int Lmax, int qi)
{
	if(Lmax >= l_final_max_max + 2)
	{
		std::cerr << "Error in Radial_Integrator::Tabulate_Bessel_Function(): L_max_max = " << l_final_max_max + 2 << " exceeded." << std::endl;
		std::exit(EXIT_FAILURE);
	}
	for(int L = L_max[qi] + 1; L <= Lmax; L++)
		for(unsigned int ri = 0; ri < r_points; ri++)
			bessel_function_list[qi][L][ri] = Spherical_Bessel_jL(L, q_grid[qi] * r_values_and_weights[ri][0]);
	L_max[qi] = Lmax;
}

double Radial_Integrator::Radial_Integral_Table(unsigned int integral_index, double k_final, double q, int l_final, int L)
{
	// Identify ki and qi
	int ki = libphysica::Locate_Closest_Location(k_grid, k_final);
	int qi = libphysica::Locate_Closest_Location(q_grid, q);

	// Check if the tables have been computed for l_final and L
	if(l_final > l_final_max[ki])
		Tabulate_Final_Wavefunction(l_final, ki);
	if(L > L_max[qi])
		Tabulate_Bessel_Function(L, qi);

	// Sum up the integral
	double integral = 0.0;
	for(unsigned int ri = 0; ri < r_values_and_weights.size(); ri++)
	{
		double r	  = r_values_and_weights[ri][0];
		double weight = r_values_and_weights[ri][1];

		if(integral_index == 1)
			integral += weight * r * r * initial_radial_wavefunction_list[ri] * final_radial_wavefunction_list[ki][l_final][ri] * bessel_function_list[qi][L][ri];
		else if(integral_index == 2)
			integral += weight * r * r * initial_radial_wavefunction_derivative_list[ri] * final_radial_wavefunction_list[ki][l_final][ri] * bessel_function_list[qi][L][ri];
		else if(integral_index == 3)
			integral += weight * r * initial_radial_wavefunction_list[ri] * final_radial_wavefunction_list[ki][l_final][ri] * bessel_function_list[qi][L][ri];
		else
		{
			std::cerr << "Radial_Integrator::Radial_Integral_Table(): Integral I_" << integral_index << " not defined." << std::endl;
			std::exit(EXIT_FAILURE);
		}
	}
	return integral;
}

Radial_Integrator::Radial_Integrator()
: using_function_tabulation(false), initial_state()
{
}

Radial_Integrator::Radial_Integrator(const Initial_Electron_State& ini_state, const Final_Electron_State& fin_state)
: using_function_tabulation(false), initial_state(ini_state)
{
	final_state = fin_state.Clone();
}

double Radial_Integrator::Radial_Integral(int integral_index, double k_final, double q, int l_final, int L)
{
	if(using_function_tabulation)
		return Radial_Integral_Table(integral_index, k_final, q, l_final, L);
	else
		return Radial_Integral_Adaptive(integral_index, k_final, q, l_final, L);
}

void Radial_Integrator::Use_Tabulated_Functions(unsigned int rpoints, const std::vector<double>& k_list, const std::vector<double>& q_list)
{
	using_function_tabulation = true;
	k_grid					  = k_list;
	q_grid					  = q_list;
	l_final_max				  = std::vector<int>(k_grid.size(), -1);
	L_max					  = std::vector<int>(q_grid.size(), -1);

	r_points			 = rpoints;
	r_max				 = 50.0 * Bohr_Radius;
	r_values_and_weights = libphysica::Compute_Gauss_Legendre_Roots_and_Weights(r_points, 0.0, r_max);

	initial_radial_wavefunction_list			= std::vector<double>(r_points, 0.0);
	initial_radial_wavefunction_derivative_list = std::vector<double>(r_points, 0.0);
	Tabulate_Initial_Wavefunction();

	// 2. Initiate final radial wavefunctions and Bessel function list
	final_radial_wavefunction_list = std::vector<std::vector<std::vector<double>>>(k_grid.size(), std::vector<std::vector<double>>(l_final_max_max, std::vector<double>(r_points, 0.0)));
	bessel_function_list		   = std::vector<std::vector<std::vector<double>>>(q_grid.size(), std::vector<std::vector<double>>(l_final_max_max + 2, std::vector<double>(r_points, 0.0)));
}

void Radial_Integrator::Set_New_States(const Initial_Electron_State& new_initial_state, const Final_Electron_State& new_final_state)
{
	// New states
	initial_state = new_initial_state;
	final_state	  = new_final_state.Clone();

	if(using_function_tabulation)
	{
		// 1. Tabulate initial radial wavefunction
		Tabulate_Initial_Wavefunction();

		// 2. Reset the final state wavefunction lists
		l_final_max					   = std::vector<int>(k_grid.size(), -1);
		final_radial_wavefunction_list = std::vector<std::vector<std::vector<double>>>(k_grid.size(), std::vector<std::vector<double>>(l_final_max_max, std::vector<double>(r_points, 0.0)));
	}
}

}	// namespace DarkART