//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef _BOOST_UBLAS_OPERATION_BLOCKED_
#define _BOOST_UBLAS_OPERATION_BLOCKED_

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/detail/vector_assign.hpp> // indexing_vector_assign
#include <boost/numeric/ublas/detail/matrix_assign.hpp> // indexing_matrix_assign

namespace boost { namespace numeric { namespace ublas {

    template<class V, typename V::size_type BS, class E1, class E2>
    BOOST_UBLAS_INLINE
    V
    block_prod (const matrix_expression<E1> &e1,
                const vector_expression<E2> &e2) {
        typedef V vector_type;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;
        typedef typename V::size_type size_type;
        typedef typename V::value_type value_type;
        const size_type block_size = BS;

        V v (e1 ().size1 ());
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v.size ());
        typedef typename type_traits<value_type>::real_type real_type;
        real_type verrorbound (norm_1 (v) + norm_1 (e1) * norm_1 (e2));
        indexing_vector_assign<scalar_assign> (cv, prod (e1, e2));
#endif
        size_type i_size = e1 ().size1 ();
        size_type j_size = BOOST_UBLAS_SAME (e1 ().size2 (), e2 ().size ());
        for (size_type i_begin = 0; i_begin < i_size; i_begin += block_size) {
            size_type i_end = i_begin + (std::min) (i_size - i_begin, block_size);
            // FIX: never ignore Martin Weiser's advice ;-(
#ifdef BOOST_UBLAS_NO_CACHE
            vector_range<vector_type> v_range (v, range (i_begin, i_end));
#else
            // vector<value_type, bounded_array<value_type, block_size> > v_range (i_end - i_begin);
            vector<value_type> v_range (i_end - i_begin);
#endif
            v_range.assign (zero_vector<value_type> (i_end - i_begin));
            for (size_type j_begin = 0; j_begin < j_size; j_begin += block_size) {
                size_type j_end = j_begin + (std::min) (j_size - j_begin, block_size);
#ifdef BOOST_UBLAS_NO_CACHE
                const matrix_range<expression1_type> e1_range (e1 (), range (i_begin, i_end), range (j_begin, j_end));
                const vector_range<expression2_type> e2_range (e2 (), range (j_begin, j_end));
                v_range.plus_assign (prod (e1_range, e2_range));
#else
                // const matrix<value_type, row_major, bounded_array<value_type, block_size * block_size> > e1_range (project (e1 (), range (i_begin, i_end), range (j_begin, j_end)));
                // const vector<value_type, bounded_array<value_type, block_size> > e2_range (project (e2 (), range (j_begin, j_end)));
                const matrix<value_type, row_major> e1_range (project (e1 (), range (i_begin, i_end), range (j_begin, j_end)));
                const vector<value_type> e2_range (project (e2 (), range (j_begin, j_end)));
                v_range.plus_assign (prod (e1_range, e2_range));
#endif
            }
#ifndef BOOST_UBLAS_NO_CACHE
            project (v, range (i_begin, i_end)).assign (v_range);
#endif
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (v - cv) <= 2 * std::numeric_limits<real_type>::epsilon () * verrorbound, internal_logic ());
#endif
        return v;
    }

    template<class V, typename V::size_type BS, class E1, class E2>
    BOOST_UBLAS_INLINE
    V
    block_prod (const vector_expression<E1> &e1,
                const matrix_expression<E2> &e2) {
        typedef V vector_type;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;
        typedef typename V::size_type size_type;
        typedef typename V::value_type value_type;
        const size_type block_size = BS;

        V v (e2 ().size2 ());
#if BOOST_UBLAS_TYPE_CHECK
        vector<value_type> cv (v.size ());
        typedef typename type_traits<value_type>::real_type real_type;
        real_type verrorbound (norm_1 (v) + norm_1 (e1) * norm_1 (e2));
        indexing_vector_assign<scalar_assign> (cv, prod (e1, e2));
#endif
        size_type i_size = BOOST_UBLAS_SAME (e1 ().size (), e2 ().size1 ());
        size_type j_size = e2 ().size2 ();
        for (size_type j_begin = 0; j_begin < j_size; j_begin += block_size) {
            size_type j_end = j_begin + (std::min) (j_size - j_begin, block_size);
            // FIX: never ignore Martin Weiser's advice ;-(
#ifdef BOOST_UBLAS_NO_CACHE
            vector_range<vector_type> v_range (v, range (j_begin, j_end));
#else
            // vector<value_type, bounded_array<value_type, block_size> > v_range (j_end - j_begin);
            vector<value_type> v_range (j_end - j_begin);
#endif
            v_range.assign (zero_vector<value_type> (j_end - j_begin));
            for (size_type i_begin = 0; i_begin < i_size; i_begin += block_size) {
                size_type i_end = i_begin + (std::min) (i_size - i_begin, block_size);
#ifdef BOOST_UBLAS_NO_CACHE
                const vector_range<expression1_type> e1_range (e1 (), range (i_begin, i_end));
                const matrix_range<expression2_type> e2_range (e2 (), range (i_begin, i_end), range (j_begin, j_end));
#else
                // const vector<value_type, bounded_array<value_type, block_size> > e1_range (project (e1 (), range (i_begin, i_end)));
                // const matrix<value_type, column_major, bounded_array<value_type, block_size * block_size> > e2_range (project (e2 (), range (i_begin, i_end), range (j_begin, j_end)));
                const vector<value_type> e1_range (project (e1 (), range (i_begin, i_end)));
                const matrix<value_type, column_major> e2_range (project (e2 (), range (i_begin, i_end), range (j_begin, j_end)));
#endif
                v_range.plus_assign (prod (e1_range, e2_range));
            }
#ifndef BOOST_UBLAS_NO_CACHE
            project (v, range (j_begin, j_end)).assign (v_range);
#endif
        }
#if BOOST_UBLAS_TYPE_CHECK
        BOOST_UBLAS_CHECK (norm_1 (v - cv) <= 2 * std::numeric_limits<real_type>::epsilon () * verrorbound, internal_logic ());
#endif
        return v;
    }

    template<class M, typename M::size_type BS, class E1, class E2>
    BOOST_UBLAS_INLINE
    M
    block_prod (const matrix_expression<E1> &e1,
                const matrix_expression<E2> &e2,
                row_major_tag) {
        typedef M matrix_type;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;
        const size_type block_size = BS;

        M m (e1 ().size1 (), e2 ().size2 ());
#if BOOST_UBLAS_TYPE_CHECK
        matrix<value_type, row_major> cm (m.size1 (), m.size2 ());
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_assign> (cm, prod (e1, e2), row_major_tag ());
        disable_type_check<bool>::value = true;
#endif
        size_type i_size = e1 ().size1 ();
        size_type j_size = e2 ().size2 ();
        size_type k_size = BOOST_UBLAS_SAME (e1 ().size2 (), e2 ().size1 ());
        for (size_type i_begin = 0; i_begin < i_size; i_begin += block_size) {
            size_type i_end = i_begin + (std::min) (i_size - i_begin, block_size);
            for (size_type j_begin = 0; j_begin < j_size; j_begin += block_size) {
                size_type j_end = j_begin + (std::min) (j_size - j_begin, block_size);
                // FIX: never ignore Martin Weiser's advice ;-(
#ifdef BOOST_UBLAS_NO_CACHE
                matrix_range<matrix_type> m_range (m, range (i_begin, i_end), range (j_begin, j_end));
#else
                // matrix<value_type, row_major, bounded_array<value_type, block_size * block_size> > m_range (i_end - i_begin, j_end - j_begin);
                matrix<value_type, row_major> m_range (i_end - i_begin, j_end - j_begin);
#endif
                m_range.assign (zero_matrix<value_type> (i_end - i_begin, j_end - j_begin));
                for (size_type k_begin = 0; k_begin < k_size; k_begin += block_size) {
                    size_type k_end = k_begin + (std::min) (k_size - k_begin, block_size);
#ifdef BOOST_UBLAS_NO_CACHE
                    const matrix_range<expression1_type> e1_range (e1 (), range (i_begin, i_end), range (k_begin, k_end));
                    const matrix_range<expression2_type> e2_range (e2 (), range (k_begin, k_end), range (j_begin, j_end));
#else
                    // const matrix<value_type, row_major, bounded_array<value_type, block_size * block_size> > e1_range (project (e1 (), range (i_begin, i_end), range (k_begin, k_end)));
                    // const matrix<value_type, column_major, bounded_array<value_type, block_size * block_size> > e2_range (project (e2 (), range (k_begin, k_end), range (j_begin, j_end)));
                    const matrix<value_type, row_major> e1_range (project (e1 (), range (i_begin, i_end), range (k_begin, k_end)));
                    const matrix<value_type, column_major> e2_range (project (e2 (), range (k_begin, k_end), range (j_begin, j_end)));
#endif
                    m_range.plus_assign (prod (e1_range, e2_range));
                }
#ifndef BOOST_UBLAS_NO_CACHE
                project (m, range (i_begin, i_end), range (j_begin, j_end)).assign (m_range);
#endif
            }
        }
#if BOOST_UBLAS_TYPE_CHECK
        disable_type_check<bool>::value = false;
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    template<class M, typename M::size_type BS, class E1, class E2>
    BOOST_UBLAS_INLINE
    M
    block_prod (const matrix_expression<E1> &e1,
                const matrix_expression<E2> &e2,
                column_major_tag) {
        typedef M matrix_type;
        typedef const E1 expression1_type;
        typedef const E2 expression2_type;
        typedef typename M::size_type size_type;
        typedef typename M::value_type value_type;
        const size_type block_size = BS;

        M m (e1 ().size1 (), e2 ().size2 ());
#if BOOST_UBLAS_TYPE_CHECK
        matrix<value_type, column_major> cm (m.size1 (), m.size2 ());
        typedef typename type_traits<value_type>::real_type real_type;
        real_type merrorbound (norm_1 (m) + norm_1 (e1) * norm_1 (e2));
        indexing_matrix_assign<scalar_assign> (cm, prod (e1, e2), column_major_tag ());
        disable_type_check<bool>::value = true;
#endif
        size_type i_size = e1 ().size1 ();
        size_type j_size = e2 ().size2 ();
        size_type k_size = BOOST_UBLAS_SAME (e1 ().size2 (), e2 ().size1 ());
        for (size_type j_begin = 0; j_begin < j_size; j_begin += block_size) {
            size_type j_end = j_begin + (std::min) (j_size - j_begin, block_size);
            for (size_type i_begin = 0; i_begin < i_size; i_begin += block_size) {
                size_type i_end = i_begin + (std::min) (i_size - i_begin, block_size);
                // FIX: never ignore Martin Weiser's advice ;-(
#ifdef BOOST_UBLAS_NO_CACHE
                matrix_range<matrix_type> m_range (m, range (i_begin, i_end), range (j_begin, j_end));
#else
                // matrix<value_type, column_major, bounded_array<value_type, block_size * block_size> > m_range (i_end - i_begin, j_end - j_begin);
                matrix<value_type, column_major> m_range (i_end - i_begin, j_end - j_begin);
#endif
                m_range.assign (zero_matrix<value_type> (i_end - i_begin, j_end - j_begin));
                for (size_type k_begin = 0; k_begin < k_size; k_begin += block_size) {
                    size_type k_end = k_begin + (std::min) (k_size - k_begin, block_size);
#ifdef BOOST_UBLAS_NO_CACHE
                    const matrix_range<expression1_type> e1_range (e1 (), range (i_begin, i_end), range (k_begin, k_end));
                    const matrix_range<expression2_type> e2_range (e2 (), range (k_begin, k_end), range (j_begin, j_end));
#else
                    // const matrix<value_type, row_major, bounded_array<value_type, block_size * block_size> > e1_range (project (e1 (), range (i_begin, i_end), range (k_begin, k_end)));
                    // const matrix<value_type, column_major, bounded_array<value_type, block_size * block_size> > e2_range (project (e2 (), range (k_begin, k_end), range (j_begin, j_end)));
                    const matrix<value_type, row_major> e1_range (project (e1 (), range (i_begin, i_end), range (k_begin, k_end)));
                    const matrix<value_type, column_major> e2_range (project (e2 (), range (k_begin, k_end), range (j_begin, j_end)));
#endif
                    m_range.plus_assign (prod (e1_range, e2_range));
                }
#ifndef BOOST_UBLAS_NO_CACHE
                project (m, range (i_begin, i_end), range (j_begin, j_end)).assign (m_range);
#endif
            }
        }
#if BOOST_UBLAS_TYPE_CHECK
        disable_type_check<bool>::value = false;
        BOOST_UBLAS_CHECK (norm_1 (m - cm) <= 2 * std::numeric_limits<real_type>::epsilon () * merrorbound, internal_logic ());
#endif
        return m;
    }

    // Dispatcher
    template<class M, typename M::size_type BS, class E1, class E2>
    BOOST_UBLAS_INLINE
    M
    block_prod (const matrix_expression<E1> &e1,
                const matrix_expression<E2> &e2) {
        typedef typename M::orientation_category orientation_category;
        return block_prod<M, BS> (e1, e2, orientation_category ());
    }

}}}

#endif


//By Fatemeh Saki, University of Texas at Dallas, Spring 2017
// Modified by Nasim Alamdari, 2017
#include "stdafx.h"
#include <iostream>
#include <fstream>
#include <vector>
#include <dlib/svm.h>

// Macros for this module
#define R2_MULTIPLIER_CONST		1.1
#define THRESHOLD_CONST			0.3

// SVM Macros
#define SVM_TYPE				SVDD
#define KERNEL_TYPE				RBF
#define TOL						1e-8
#define PI						3.14159265358979323846
#define CACHE_SIZE				100

// -----------------------------------------------------------------------------------------------
// Definition of the Classification function
void Classification(const SVCluster* ClusterSet, double** frame, int* ClassOut, const int sizeC, const int m, const int n)
{
	/*
	initialize the output vector
	ClassOut[0] is equivalent of CloseCL	in MATLAB
	ClassOut[1] is equivalent of ClassType	in MATLAB
	ClassOut[2] is equivalent of NovOp		in MATLAB
	*/
	for (int i = 0; i < 2; i++)
		ClassOut[i] = 0;

	// allocating memory for the Dist results
	double **Dist = (double **)malloc(sizeC * sizeof(double *));
	for (int i = 0; i < sizeC; i++)
		Dist[i] = (double *)malloc(m* sizeof(double)); // modified by Nasim

	// allocating memory for the closeness index results
	double **CloseIdx = (double **)malloc(sizeC * sizeof(double *));
	for (int i = 0; i < sizeC; i++)
		CloseIdx[i] = (double *)malloc(m* sizeof(double)); // modified by Nasim

	// loop over the cluster set and perform the calculation
	for (int i = 0; i < sizeC; i++){

		// extract the structure information for the ith element
		int c = (ClusterSet + i)->c;
		int r = (ClusterSet + i)->r;
		double Kxx = (ClusterSet + i)->Kxx;
		double R2 = (ClusterSet + i)->R2;
		double sigma = (ClusterSet + i)->sigma;
		double offsets = (ClusterSet + i)->offsets;
		double* alpha = (ClusterSet + i)->alpha;
		double** SVvect = (ClusterSet + i)->SVvectors;

		// declare the kernelDist vector
		double *kernelDist = (double *)malloc(m * sizeof(double)); //modified by Nasim

		// allocating memory for the result pauerwise euclidian distance
		double **DistMtx = (double **)malloc(r * sizeof(double *));
		for (int j = 0; j < r; j++)
			DistMtx[j] = (double *)malloc(m* sizeof(double));

		// calculate the distance
		MyDistm(SVvect, frame, DistMtx, r, m, c);// corrected by Nasim

		// claculate alphaK (to save memory we reuse DistMtx)
		myExp(DistMtx, alpha, r, m, sigma); // corrected by Nasim

		//ofstream out_data;
		//out_data.open("OFC Results.txt", ios::trunc); //modified by Nasim
		// loop over j to calculate the kernelDist
		for (int j = 0; j < m; j++){ // modified by Nasim
			double sum = 0;
			for (int k = 0; k < r; k++){
				sum += DistMtx[k][j]; //modified by Nasim (previously was DistMtx[j][k];)
			}

			//kernelDist[j] = (1 - 2 * sum + Kxx) / (R2_MULTIPLIER_CONST * R2);
			kernelDist[j] = (1 - 2 * sum + Kxx); // Modified by Nasim
			Dist[i][j] = kernelDist[j] / (R2_MULTIPLIER_CONST * R2);

			// Perform a comparision for the calculation of CloseIdx
			if (kernelDist[j] <= (R2_MULTIPLIER_CONST * R2))
				CloseIdx[i][j] = 1; //modified by Nasim
			else
				CloseIdx[i][j] = 0;

			//out_data << CloseIdx[i][j] << '\n';

		}// end of the for loop
		//out_data.close();
		// free the memory for the current SVCluster
		for (int j = 0; j < r; j++) free(DistMtx[j]);
		free(DistMtx);

		free(kernelDist);

		DistMtx = NULL;
		kernelDist = NULL;
	}// end of the for loop

	// declare the sumCloseIdx vector
	double *sumCloseIdx = (double *)malloc(sizeC * sizeof(double));

	// calculate the sum of closeIdx matrix across the first dimension
	for (int i = 0; i < sizeC; i++){
		double sum = 0;
		for (int j = 0; j < m; j++) //modified by Nasim
			sum += CloseIdx[i][j];

		sumCloseIdx[i] = sum;
	}

	// define a threshold
	double threshold = floor(THRESHOLD_CONST * m);

	// perform the required comparisions
	if (sizeC == 1){
		if (compareArrays(sumCloseIdx, threshold, sizeC)){
			ClassOut[0] = 1;
			ClassOut[1] = 1;
			ClassOut[2] = 0; // modified by Nasim
		}
		else
			ClassOut[0] = 0; // modified by Nasim
		    ClassOut[1] = 0; // modified by Nasim
			ClassOut[2] = 1;

	}
	else if (sizeC > 1){
		// declare the Clust vector
		int *Clust = (int *)malloc(m * sizeof(int)); // modified by Nasim
		// find the min of matrix dist across the first dimension
		minMatrixR(Dist, Clust, sizeC, m);

		//ofstream out_data;
		//out_data.open("OFC Results.txt", ios::trunc); //modified by Nasim
		int *Clust2 = (int *)malloc(m * sizeof(int)); //added by Nasim
		for (int i = 0; i < m; i++){ //added by Nasim
			Clust2[i] = Clust[i] + 1;
			//out_data << Clust2[i] << ',';
		}
		//out_data.close();
		// take the statistical mode of array Clust
		int CloseCL2 = mode(Clust2, m); // modified by Nasim
		// now compute the sum
		double sum = 0;
		for (int i = 0; i < m; i++) //modified by Nasim
			sum += CloseIdx[CloseCL2-1][i]; // modified by Nasim

		if (sum>=threshold){
			ClassOut[0] = CloseCL2;
			ClassOut[1] = 1;
		}
		else if (sum==0)
			ClassOut[2] = 1;

	}

	//free the allocated memory
	for (int i = 0; i < sizeC; i++) free(Dist[i]);
	free(Dist);

	for (int i = 0; i < sizeC; i++) free(CloseIdx[i]);
	free(CloseIdx);

	free(sumCloseIdx);

	Dist		= NULL;
	CloseIdx	= NULL;
	sumCloseIdx = NULL;

	return;
}

// -----------------------------------------------------------------------------------------------
// Definition of the connection Check function
int connectionCheck(double** frame1, double** frame2, const int Rowsize1, const int Rowsize2, const int ColSize, const double eps1, const double eps2){

	// allocating memory for the euclidian distance matrix
	double **DistMtx = (double **)malloc(Rowsize1 * sizeof(double *));
	for (int i = 0; i < Rowsize1; i++)
		DistMtx[i] = (double *)malloc(Rowsize2 * sizeof(double)); //modified byNasim

	// run the Eclidean distance function
	MyDistm(frame1, frame2, DistMtx, Rowsize1, Rowsize2, ColSize);

	// calculate the conditional sum of the matrix entries
	double clustDist = sumMatrixCond(DistMtx, Rowsize1, Rowsize2, eps1 + eps2); // modified by Nasim

	// free the allocated memory
	for (int i = 0; i < Rowsize1; i++) free(DistMtx[i]); //modified by Nasim
	free(DistMtx);

	// avoid danggling pointer
	DistMtx = NULL;

	if (clustDist > (THRESHOLD_CONST * Rowsize1 * THRESHOLD_CONST * Rowsize2))
		return +1;

	return -1;
}

// -----------------------------------------------------------------------------------------------
// Definition of the Frame analysis function
//Confirmed by Nasim
double frameAnalysis(double** frame, const int RowSize, const int ColSize)
{
	// allocating memory for the euclidian distance matrix
	double **DistMtx = (double **)malloc(RowSize * sizeof(double *));
	for (int i = 0; i < RowSize; i++)
		DistMtx[i] = (double *)malloc(RowSize * sizeof(double));

	// run the Eclidean distance function
	MyDistm(frame, frame, DistMtx, RowSize, RowSize, ColSize);

	// find the mean of the matrix entries
	double mDist = meanMatrix(DistMtx, RowSize, ColSize)/2;

	// find the maximum element
	double radius = maxMatrix(DistMtx, RowSize, ColSize);

	// for debug purpose only
	//printFrame(DistMtx, Rowsize, ColSize);

	// free the allocated memory
	for (int i = 0; i < RowSize; i++) free(DistMtx[i]);
	free(DistMtx);

	// avoid danggling pointer
	DistMtx = NULL;

	return radius;
}

// -----------------------------------------------------------------------------------------------
// Definition of the Cluster Creation function //modified by Nasim
SVCluster* ClusterCreation(double** svData, const int svRow, const int svCol, const double fracRejection, const double sigma){

	//******************************* Nasim added: dlib one-class SVM****************
	// We will use column vectors to store our points.  Here we make a convenient typedef
	// for the kind of vector we will use.
	typedef dlib::matrix<double,0, 1> sample_type; //Nasim: This typedef declares a matrix with unknow(0) rows and 1 column. This is column vector.

	// Then we select the kernel we want to use.  For our present problem the radial basis
  // kernel is quite effective.
	typedef dlib::radial_basis_kernel<sample_type> kernel_type; //Nasim: use same

	std::vector<sample_type> samples;
	sample_type samp(svCol); //Nasim: my data should be m(8), since 8 features every vector

	for (int i = 0; i< svRow; i++)
	{
			samp(0) = svData[i][0];
			samp(1) = svData[i][1];
			samp(2) = svData[i][2];
			samp(3) = svData[i][3];
			samp(4) = svData[i][4];
			samp(5) = svData[i][5];
			samp(6) = svData[i][6];
			samp(7) = svData[i][7];
			samples.push_back(samp);
	}

	//vector_normalizer<sample_type> normalizer;
	// let the normalizer learn the mean and standard deviation of the samples
	//normalizer.train(samples);
	// now normalize each sample
	//for (unsigned long i = 0; i < samples.size(); ++i)
	//	samples[i] = normalizer(samples[i]);

	// Now make the object responsible for training one-class SVMs.
	dlib::svm_one_class_trainer<kernel_type> trainer;

	// Here we set the width of the radial basis kernel
	trainer.set_kernel(kernel_type(sigma)); // Nasim 4.0 is width of rbf kernel (sigma)
	double NU = svRow*fracRejection;
	trainer.set_nu((NU < 1) ? 1 : 1 / NU);
	trainer.set_epsilon(TOL);
	//trainer.set_nu(1/svRow*fracRejection);
	//trainer.set_lambda(0.00001);
	//const double C = get_option(parser, "c", 1.0);
	//trainer.set_c(C);

	// Now train a one-class SVM.  The result is a function, df(),
	dlib::decision_function<kernel_type> df = trainer.train(samples);

	//cout << "\n number of support vectors in our learned_function is "
	//	<< df.basis_vectors.size() << endl;

	//df.alpha;
	//df.b;
	//df.basis_vectors;
	//df.kernel_function;
	//df.kernel_function.gamma;

	//***********************************************************************************
	// initialize mySVClust by allocating memory dynamically
	SVCluster* mySVClust = (SVCluster *)malloc(sizeof(SVCluster));

	// allocating memory for the SVvectors matrix //modified by Nasim
	double **SVvectors = (double **)malloc(df.basis_vectors.size() * sizeof(double *));
	for (int j = 0; j < df.basis_vectors.size(); j++)
	SVvectors[j] = (double *)malloc(svCol * sizeof(double));

	// populate the SVvectors matrix from the model
	for (int i = 0; i < df.basis_vectors.size(); i++) {
		for (int j = 0; j < svCol; j++) {
			SVvectors[i][j] = df.basis_vectors(i)(j);

		}

	}

	double sumAlpha = 0;
	for (int i = 0; i < df.basis_vectors.size(); i++) {
		sumAlpha += df.alpha(i);
	}

	double *alpha = (double *)malloc(df.basis_vectors.size() * sizeof(double));
	for (int i = 0; i < df.basis_vectors.size(); i++) {
		alpha[i] = df.alpha(i) / sumAlpha; //Nasim: not sure about devision by sumAlpha
		//alpha[i] = df.alpha(i); //Nasim

	}

	// allocating memory for the result pauerwise euclidian distance
	double **DistMtx = (double **)malloc(df.basis_vectors.size() * sizeof(double *));
	for (int j = 0; j < df.basis_vectors.size(); j++)
	DistMtx[j] = (double *)malloc(df.basis_vectors.size() * sizeof(double));

	// calculate the distance
	MyDistm(SVvectors, SVvectors, DistMtx, df.basis_vectors.size(), df.basis_vectors.size(), svCol);

	// claculate Kxx (to save memory we reuse DistMtx)
	double Kxx = myExp_alpha(DistMtx, alpha, df.basis_vectors.size(), sigma);

	//------added by Nasim----------------------------------------
	double *Dx = (double *)malloc(df.basis_vectors.size() * sizeof(double));
	for (int i = 0; i < df.basis_vectors.size(); i++) {
		double K = 0;
		for (int j = 0; j < df.basis_vectors.size(); j++) {
			K += alpha[i]*exp(-DistMtx[j][i] / (sigma * sigma));
		}
		Dx[i] = -2 * K;
	}

	double offs = 1 + Kxx;
	double sum = 0;
	for (int i = 0; i < df.basis_vectors.size(); i++) {
		sum += Dx[i];
	}
	double R2 = sum / df.basis_vectors.size(); //
	double threshold = R2 + offs;
	//------------------------------------------------------------------
	// free the memory for DistMtx
	for (int j = 0; j < df.basis_vectors.size(); j++) free(DistMtx[j]);
	free(DistMtx);

	DistMtx = NULL;

	// pack the data in the structure
	mySVClust->SVvectors    = SVvectors;
	mySVClust->alpha        = alpha;
	mySVClust->r			= df.basis_vectors.size();
	mySVClust->c            = svCol;
	mySVClust->Kxx			= Kxx;
	mySVClust->R2           = threshold;//model->r_square;	// had to make changes to the svm library to get access to this
	mySVClust->sigma        = df.kernel_function.gamma;			// redundant
	mySVClust->offsets      = offs;				// // Modified by Nasim

										// free the svm-related dynamic memory
										//free(prob.x);
										//free(prob.y);
										//free(x_space);
										//svm_free_and_destroy_model(&model);
										//svm_destroy_param(&param);

	return mySVClust;
}


#include "SeparationAlgorithms.hpp"

#include "LinearConstraint.hpp"
#include "LinearVariableComposition.hpp"
#include "Model.hpp"
#include "SupportGraphs.hpp"
#include "Variable.hpp"
#include "WeightManager.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/graph/stoer_wagner_min_cut.hpp>
#include <boost/range/iterator_range.hpp>

#include <xtensor/xtensor.hpp>
#include <xtensor/xvectorize.hpp>
#include <xtensor/xview.hpp>

namespace tsplp::graph
{
    using EdgeWeightProperty = boost::property<boost::edge_weight_t, double>;
    using UndirectedGraph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, boost::no_property, EdgeWeightProperty>;

    Separator::Separator(const xt::xtensor<Variable, 3>& variables, const WeightManager& weightManager, const Model& model)
        : m_variables(variables), m_weightManager(weightManager), m_model(model), m_spSupportGraph(std::make_unique<PiSigmaSupportGraph>(variables, weightManager.Dependencies(), model))
    {
    }

    Separator::~Separator() noexcept = default;

    std::optional<LinearConstraint> Separator::Ucut() const
    {
        const auto N = m_weightManager.N();
        const auto vf = xt::vectorize([this](Variable v) { return v.GetObjectiveValue(m_model); });
        const auto values = vf(m_variables);

        UndirectedGraph graph(N);
        for (size_t u = 0; u < N; ++u)
        {
            for (size_t v = u + 1; v < N; ++v)
            {
                const auto weight = xt::sum(xt::view(values, xt::all(), u, v))() + xt::sum(xt::view(values, xt::all(), v, u))();
                boost::add_edge(u, v, weight, graph);
            }
        }

        const auto parities = boost::make_one_bit_color_map(N, get(boost::vertex_index, graph));

        const auto cutSize = boost::stoer_wagner_min_cut(graph, get(boost::edge_weight, graph), boost::parity_map(parities));

        if (cutSize >= 2.0 - 1.e-10)
            return std::nullopt;

        LinearVariableComposition sum;
        for (size_t u = 0; u < N; ++u)
            for (size_t v = 0; v < N; ++v)
                if (get(parities, u) != get(parities, v))
                    sum += xt::sum(xt::view(m_variables, xt::all(), u, v) + 0)();

        return sum >= 2;
    }

    std::optional<LinearConstraint> Separator::Pi() const
    {
        if (m_weightManager.Dependencies().GetArcs().empty())
            return std::nullopt;

        const auto N = m_weightManager.N();
        const auto A = m_weightManager.A();

        for (size_t n = 0; n < N; ++n)
        {
            if (m_weightManager.Dependencies().GetIncomingSpan(n).empty())
                continue;

            for (size_t a = 0; a < A; ++a)
            {
                const auto e = m_weightManager.EndPositions()[a];
                if (n == e)
                    continue;

                const auto [cutSize, cutEdges] = m_spSupportGraph->FindMinCut(n, e, PiSigmaSupportGraph::ConstraintType::Pi);

                if (cutSize < 1.0 - 1.e-10)
                {

                    LinearVariableComposition sum;
                    for (const auto& [u, v] : cutEdges)
                        sum += xt::sum(xt::view(m_variables, xt::all(), u, v) + 0)();

                    assert(std::abs(sum.Evaluate(m_model) - cutSize) < 1.e-10);
                    auto constraint = sum >= 1;
                    assert(!constraint.Evaluate(m_model));

                    return constraint;
                }
            }
        }

        return std::nullopt;
    }

    std::optional<LinearConstraint> Separator::Sigma() const
    {
        if (m_weightManager.Dependencies().GetArcs().empty())
            return std::nullopt;

        const auto N = m_weightManager.N();
        const auto A = m_weightManager.A();

        for (size_t n = 0; n < N; ++n)
        {
            if (m_weightManager.Dependencies().GetOutgoingSpan(n).empty())
                continue;

            for (size_t a = 0; a < A; ++a)
            {
                const auto s = m_weightManager.StartPositions()[a];
                if (n == s)
                    continue;

                const auto [cutSize, cutEdges] = m_spSupportGraph->FindMinCut(s, n, PiSigmaSupportGraph::ConstraintType::Sigma);

                if (cutSize < 1.0 - 1.e-10)
                {
                    LinearVariableComposition sum;
                    for (const auto& [u, v] : cutEdges)
                        sum += xt::sum(xt::view(m_variables, xt::all(), u, v) + 0)();

                    auto constraint = sum >= 1;
                    assert(!constraint.Evaluate(m_model));

                    return constraint;
                }
            }
        }

        return std::nullopt;
    }

    std::optional<LinearConstraint> Separator::PiSigma() const
    {
        if (m_weightManager.Dependencies().GetArcs().empty())
            return std::nullopt;

        for (const auto& [s, t] : m_weightManager.Dependencies().GetArcs())
        {
            const auto [cutSize, cutEdges] = m_spSupportGraph->FindMinCut(s, t, PiSigmaSupportGraph::ConstraintType::PiSigma);

            if (cutSize < 1.0 - 1.e-10)
            {
                LinearVariableComposition sum;
                for (const auto& [u, v] : cutEdges)
                    sum += xt::sum(xt::view(m_variables, xt::all(), u, v) + 0)();

                assert(std::abs(sum.Evaluate(m_model) - cutSize) < 1.e-10);
                auto constraint = sum >= 1;
                assert(!constraint.Evaluate(m_model));

                return constraint;
            }
        }

        return std::nullopt;
    }
}


// Copyright (c) 2012-2017 VideoStitch SAS
// Copyright (c) 2018 stitchEm

#ifndef __INPUT_DISTANCE_HPP__
#define __INPUT_DISTANCE_HPP__

#include "calibrationConfig.hpp"
#include "camera.hpp"

#include <ceres/ceres.h>
#include <Eigen/Dense>

#include <unordered_map>
#include <assert.h>

namespace VideoStitch {
namespace Calibration {

#define INDEX_HFOCAL_CAM1 0
#define INDEX_HFOCAL_CAM2 1
#define INDEX_VFOCAL_CAM1 2
#define INDEX_VFOCAL_CAM2 3
#define INDEX_HCENTER_CAM1 4
#define INDEX_HCENTER_CAM2 5
#define INDEX_VCENTER_CAM1 6
#define INDEX_VCENTER_CAM2 7
#define INDEX_LENSDISTORTA_CAM1 8
#define INDEX_LENSDISTORTA_CAM2 9
#define INDEX_LENSDISTORTB_CAM1 10
#define INDEX_LENSDISTORTB_CAM2 11
#define INDEX_LENSDISTORTC_CAM1 12
#define INDEX_LENSDISTORTC_CAM2 13
#define INDEX_ROTATION_CAM1 14
#define INDEX_ROTATION_CAM2 15
#define INDEX_TX_CAM1 16
#define INDEX_TX_CAM2 17
#define INDEX_TY_CAM1 18
#define INDEX_TY_CAM2 19
#define INDEX_TZ_CAM1 20
#define INDEX_TZ_CAM2 21
#define INDEX_3DPOINT 22
#define INDEX_LAST 23

#define SIZE_VFOCAL_CAM1 4
#define SIZE_VFOCAL_CAM2 4
#define SIZE_HFOCAL_CAM1 4
#define SIZE_HFOCAL_CAM2 4
#define SIZE_VCENTER_CAM1 4
#define SIZE_VCENTER_CAM2 4
#define SIZE_HCENTER_CAM1 4
#define SIZE_HCENTER_CAM2 4
#define SIZE_LENSDISTORTA_CAM1 4
#define SIZE_LENSDISTORTA_CAM2 4
#define SIZE_LENSDISTORTB_CAM1 4
#define SIZE_LENSDISTORTB_CAM2 4
#define SIZE_LENSDISTORTC_CAM1 4
#define SIZE_LENSDISTORTC_CAM2 4
#define SIZE_ROTATION_CAM1 9
#define SIZE_ROTATION_CAM2 9
#define SIZE_TX_CAM1 4
#define SIZE_TX_CAM2 4
#define SIZE_TY_CAM1 4
#define SIZE_TY_CAM2 4
#define SIZE_TZ_CAM1 4
#define SIZE_TZ_CAM2 4
#define SIZE_3DPOINT 3

class inputDistanceCostFunction : public ceres::CostFunction {
 public:
  inputDistanceCostFunction(const std::shared_ptr<Camera>& cam1, const std::shared_ptr<Camera>& cam2,
                            const Eigen::Vector2d& impt1, const Eigen::Vector2d& impt2, const CalibrationConfig& config,
                            const double sphereScale)
      : camera1(cam1->clone()),
        camera2(cam2->clone()),
        impt1(impt1),
        impt2(impt2),
        sphereScale(sphereScale),
        needsScaling(std::abs(sphereScale - 1.) > 1e-6) {
    // prepare hashmap of parameter indexes, to get continuously increasing indices and parameter availability through
    // the has() function not all of them may be used in a cost function, depending on the calibration config this
    // hashmap is used to find
    hashmap.reserve(INDEX_LAST);
    int index = 0;

    hashmap[INDEX_HFOCAL_CAM1] = index++;
    if (!config.hasSingleFocal()) {
      hashmap[INDEX_HFOCAL_CAM2] = index++;
    }
    hashmap[INDEX_VFOCAL_CAM1] = index++;
    if (!config.hasSingleFocal()) {
      hashmap[INDEX_VFOCAL_CAM2] = index++;
    }
    hashmap[INDEX_HCENTER_CAM1] = index++;
    hashmap[INDEX_HCENTER_CAM2] = index++;
    hashmap[INDEX_VCENTER_CAM1] = index++;
    hashmap[INDEX_VCENTER_CAM2] = index++;
    hashmap[INDEX_LENSDISTORTA_CAM1] = index++;
    hashmap[INDEX_LENSDISTORTA_CAM2] = index++;
    hashmap[INDEX_LENSDISTORTB_CAM1] = index++;
    hashmap[INDEX_LENSDISTORTB_CAM2] = index++;
    hashmap[INDEX_LENSDISTORTC_CAM1] = index++;
    hashmap[INDEX_LENSDISTORTC_CAM2] = index++;
    hashmap[INDEX_ROTATION_CAM1] = index++;
    hashmap[INDEX_ROTATION_CAM2] = index++;
    assert(index <= INDEX_LAST);

    set_num_residuals(2);
    std::vector<int>* blocks = mutable_parameter_block_sizes();

    // adds the block params if index is in hashmap
#define CONDITIONAL_ADD_BLOCK(PARAM) \
  if (has(INDEX_##PARAM)) blocks->push_back(SIZE_##PARAM)

    CONDITIONAL_ADD_BLOCK(HFOCAL_CAM1);
    CONDITIONAL_ADD_BLOCK(HFOCAL_CAM2);
    CONDITIONAL_ADD_BLOCK(VFOCAL_CAM1);
    CONDITIONAL_ADD_BLOCK(VFOCAL_CAM2);
    CONDITIONAL_ADD_BLOCK(HCENTER_CAM1);
    CONDITIONAL_ADD_BLOCK(HCENTER_CAM2);
    CONDITIONAL_ADD_BLOCK(VCENTER_CAM1);
    CONDITIONAL_ADD_BLOCK(VCENTER_CAM2);
    CONDITIONAL_ADD_BLOCK(LENSDISTORTA_CAM1);
    CONDITIONAL_ADD_BLOCK(LENSDISTORTA_CAM2);
    CONDITIONAL_ADD_BLOCK(LENSDISTORTB_CAM1);
    CONDITIONAL_ADD_BLOCK(LENSDISTORTB_CAM2);
    CONDITIONAL_ADD_BLOCK(LENSDISTORTC_CAM1);
    CONDITIONAL_ADD_BLOCK(LENSDISTORTC_CAM2);
    CONDITIONAL_ADD_BLOCK(ROTATION_CAM1);
    CONDITIONAL_ADD_BLOCK(ROTATION_CAM2);

#undef CONDITIONAL_ADD_BLOCK
  }

  bool has(char parameter_index) const {
    assert(parameter_index < INDEX_LAST);
    return hashmap.find(parameter_index) != hashmap.end();
  }

  size_t map(char parameter_index) const {
    assert(parameter_index < INDEX_LAST);
    return hashmap.at(parameter_index);
  }

  void resetJacobians(double** jacobians) const {
    if (jacobians == nullptr) {
      return;
    }

#define RESET_JACOBIAN(PARAM, ROWS, COLS)                             \
  if (has(INDEX_##PARAM) && jacobians[map(INDEX_##PARAM)] != nullptr) \
  Eigen::Map<Eigen::Matrix<double, ROWS, COLS, Eigen::RowMajor> >(jacobians[map(INDEX_##PARAM)]).fill(0)

    RESET_JACOBIAN(HFOCAL_CAM1, 2, 4);
    RESET_JACOBIAN(HFOCAL_CAM2, 2, 4);
    RESET_JACOBIAN(VFOCAL_CAM1, 2, 4);
    RESET_JACOBIAN(VFOCAL_CAM2, 2, 4);
    RESET_JACOBIAN(HCENTER_CAM1, 2, 4);
    RESET_JACOBIAN(HCENTER_CAM2, 2, 4);
    RESET_JACOBIAN(VCENTER_CAM1, 2, 4);
    RESET_JACOBIAN(VCENTER_CAM2, 2, 4);
    RESET_JACOBIAN(LENSDISTORTA_CAM1, 2, 4);
    RESET_JACOBIAN(LENSDISTORTA_CAM2, 2, 4);
    RESET_JACOBIAN(LENSDISTORTB_CAM1, 2, 4);
    RESET_JACOBIAN(LENSDISTORTB_CAM2, 2, 4);
    RESET_JACOBIAN(LENSDISTORTC_CAM1, 2, 4);
    RESET_JACOBIAN(LENSDISTORTC_CAM2, 2, 4);
    RESET_JACOBIAN(ROTATION_CAM1, 2, 9);
    RESET_JACOBIAN(ROTATION_CAM2, 2, 9);

#undef RESET_JACOBIAN
  }

  virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const {
    bool validProj, validLift;
    Eigen::Vector3d refpt;
    Eigen::Vector2d estimpt;
    Eigen::Matrix<double, 3, 4> Jhfocal_lift;
    Eigen::Matrix<double, 3, 4> Jvfocal_lift;
    Eigen::Matrix<double, 3, 4> Jhcenter_lift;
    Eigen::Matrix<double, 3, 4> Jvcenter_lift;
    Eigen::Matrix<double, 3, 4> JdistortA_lift;
    Eigen::Matrix<double, 3, 4> JdistortB_lift;
    Eigen::Matrix<double, 3, 4> JdistortC_lift;
    Eigen::Matrix<double, 3, 9> Jrotation_lift;
    Eigen::Matrix<double, 3, 4> JtX_lift;
    Eigen::Matrix<double, 3, 4> JtY_lift;
    Eigen::Matrix<double, 3, 4> JtZ_lift;
    Eigen::Matrix<double, 2, 3> Jpoint_project;
    Eigen::Matrix<double, 2, 4> Jhfocal_project;
    Eigen::Matrix<double, 2, 4> Jvfocal_project;
    Eigen::Matrix<double, 2, 4> Jhcenter_project;
    Eigen::Matrix<double, 2, 4> Jvcenter_project;
    Eigen::Matrix<double, 2, 4> JdistortA_project;
    Eigen::Matrix<double, 2, 4> JdistortB_project;
    Eigen::Matrix<double, 2, 4> JdistortC_project;
    Eigen::Matrix<double, 2, 9> Jrotation_project;
    Eigen::Matrix<double, 2, 4> JtX_project;
    Eigen::Matrix<double, 2, 4> JtY_project;
    Eigen::Matrix<double, 2, 4> JtZ_project;

    residuals[0] = 0.0;
    residuals[1] = 0.0;
    resetJacobians(jacobians);

    if (has(INDEX_HFOCAL_CAM1)) {
      camera1->setHorizontalFocal(parameters[map(INDEX_HFOCAL_CAM1)]);
    }
    if (has(INDEX_HFOCAL_CAM2)) {
      camera2->setHorizontalFocal(parameters[map(INDEX_HFOCAL_CAM2)]);
    } else {
      camera2->setHorizontalFocal(parameters[map(INDEX_HFOCAL_CAM1)]);
    }
    if (has(INDEX_VFOCAL_CAM1)) {
      camera1->setVerticalFocal(parameters[map(INDEX_VFOCAL_CAM1)]);
    }
    if (has(INDEX_VFOCAL_CAM2)) {
      camera2->setVerticalFocal(parameters[map(INDEX_VFOCAL_CAM2)]);
    } else {
      camera2->setVerticalFocal(parameters[map(INDEX_VFOCAL_CAM1)]);
    }
    if (has(INDEX_HCENTER_CAM1)) {
      camera1->setHorizontalCenter(parameters[map(INDEX_HCENTER_CAM1)]);
    }
    if (has(INDEX_HCENTER_CAM2)) {
      camera2->setHorizontalCenter(parameters[map(INDEX_HCENTER_CAM2)]);
    }
    if (has(INDEX_VCENTER_CAM1)) {
      camera1->setVerticalCenter(parameters[map(INDEX_VCENTER_CAM1)]);
    }
    if (has(INDEX_VCENTER_CAM2)) {
      camera2->setVerticalCenter(parameters[map(INDEX_VCENTER_CAM2)]);
    }
    if (has(INDEX_LENSDISTORTA_CAM1)) {
      camera1->setDistortionA(parameters[map(INDEX_LENSDISTORTA_CAM1)]);
    }
    if (has(INDEX_LENSDISTORTA_CAM2)) {
      camera2->setDistortionA(parameters[map(INDEX_LENSDISTORTA_CAM2)]);
    }
    if (has(INDEX_LENSDISTORTB_CAM1)) {
      camera1->setDistortionB(parameters[map(INDEX_LENSDISTORTB_CAM1)]);
    }
    if (has(INDEX_LENSDISTORTB_CAM2)) {
      camera2->setDistortionB(parameters[map(INDEX_LENSDISTORTB_CAM2)]);
    }
    if (has(INDEX_LENSDISTORTC_CAM1)) {
      camera1->setDistortionC(parameters[map(INDEX_LENSDISTORTC_CAM1)]);
    }
    if (has(INDEX_LENSDISTORTC_CAM2)) {
      camera2->setDistortionC(parameters[map(INDEX_LENSDISTORTC_CAM2)]);
    }
    if (has(INDEX_ROTATION_CAM1)) {
      camera1->setRotation(parameters[map(INDEX_ROTATION_CAM1)]);
    }
    if (has(INDEX_ROTATION_CAM2)) {
      camera2->setRotation(parameters[map(INDEX_ROTATION_CAM2)]);
    }

    validLift =
        camera1->lift(refpt, Jhfocal_lift, Jvfocal_lift, Jhcenter_lift, Jvcenter_lift, JdistortA_lift, JdistortB_lift,
                      JdistortC_lift, Jrotation_lift, JtX_lift, JtY_lift, JtZ_lift, impt1, sphereScale);
    if (!validLift) {
      return true;
    }

    assert(std::abs(refpt.norm() - sphereScale) < 1e-6 && (bool)"lift() failed to put point at sphereScale");

    validProj = camera2->project(estimpt, Jpoint_project, Jhfocal_project, Jvfocal_project, Jhcenter_project,
                                 Jvcenter_project, JdistortA_project, JdistortB_project, JdistortC_project,
                                 Jrotation_project, JtX_project, JtY_project, JtZ_project, refpt);
    if (!validProj) {
      return true;
    }

    residuals[0] = estimpt(0) - impt2(0);
    residuals[1] = estimpt(1) - impt2(1);

    if (VS_ISNAN(residuals[0])) {
      residuals[0] = 0;
      residuals[1] = 0;
      return true;
    }

    if (jacobians == nullptr) {
      return true;
    }

#define SET_JACOBIAN(PARAM, ROWS, COLS, VALUE)                                                        \
  if (has(INDEX_##PARAM) && jacobians[map(INDEX_##PARAM)] != nullptr) {                               \
    Eigen::Map<Eigen::Matrix<double, ROWS, COLS, Eigen::RowMajor> > J(jacobians[map(INDEX_##PARAM)]); \
    J = (VALUE);                                                                                \
  }

    SET_JACOBIAN(HFOCAL_CAM1, 2, 4, Jpoint_project * Jhfocal_lift);
    SET_JACOBIAN(HFOCAL_CAM2, 2, 4, Jhfocal_project);
    SET_JACOBIAN(VFOCAL_CAM1, 2, 4, Jpoint_project * Jvfocal_lift);
    SET_JACOBIAN(VFOCAL_CAM2, 2, 4, Jvfocal_project);
    SET_JACOBIAN(HCENTER_CAM1, 2, 4, Jpoint_project * Jhcenter_lift);
    SET_JACOBIAN(HCENTER_CAM2, 2, 4, Jhcenter_project);
    SET_JACOBIAN(VCENTER_CAM1, 2, 4, Jpoint_project * Jvcenter_lift);
    SET_JACOBIAN(VCENTER_CAM2, 2, 4, Jvcenter_project);
    SET_JACOBIAN(LENSDISTORTA_CAM1, 2, 4, Jpoint_project * JdistortA_lift);
    SET_JACOBIAN(LENSDISTORTA_CAM2, 2, 4, JdistortA_project);
    SET_JACOBIAN(LENSDISTORTB_CAM1, 2, 4, Jpoint_project * JdistortB_lift);
    SET_JACOBIAN(LENSDISTORTB_CAM2, 2, 4, JdistortB_project);
    SET_JACOBIAN(LENSDISTORTC_CAM1, 2, 4, Jpoint_project * JdistortC_lift);
    SET_JACOBIAN(LENSDISTORTC_CAM2, 2, 4, JdistortC_project);
    SET_JACOBIAN(ROTATION_CAM1, 2, 9, Jpoint_project * Jrotation_lift);
    SET_JACOBIAN(ROTATION_CAM2, 2, 9, Jrotation_project);

#undef SET_JACOBIAN

    return true;
  }

 private:
  std::shared_ptr<Camera> camera1;
  std::shared_ptr<Camera> camera2;
  Eigen::Vector2d impt1;
  Eigen::Vector2d impt2;
  double sphereScale;
  bool needsScaling;
  std::unordered_map<char, size_t> hashmap;
};

}  // namespace Calibration
}  // namespace VideoStitch

#endif


/*
 * $Revision: 223 $ $Date: 2010-03-30 05:44:44 -0700 (Tue, 30 Mar 2010) $
 *
 * Copyright by Astos Solutions GmbH, Germany
 *
 * this file is published under the Astos Solutions Free Public License
 * For details on copyright and terms of use see
 * http://www.astos.de/Astos_Solutions_Free_Public_License.html
 */

#include "Spectrum.h"
#include <Eigen/Array>
#include <Eigen/LU>
#include <cmath>

using namespace vesta;
using namespace Eigen;
using namespace std;

static float XYZtoSRGBMatrixValues[9] =
{
    3.2410f, -1.5374f, -0.4986f,
   -0.9692f,  1.8760f,  0.0416f,
    0.0556f, -0.2040f,  1.0570f
};

static Matrix3f XYZtoSRGB = Matrix3f::Map(XYZtoSRGBMatrixValues).transpose();
static Matrix3f SRGBtoXYZ = XYZtoSRGB.inverse();

/** Normalize the spectrum so that the largest component is equal
  * to 1.0.
  */
void
Spectrum::normalize()
{
    float maxValue = m_samples.cwise().abs().maxCoeff();
    if (maxValue > 0.0f)
    {
        m_samples /= maxValue;
    }
}

/** Convert from CIE XYZ color space to linear sRGB. sRGB gamma
  * correction must be applied in order to convert to the standard
  * sRGB color space. \see Spectrum::LinearSRGBtoSRGB
  */
Spectrum
Spectrum::XYZtoLinearSRGB(const Spectrum& xyz)
{
    Vector3f srgb = XYZtoSRGB * xyz.m_samples.start<3>();
    return Spectrum(srgb.x(), srgb.y(), srgb.z());
}

/** Convert from linear sRGB color space to CIE XYZ.
  */
Spectrum
Spectrum::LinearSRGBtoXYZ(const Spectrum& srgb)
{
    Vector3f xyz = SRGBtoXYZ * srgb.m_samples.start<3>();
    return Spectrum(xyz.x(), xyz.y(), xyz.z());
}

static float fromLinearSRGB(float x)
{
    return x < 0.0031308f ? 12.92f * x : 1.055 * pow(x, 1.0f / 2.4f) - 0.055f;
}

static float toLinearSRGB(float x)
{
    return x < 0.04045f ? x / 12.92f : pow((x + 0.055f) / 1.055f, 2.4f);
}

/** Apply the inverse sRGB gamma correction step to convert from 'linear sRGB'
  * color to sRGB color.
  */
Spectrum
Spectrum::LinearSRGBtoSRGB(const Spectrum& srgb)
{
    return Spectrum(fromLinearSRGB(srgb.red()),
                    fromLinearSRGB(srgb.green()),
                    fromLinearSRGB(srgb.blue()));
}

/** Apply the sRGB gamma correction step to convert from sRGB color space to
  * a linear color space that uses the sRGB tristimulus values.
  */
Spectrum
Spectrum::SRGBtoLinearSRGB(const Spectrum& srgb)
{
    return Spectrum(toLinearSRGB(srgb.red()),
                    toLinearSRGB(srgb.green()),
                    toLinearSRGB(srgb.blue()));
}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

/*! \file PID.hpp
  \brief Proportional-Integral-Derivative Controller
*/

#ifndef PID_H
#define PID_H

#include "Actuator.hpp"
#include "SiconosAlgebraTypeDef.hpp"
#include <boost/circular_buffer.hpp>

class PID : public Actuator
{
private:
  /** default constructor */
  PID() {};

  /** serialization hooks
   */
  ACCEPT_SERIALIZATION(PID);

  /** error vector */
  std::shared_ptr<boost::circular_buffer<double> > _err;

  /** reference we are tracking */
  double _ref;

  double _curDeltaT;

  /** vector of gains */
  SP::SiconosVector _K;

public:

  /** Constructor.
   * \param sensor the ControlSensor feeding the Actuator
   * \param B the B matrix
   */
  PID(SP::ControlSensor sensor, SP::SimpleMatrix B = std::shared_ptr<SimpleMatrix>());

  /** destructor
   */
  virtual ~PID();

  /** initialize actuator data.
   * \param nsds a NonSmoothDynamicalSystem
   * \param s the simulation
   */
  virtual void initialize(const NonSmoothDynamicalSystem& nsds, const Simulation& s);

  /** Compute the new control law at each event
   * Here we are using the following formula:
   * \f$ u_k = u_{k-1} + c_1 e_k + c_2 e_{k-1} + c_3 e_{k-2} \f$ , where
   * \f{array} c_1 &= K_P - \frac{K_D}{\Delta t} + K_I \Delta t \\
   * c_2 &= -1 - \frac{2K_D}{\Delta t} \\
   * c_3 &= \frac{K_D}{\Delta t} \\
   * \f}
   */
  void actuate();

  /** Set K
   * \param K SP::SiconosVector \f$ [K_P, K_I, K_D] \f$
   */
  void setK(SP::SiconosVector K);

  /** Set the value of _ref to reference
   * \param reference the new value
   */
  void inline setRef(double reference)
  {
    _ref = reference;
  }

  /** Get the timestep from the TimeDiscretisation associated with this PID controller
  *  \param td the TimeDiscretisation for this Actuator
  */
  virtual void setTimeDiscretisation(const TimeDiscretisation& td);

  void setDeltaT(double deltaT)
  {
    _curDeltaT = deltaT;
  }
/** display the data of the Actuator on the standard output
   */
  virtual void display() const;

};
#endif


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

// Original version: Melonee Wise <mwise@willowgarage.com>
#include <cstdio>
#include "youbot_driver/generic/PidController.hpp"
#include <boost/math/special_functions/fpclassify.hpp>

namespace youbot {

PidController::PidController(double P, double I, double D, double I1, double I2) :
  p_gain_(P), i_gain_(I), d_gain_(D), i_max_(I1), i_min_(I2)
{
  p_error_last_ = 0.0;
  p_error_ = 0.0;
  d_error_ = 0.0;
  i_error_ = 0.0;
  cmd_ = 0.0;
  last_i_error = 0.0;
}

PidController::~PidController()
{
}

void PidController::initPid(double P, double I, double D, double I1, double I2)
{
  p_gain_ = P;
  i_gain_ = I;
  d_gain_ = D;
  i_max_ = I1;
  i_min_ = I2;

  reset();
}

void PidController::reset()
{
  p_error_last_ = 0.0;
  p_error_ = 0.0;
  d_error_ = 0.0;
  i_error_ = 0.0;
  cmd_ = 0.0;
}

void PidController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
{
  p = p_gain_;
  i = i_gain_;
  d = d_gain_;
  i_max = i_max_;
  i_min = i_min_;
}

void PidController::setGains(double P, double I, double D, double I1, double I2)
{
  p_gain_ = P;
  i_gain_ = I;
  d_gain_ = D;
  i_max_ = I1;
  i_min_ = I2;
}

double PidController::updatePid(double error, boost::posix_time::time_duration dt)
{
  double p_term, d_term, i_term;
  p_error_ = error; //this is pError = pState-pTarget
  double deltatime = (double)dt.total_microseconds()/1000.0; //in milli seconds

  if (deltatime == 0.0 || boost::math::isnan(error) || boost::math::isinf(error))
    return 0.0;

  // Calculate proportional contribution to command
  p_term = p_gain_ * p_error_;

  // Calculate the integral error

  i_error_ = last_i_error + deltatime * p_error_;
  last_i_error = deltatime * p_error_;

  //Calculate integral contribution to command
  i_term = i_gain_ * i_error_;

  // Limit i_term so that the limit is meaningful in the output
  if (i_term > i_max_)
  {
    i_term = i_max_;
    i_error_=i_term/i_gain_;
  }
  else if (i_term < i_min_)
  {
    i_term = i_min_;
    i_error_=i_term/i_gain_;
  }

  // Calculate the derivative error
  if (deltatime != 0)
  {
    d_error_ = (p_error_ - p_error_last_) / deltatime;
    p_error_last_ = p_error_;
  }
  // Calculate derivative contribution to command
  d_term = d_gain_ * d_error_;
  cmd_ = -p_term - i_term - d_term;

 // printf(" p_error_ %lf  i_error_ %lf  p_term %lf i_term %lf  dt %lf out %lf\n", p_error_, i_error_, p_term, i_term, deltatime, cmd_);

  return cmd_;
}

double PidController::updatePid(double error, double error_dot, boost::posix_time::time_duration dt)
{
  double p_term, d_term, i_term;
  p_error_ = error; //this is pError = pState-pTarget
  d_error_ = error_dot;
  double deltatime = (double)dt.total_microseconds()/1000.0;  //in milli seconds

  if (deltatime == 0.0 || boost::math::isnan(error) || boost::math::isinf(error) || boost::math::isnan(error_dot) || boost::math::isinf(error_dot))
    return 0.0;

  // Calculate proportional contribution to command
  p_term = p_gain_ * p_error_;

  // Calculate the integral error
  i_error_ = last_i_error + deltatime * p_error_;
  last_i_error = deltatime * p_error_;

 // i_error_ = i_error_ + deltatime * p_error_;
 //   printf("i_error_ %lf dt.fractional_seconds() %lf\n", i_error_, deltatime);

  //Calculate integral contribution to command
  i_term = i_gain_ * i_error_;

  // Limit i_term so that the limit is meaningful in the output
  if (i_term > i_max_)
  {
    i_term = i_max_;
    i_error_=i_term/i_gain_;
  }
  else if (i_term < i_min_)
  {
    i_term = i_min_;
    i_error_=i_term/i_gain_;
  }

  // Calculate derivative contribution to command
  d_term = d_gain_ * d_error_;
  cmd_ = -p_term - i_term - d_term;

  return cmd_;
}

void PidController::setCurrentCmd(double cmd)
{
  cmd_ = cmd;
}

double PidController::getCurrentCmd()
{
  return cmd_;
}

void PidController::getCurrentPIDErrors(double& pe, double& ie, double& de)
{
  pe = p_error_;
  ie = i_error_;
  de = d_error_;
}

}


#ifndef STAN_MATH_PRIM_SCAL_PROB_VON_MISES_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_VON_MISES_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_greater.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a von Mises random variate for the given location and concentration
 * using the specified random number generator.
 *
 * mu and kappa can each be a scalar or a vector. Any non-scalar
 * inputs must be the same length.
 *
 * The algorithm used in von_mises_rng is a modified version of the
 * algorithm in:
 *
 * Efficient Simulation of the von Mises Distribution
 * D. J. Best and N. I. Fisher
 * Journal of the Royal Statistical Society. Series C (Applied Statistics),
 * Vol. 28, No. 2 (1979), pp. 152-157
 *
 * See licenses/stan-license.txt for Stan license.
 *
 * @tparam T_loc Type of location parameter
 * @tparam T_conc Type of concentration parameter
 * @tparam RNG type of random number generator
 * @param mu (Sequence of) location parameter(s)
 * @param kappa (Sequence of) positive concentration parameter(s)
 * @param rng random number generator
 * @return (Sequence of) von Mises random variate(s)
 * @throw std::domain_error if mu is infinite or kappa is nonpositive
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_loc, typename T_conc, class RNG>
inline typename VectorBuilder<true, double, T_loc, T_conc>::type von_mises_rng(
    const T_loc& mu, const T_conc& kappa, RNG& rng) {
  using boost::random::uniform_real_distribution;
  using boost::variate_generator;
  static const char* function = "von_mises_rng";

  check_finite(function, "mean", mu);
  check_positive_finite(function, "inverse of variance", kappa);
  check_consistent_sizes(function, "Location parameter", mu,
                         "Concentration Parameter", kappa);

  scalar_seq_view<T_loc> mu_vec(mu);
  scalar_seq_view<T_conc> kappa_vec(kappa);
  size_t N = max_size(mu, kappa);
  VectorBuilder<true, double, T_loc, T_conc> output(N);

  variate_generator<RNG&, uniform_real_distribution<> > uniform_rng(
      rng, uniform_real_distribution<>(0.0, 1.0));

  for (size_t n = 0; n < N; ++n) {
    double r = 1 + std::pow((1 + 4 * kappa_vec[n] * kappa_vec[n]), 0.5);
    double rho = 0.5 * (r - std::pow(2 * r, 0.5)) / kappa_vec[n];
    double s = 0.5 * (1 + rho * rho) / rho;

    bool done = false;
    double W;
    while (!done) {
      double Z = std::cos(pi() * uniform_rng());
      W = (1 + s * Z) / (s + Z);
      double Y = kappa_vec[n] * (s - W);
      double U2 = uniform_rng();
      done = Y * (2 - Y) - U2 > 0;

      if (!done) {
        done = std::log(Y / U2) + 1 - Y >= 0;
      }
    }

    double U3 = uniform_rng() - 0.5;
    double sign = ((U3 >= 0) - (U3 <= 0));

    //  it's really an fmod() with a positivity constraint
    output[n]
        = sign * std::acos(W)
          + std::fmod(std::fmod(mu_vec[n], 2 * pi()) + 2 * stan::math::pi(),
                      2 * pi());
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


#ifndef I_SEE_PEE_ITERATOR_HPP
#define I_SEE_PEE_ITERATOR_HPP

#include <Eigen/Dense>
#include <cmath>

namespace i_see_pee {

template<typename T>
struct centered_submap_iterator {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  using data_t = Eigen::Matrix<T, 2ul, 1ul>;
  using bound_t = Eigen::Matrix<size_t, 2ul, 1ul>;

  explicit centered_submap_iterator(const bound_t &_dim) noexcept :
          size_(_dim.cast<T>()), curr_(data_t::Zero()), prod_(size_.prod()) {}

  centered_submap_iterator &operator++() noexcept {
    ++curr_(1);
    if (curr_(1) >= size_(1)) {
      curr_(1) = 0;
      ++curr_(0);
    }
    return *this;
  }

  inline const data_t &operator*() const noexcept {
    return curr_;
  }

  inline const data_t *operator->() const noexcept {
    return &curr_;
  }

  inline bool past_end() const noexcept {
    return curr_(0) >= size_(0) || !prod_;
  }

private:
  data_t size_, curr_;
  size_t prod_;
};

template<typename T>
struct submap_iterator {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  using centered_submap_iterator_t = centered_submap_iterator<T>;
  using data_t = typename centered_submap_iterator_t::data_t;
  using bound_t = typename centered_submap_iterator_t::bound_t;

  submap_iterator() noexcept :
          iter_(bound_t::Zero()), begin_(data_t::Zero()), curr_(data_t::Zero()) {}

  submap_iterator(const data_t &_begin, const bound_t &_size) noexcept :
          iter_(_size), begin_(_begin), curr_(_begin) {}

  submap_iterator &operator++() noexcept {
    ++iter_;
    curr_ = begin_ + *iter_;
    return *this;
  }

  inline const data_t &operator*() const noexcept {
    return curr_;
  }

  inline const data_t *operator->() const noexcept {
    return &curr_;
  }

  inline bool past_end() const noexcept {
    return iter_.past_end();
  }

private:
  data_t begin_, curr_;
  centered_submap_iterator_t iter_;
};

template<typename T>
struct circle_iterator {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  using submap_iterator_t = submap_iterator<T>;
  using data_t = typename submap_iterator_t::data_t;
  using bound_t = typename submap_iterator_t::bound_t;

  circle_iterator(const data_t &_center, T _radius) noexcept :
          center_(_center), radius_(_radius) {
    // get the positive radius
    _radius = std::abs(_radius);

    // get the submap parameters with 1 pixel at the center
    const data_t rad(_radius, _radius);
    const data_t begin = _center - rad;
    const bound_t size = rad.template cast<size_t>() * 2 + bound_t::Ones();
    iter_ = submap_iterator_t(begin, size);

    // advance till valid position to allow instant access
    while (!valid(*iter_)) {
      ++iter_;
    }
  }

  circle_iterator &operator++() noexcept {
    do {
      ++iter_;
    } while (!iter_.past_end() && !valid(*iter_));
    return *this;
  }

  inline const data_t &operator*() const noexcept {
    return *iter_;
  }

  inline const data_t *operator->() const noexcept {
    return iter_.operator->();
  }

  bool past_end() const noexcept {
    return iter_.past_end();
  }

private:

  bool valid(const data_t &_d) const noexcept {
    return (center_ - _d).norm() <= radius_;
  }

  const data_t center_;
  const T radius_;
  submap_iterator_t iter_;
};

} // namespace i_see_pee

#endif //I_SEE_PEE_ITERATOR_HPP


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#ifndef LIMBO_ACQUI_EI_HPP
#define LIMBO_ACQUI_EI_HPP

#include <Eigen/Core>
#include <cmath>
#include <vector>

#include <limbo/opt/optimizer.hpp>
#include <limbo/tools/macros.hpp>

namespace limbo {
    namespace defaults {
        struct acqui_ei {
            /// @ingroup acqui_defaults
            BO_PARAM(double, jitter, 0.0);
        };
    }
    namespace acqui {
        /** @ingroup acqui
        \rst
        Classic EI (Expected Improvement). See :cite:`brochu2010tutorial`, p. 14

          .. math::
            EI(x) = (\mu(x) - f(x^+) - \xi)\Phi(Z) + \sigma(x)\phi(Z),\\\text{with } Z = \frac{\mu(x)-f(x^+) - \xi}{\sigma(x)}.

        Parameters:
          - ``double jitter`` - :math:`\xi`
        \endrst
        */
        template <typename Params, typename Model>
        class EI {
        public:
            EI(const Model& model, int iteration = 0) : _model(model), _nb_samples(-1) {}

            size_t dim_in() const { return _model.dim_in(); }

            size_t dim_out() const { return _model.dim_out(); }

            template <typename AggregatorFunction>
            opt::eval_t operator()(const Eigen::VectorXd& v, const AggregatorFunction& afun, bool gradient)
            {
                assert(!gradient);

                Eigen::VectorXd mu;
                double sigma_sq;
                std::tie(mu, sigma_sq) = _model.query(v);
                double sigma = std::sqrt(sigma_sq);

                // If \sigma(x) = 0 or we do not have any observation yet we return 0
                if (sigma < 1e-10 || _model.samples().size() < 1)
                    return opt::no_grad(0.0);

                // Compute EI(x)
                // First find the best so far (predicted) observation -- if needed
                if (_nb_samples != _model.nb_samples()) {
                    std::vector<double> rewards;
                    for (auto s : _model.samples()) {
                        rewards.push_back(afun(_model.mu(s)));
                    }

                    _nb_samples = _model.nb_samples();
                    _f_max = *std::max_element(rewards.begin(), rewards.end());
                }
                // Calculate Z and \Phi(Z) and \phi(Z)
                double X = afun(mu) - _f_max - Params::acqui_ei::jitter();
                double Z = X / sigma;
                double phi = std::exp(-0.5 * std::pow(Z, 2.0)) / std::sqrt(2.0 * M_PI);
                double Phi = 0.5 * std::erfc(-Z / std::sqrt(2)); //0.5 * (1.0 + std::erf(Z / std::sqrt(2)));

                return opt::no_grad(X * Phi + sigma * phi);
            }

        protected:
            const Model& _model;
            int _nb_samples;
            double _f_max;
        };
    }
}

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SEC_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SEC_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing sec capabilities

    secant of the angle in radian: \f$1/\cos(x)\f$.

    @par Semantic:

    For every parameter of floating type

    @code
    auto r = sec(x);
    @endcode

    @see cos, secd, secpi, rec

  **/
  Value sec(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/sec.hpp>
#include <boost/simd/function/simd/sec.hpp>

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef LA_ALGEBRAIC_CONCEPTS_DOC_INCLUDE
#define LA_ALGEBRAIC_CONCEPTS_DOC_INCLUDE

#ifdef __GXX_CONCEPTS__
#  include <concepts>
#else
#  include <boost/numeric/linear_algebra/pseudo_concept.hpp>
#endif

#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/inverse.hpp>

/// Namespace for purely algebraic concepts
namespace algebra {

/** @addtogroup Concepts
 *  @{
 */

#ifndef __GXX_CONCEPTS__
    //! Concept Commutative
    /*!
        \param Operation A functor implementing a binary operation
        \param Element The type upon the binary operation is defined

        \par Notation:

            op
    	Object of type Operation

            x, y
    	Objects of type Element

        \invariant

            Commutativity
    	op(x, y) == op(y, x)

     */
    template <typename Operation, typename Element>
    struct Commutative
    {};
#else
    concept Commutative<typename Operation, typename Element>
    {
	axiom Commutativity(Operation op, Element x, Element y)
	{
	    op(x, y) == op(y, x);
	}
    };
#endif

#ifndef __GXX_CONCEPTS__
    //! Concept Associative
    /*!
        \param Operation A functor implementing a binary operation
        \param Element The type upon the binary operation is defined

        \par Notation:

            op
    	Object of type Operation

            x, y, z
    	Objects of type Element

        \invariant

            Associativity
    	op(x, op(y, z)) == op(op(x, y), z)

     */
    template <typename Operation, typename Element>
    struct Associative
    {};
#else
    concept Associative<typename Operation, typename Element>
    {
        axiom Associativity(Operation op, Element x, Element y, Element z)
        {
	    op(x, op(y, z)) == op(op(x, y), z);
        }
    };
#endif

#ifndef __GXX_CONCEPTS__
    //! Concept SemiGroup
    /*!
        \param Operation A functor implementing a binary operation
        \param Element The type upon the binary operation is defined

        \note
        -# The algebraic concept SemiGroup only requires associativity and is identical with the concept Associative.
     */
    template <typename Operation, typename Element>
    struct SemiGroup
        : Associative<Operation, Element>
    {};
#else
    auto concept SemiGroup<typename Operation, typename Element>
      : Associative<Operation, Element>
    {};
#endif

#ifndef __GXX_CONCEPTS__
    //! Concept Monoid
    /*!
        \param Operation A functor implementing a binary operation
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- SemiGroup
        \par Notation:

            op
    	Object of type Operation

            x
    	Object of type Element

        \invariant

            Neutrality from right
	    op( x, identity(op, x) ) == x

            Neutrality from left
	    op( identity(op, x), x ) == x

     */
    template <typename Operation, typename Element>
    struct Monoid
      : SemiGroup<Operation, Element>
    {
	/// Associated type; if not defined in concept_map automatically detected as result of identity
        typedef associated_type identity_result_type;
        identity_result_type identity(Operation, Element); ///< Identity element of Operation
    };
#else
    concept Monoid<typename Operation, typename Element>
      : SemiGroup<Operation, Element>
    {
        typename identity_result_type;
        identity_result_type identity(Operation, Element);

        axiom Neutrality(Operation op, Element x)
        {
	    op( x, identity(op, x) ) == x;
	    op( identity(op, x), x ) == x;
        }
    };
#endif

#ifdef __GXX_CONCEPTS__
    auto concept Inversion<typename Operation, typename Element>
    {
        typename inverse_result_type;
        inverse_result_type inverse(Operation, Element);

    };
#else
    //! Concept Inversion
    /*!
        \param Operation A functor implementing a binary operation
        \param Element The type upon the binary operation is defined

	\par Associated Types:
	- inverse_result_type
	\par Valid Expressions:
	- inverse(op, x);
     */
    template <typename Operation, typename Element>
    struct Inversion
    {
	/// Associated type; if not defined in concept_map automatically detected as result of inverse
        typedef associated_type inverse_result_type;

	/// Returns inverse of \p x regarding operation \p op
        inverse_result_type inverse(Operation op, Element x);
    };
#endif

#ifdef __GXX_CONCEPTS__
    concept Group<typename Operation, typename Element>
      : Monoid<Operation, Element>, Inversion<Operation, Element>
    {
        axiom Inversion(Operation op, Element x)
        {
	    op( x, inverse(op, x) ) == identity(op, x);
	    op( inverse(op, x), x ) == identity(op, x);
        }
    };
#else
    //! Concept Group
    /*!
        \param Operation A functor implementing a binary operation
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- Monoid
	- Inversion
        \par Notation:

            op
    	Object of type Operation

            x
    	Object of type Element

        \invariant

            Inverse from right
    	op( x, inverse(op, x) ) == identity(op, x)

            Inverse from left
    	op( inverse(op, x), x ) == identity(op, x)

     */
    template <typename Operation, typename Element>
    struct Group
      : Monoid<Operation, Element>,
	Inversion<Operation, Element>
    {};
#endif

#ifdef __GXX_CONCEPTS__
    auto concept AbelianGroup<typename Operation, typename Element>
      : Group<Operation, Element>, Commutative<Operation, Element>
    {};
#else
    //! Concept AbelianGroup
    /*!
        \param Operation A functor implementing a binary operation
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- Group
	- Commutative
     */
    template <typename Operation, typename Element>
    struct AbelianGroup
      : Group<Operation, Element>,
	Commutative<Operation, Element>
    {};
#endif

#ifdef __GXX_CONCEPTS__
    concept Distributive<typename AddOp, typename MultOp, typename Element>
    {
        axiom Distributivity(AddOp add, MultOp mult, Element x, Element y, Element z)
        {
	    // From left
	    mult(x, add(y, z)) == add(mult(x, y), mult(x, z));
	    // z right
	    mult(add(x, y), z) == add(mult(x, z), mult(y, z));
        }
    };
#else
    //! Concept Distributive
    /*!
        \param AddOp A functor implementing a binary operation representing addition
        \param MultOp A functor implementing a binary operation representing multiplication
        \param Element The type upon the binary operation is defined

        \par Notation:

            add
	    Object of type AddOp

            mult
	    Object of type Multop

            x, y, z
	    Objects of type Element

        \invariant

            Distributivity from left
	    mult(x, add(y, z)) == add(mult(x, y), mult(x, z))

            Distributivity from right
	    mult(add(x, y), z) == add(mult(x, z), mult(y, z))

     */
    template <typename AddOp, typename MultOp, typename Element>
    struct Distributive
    {};
#endif

#ifdef __GXX_CONCEPTS__
    auto concept Ring<typename AddOp, typename MultOp, typename Element>
      : AbelianGroup<AddOp, Element>,
        SemiGroup<MultOp, Element>,
        Distributive<AddOp, MultOp, Element>
    {};
#else
    //! Concept Ring
    /*!
        \param AddOp A functor implementing a binary operation representing addition
        \param MultOp A functor implementing a binary operation representing multiplication
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- AbelianGroup <MultOp, Element>
	- SemiGroup <MultOp, Element>
        - Distributive <AddOp, MultOp, Element>
     */
    template <typename AddOp, typename MultOp, typename Element>
    struct Ring
      : AbelianGroup<AddOp, Element>,
        SemiGroup<MultOp, Element>,
        Distributive<AddOp, MultOp, Element>
    {};
#endif

#ifdef __GXX_CONCEPTS__
    auto concept RingWithIdentity<typename AddOp, typename MultOp, typename Element>
      : Ring<AddOp, MultOp, Element>,
        Monoid<MultOp, Element>
    {};
#else
    //! Concept RingWithIdentity
    /*!
        \param AddOp A functor implementing a binary operation representing addition
        \param MultOp A functor implementing a binary operation representing multiplication
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- Ring <AddOp, MultOp, Element>
        - Monoid <MultOp, Element>
     */
    template <typename AddOp, typename MultOp, typename Element>
    struct RingWithIdentity
      : Ring<AddOp, MultOp, Element>,
        Monoid<MultOp, Element>
    {};
#endif

#ifdef __GXX_CONCEPTS__
    concept DivisionRing<typename AddOp, typename MultOp, typename Element>
      : RingWithIdentity<AddOp, MultOp, Element>,
        Inversion<MultOp, Element>
    {
        // 0 != 1, otherwise trivial
        axiom ZeroIsDifferentFromOne(AddOp add, MultOp mult, Element x)
        {
	    identity(add, x) != identity(mult, x);
        }

        // Non-zero divisibility from left and from right
        axiom NonZeroDivisibility(AddOp add, MultOp mult, Element x)
        {
	    if (x != identity(add, x))
		mult(inverse(mult, x), x) == identity(mult, x);
	    if (x != identity(add, x))
		mult(x, inverse(mult, x)) == identity(mult, x);
        }
    };
#else
    //! Concept DivisionRing
    /*!
        \param AddOp A functor implementing a binary operation representing addition
        \param MultOp A functor implementing a binary operation representing multiplication
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- RingWithIdentity <AddOp, MultOp, Element>
        - Inversion <MultOp, Element>

        \par Notation:

            add
	    Object of type AddOp

            mult
	    Object of type Multop

            x, y, z
	    Objects of type Element

        \invariant

            Non-zero divisibility from left
	    mult(inverse(mult, x), x) == identity(mult, x)
	    if x != identity(add, x)

            Non-zero divisibility from right
	    mult(x, inverse(mult, x)) == identity(mult, x)
	    if x != identity(add, x)

            Zero is different from one
	    identity(add, x) != identity(mult, x)

	\note
	-# Zero and one can be theoretically identical in a DivisionRing.  However,
	   this implies that there is only one element x in the Ring with x + x = x and
	   x * x = x (which is actually even a Field).
	   Because this structure has no practical value we exclude it from
	   consideration.
     */
    template <typename AddOp, typename MultOp, typename Element>
    struct DivisionRing
      : RingWithIdentity<AddOp, MultOp, Element>,
        Inversion<MultOp, Element>
    {};
#endif

#ifdef __GXX_CONCEPTS__
    // SkewField is defined as synonym for DivisionRing
    auto concept SkewField<typename AddOp, typename MultOp, typename Element>
      : DivisionRing<AddOp, MultOp, Element>
    {};
#else
    //! Concept SkewField
    /*!
        \param AddOp A functor implementing a binary operation representing addition
        \param MultOp A functor implementing a binary operation representing multiplication
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- DivisionRing <AddOp, MultOp, Element>
	\note
	- Because the refinement of DivisionRing to SkewField is automatic the two concepts
	  are identical.
     */
    template <typename AddOp, typename MultOp, typename Element>
    struct SkewField
      : DivisionRing<AddOp, MultOp, Element>
    {};
#endif

#ifdef __GXX_CONCEPTS__
    auto concept Field<typename AddOp, typename MultOp, typename Element>
      : DivisionRing<AddOp, MultOp, Element>,
        Commutative<MultOp, Element>
    {};
#else
    //! Concept Field
    /*!
        \param AddOp A functor implementing a binary operation representing addition
        \param MultOp A functor implementing a binary operation representing multiplication
        \param Element The type upon the binary operation is defined

        \par Refinement of:
	- DivisionRing <AddOp, MultOp, Element>
	- Commutative <MultOp, Element>
     */
    template <typename AddOp, typename MultOp, typename Element>
    struct Field
      : DivisionRing<AddOp, MultOp, Element>,
        Commutative<MultOp, Element>
    {};
#endif

/*@}*/ // end of group Concepts

} // algebra

#endif // LA_ALGEBRAIC_CONCEPTS_DOC_INCLUDE


/* ----------------------------------------------------------------------------

 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 * @file Unit3.h
 * @date Feb 02, 2011
 * @author Can Erdogan
 * @author Frank Dellaert
 * @author Alex Trevor
 * @author Zhaoyang Lv
 * @brief The Unit3 class - basically a point on a unit sphere
 */

#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/config.h>  // for GTSAM_USE_TBB

#ifdef __clang__
#  pragma clang diagnostic push
#  pragma clang diagnostic ignored "-Wunused-variable"
#endif
#include <boost/random/uniform_on_sphere.hpp>
#ifdef __clang__
#  pragma clang diagnostic pop
#endif

#include <boost/random/variate_generator.hpp>
#include <iostream>
#include <limits>
#include <cmath>
#include <vector>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2, 3> H) {
  // 3*3 Derivative of representation with respect to point is 3*3:
  Matrix3 D_p_point;
  Unit3 direction;
  direction.p_ = normalize(point, H ? &D_p_point : 0);
  if (H)
    *H << direction.basis().transpose() * D_p_point;
  return direction;
}

/* ************************************************************************* */
Unit3 Unit3::Random(boost::mt19937 & rng) {
  // TODO(dellaert): allow any engine without including all of boost :-(
  boost::uniform_on_sphere<double> randomDirection(3);
  // This variate_generator object is required for versions of boost somewhere
  // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
  boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> > generator(
      rng, randomDirection);
  const vector<double> d = generator();
  return Unit3(d[0], d[1], d[2]);
}

/* ************************************************************************* */
// Get the axis of rotation with the minimum projected length of the point
static Point3 CalculateBestAxis(const Point3& n) {
  double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
  if ((mx <= my) && (mx <= mz)) {
    return Point3(1.0, 0.0, 0.0);
  } else if ((my <= mx) && (my <= mz)) {
    return Point3(0.0, 1.0, 0.0);
  } else {
    return Point3(0, 0, 1);
  }
}

/* ************************************************************************* */
const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
#ifdef GTSAM_USE_TBB
  // NOTE(hayk): At some point it seemed like this reproducably resulted in
  // deadlock. However, I don't know why and I can no longer reproduce it.
  // It either was a red herring or there is still a latent bug left to debug.
  tbb::mutex::scoped_lock lock(B_mutex_);
#endif

  const bool cachedBasis = static_cast<bool>(B_);
  const bool cachedJacobian = static_cast<bool>(H_B_);

  if (H) {
    if (!cachedJacobian) {
      // Compute Jacobian. Recomputes B_
      Matrix32 B;
      Matrix62 jacobian;
      Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1;

      // Choose the direction of the first basis vector b1 in the tangent plane
      // by crossing n with the chosen axis.
      const Point3 n(p_), axis = CalculateBestAxis(n);
      const Point3 B1 = gtsam::cross(n, axis, &H_B1_n);

      // Normalize result to get a unit vector: b1 = B1 / |B1|.
      B.col(0) = normalize(B1, &H_b1_B1);

      // Get the second basis vector b2, which is orthogonal to n and b1.
      B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1);

      // Chain rule tomfoolery to compute the jacobian.
      const Matrix32& H_n_p = B;
      jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p;
      auto H_b1_p = jacobian.block<3, 2>(0, 0);
      jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p;

      // Cache the result and jacobian
      H_B_.reset(jacobian);
      B_.reset(B);
    }

    // Return cached jacobian, possibly computed just above
    *H = *H_B_;
  }

  if (!cachedBasis) {
    // Same calculation as above, without derivatives.
    // Done after H block, as that possibly computes B_ for the first time
    Matrix32 B;

    const Point3 n(p_), axis = CalculateBestAxis(n);
    const Point3 B1 = gtsam::cross(n, axis);
    B.col(0) = normalize(B1);
    B.col(1) = gtsam::cross(n, B.col(0));
    B_.reset(B);
  }

  return *B_;
}

/* ************************************************************************* */
Point3 Unit3::point3(OptionalJacobian<3, 2> H) const {
  if (H)
    *H = basis();
  return Point3(p_);
}

/* ************************************************************************* */
Vector3 Unit3::unitVector(OptionalJacobian<3, 2> H) const {
  if (H)
    *H = basis();
  return p_;
}

/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Unit3& pair) {
  os << pair.p_ << endl;
  return os;
}

/* ************************************************************************* */
void Unit3::print(const std::string& s) const {
  cout << s << ":" << p_ << endl;
}

/* ************************************************************************* */
Matrix3 Unit3::skew() const {
  return skewSymmetric(p_.x(), p_.y(), p_.z());
}

/* ************************************************************************* */
double Unit3::dot(const Unit3& q, OptionalJacobian<1, 2> H_p,
                  OptionalJacobian<1, 2> H_q) const {
  // Get the unit vectors of each, and the derivative.
  Matrix32 H_pn_p;
  Point3 pn = point3(H_p ? &H_pn_p : nullptr);

  Matrix32 H_qn_q;
  const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr);

  // Compute the dot product of the Point3s.
  Matrix13 H_dot_pn, H_dot_qn;
  double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : nullptr, H_q ? &H_dot_qn : nullptr);

  if (H_p) {
    (*H_p) << H_dot_pn * H_pn_p;
  }

  if (H_q) {
    (*H_q) = H_dot_qn * H_qn_q;
  }

  return d;
}

/* ************************************************************************* */
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2, 2> H_q) const {
  // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
  const Vector2 xi = basis().transpose() * q.p_;
  if (H_q) {
    *H_q = basis().transpose() * q.basis();
  }
  return xi;
}

/* ************************************************************************* */
Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p,
                           OptionalJacobian<2, 2> H_q) const {
  // Get the point3 of this, and the derivative.
  Matrix32 H_qn_q;
  const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr);

  // 2D error here is projecting q into the tangent plane of this (p).
  Matrix62 H_B_p;
  Matrix23 Bt = basis(H_p ? &H_B_p : nullptr).transpose();
  Vector2 xi = Bt * qn;

  if (H_p) {
    // Derivatives of each basis vector.
    const Matrix32& H_b1_p = H_B_p.block<3, 2>(0, 0);
    const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0);

    // Derivatives of the two entries of xi wrt the basis vectors.
    const Matrix13 H_xi1_b1 = qn.transpose();
    const Matrix13 H_xi2_b2 = qn.transpose();

    // Assemble dxi/dp = dxi/dB * dB/dp.
    const Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p;
    const Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p;
    *H_p << H_xi1_p, H_xi2_p;
  }

  if (H_q) {
    // dxi/dq is given by dxi/dqu * dqu/dq, where qu is the unit vector of q.
    const Matrix23 H_xi_qu = Bt;
    *H_q = H_xi_qu * H_qn_q;
  }

  return xi;
}

/* ************************************************************************* */
double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const {
  Matrix2 H_xi_q;
  const Vector2 xi = error(q, H ? &H_xi_q : nullptr);
  const double theta = xi.norm();
  if (H)
    *H = (xi.transpose() / theta) * H_xi_q;
  return theta;
}

/* ************************************************************************* */
Unit3 Unit3::retract(const Vector2& v, OptionalJacobian<2,2> H) const {
  // Compute the 3D xi_hat vector
  const Vector3 xi_hat = basis() * v;
  const double theta = xi_hat.norm();
  const double c = std::cos(theta);

  // Treat case of very small v differently.
  Matrix23 H_from_point;
  if (theta < std::numeric_limits<double>::epsilon()) {
    const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat,
                                                 H? &H_from_point : nullptr);
    if (H) { // Jacobian
      *H = H_from_point *
          (-p_ * xi_hat.transpose() + Matrix33::Identity()) * basis();
    }
    return exp_p_xi_hat;
  }

  const double st = std::sin(theta) / theta;
  const Unit3 exp_p_xi_hat = Unit3::FromPoint3(c * p_ + xi_hat * st,
                                               H? &H_from_point : nullptr);
  if (H) { // Jacobian
    *H = H_from_point *
        (p_ * -st * xi_hat.transpose() + st * Matrix33::Identity() +
        xi_hat * ((c - st) / std::pow(theta, 2)) * xi_hat.transpose()) * basis();
  }
  return exp_p_xi_hat;
}

/* ************************************************************************* */
Vector2 Unit3::localCoordinates(const Unit3& other) const {
  const double x = p_.dot(other.p_);
  // Crucial quantity here is y = theta/sin(theta) with theta=acos(x)
  // Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2)
  // We treat the special case 1 and -1 below
  const double x2 = x * x;
  const double z = 1 - x2;
  double y;
  if (z < std::numeric_limits<double>::epsilon()) {
    if (x > 0)  // first order expansion at x=1
      y = 1.0 - (x - 1.0) / 3.0;
    else  // cop out
      return Vector2(M_PI, 0.0);
  } else {
    // no special case
    y = acos(x) / sqrt(z);
  }
  return basis().transpose() * y * (other.p_ - x * p_);
}
/* ************************************************************************* */

}  // namespace gtsam


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file bounded_degree_mst_example.cpp
 * @brief
 * @author Piotr Godlewski
 * @version 1.0
 * @date 2013-11-21
 */

//! [Bounded-Degree Minimum Spanning Tree Example]
#include "paal/iterative_rounding/bounded_degree_min_spanning_tree/bounded_degree_mst.hpp"
#include "paal/utils/functors.hpp"

#include <boost/graph/adjacency_list.hpp>

#include <iostream>
#include <vector>

int main() {
    using Graph = boost::adjacency_list<boost::vecS, boost::vecS,
        boost::undirectedS, boost::no_property,
        boost::property<boost::edge_weight_t, int>>;
    using Edge = boost::graph_traits<Graph>::edge_descriptor;

    // sample problem
    std::vector<std::pair<int, int>> edges {{0,1},{0,2},{1,2},{1,3},{1,4},
        {1,5},{5,0},{3,4}};
    std::vector<int> costs {1,2,1,2,1,1,1,5};
    std::vector<int> bounds {3,2,2,2,2,2};

    Graph g(edges.begin(), edges.end(), costs.begin(), 6);
    auto degree_bounds = paal::utils::make_array_to_functor(bounds);

    std::vector<Edge> result_tree;

    // optional input validity checking
    auto bdmst = paal::ir::make_bounded_degree_mst(
        g, degree_bounds, std::back_inserter(result_tree));
    auto error = bdmst.check_input_validity();
    if (error) {
        std::cerr << "The input is not valid!" << std::endl;
        std::cerr << *error << std::endl;
        return -1;
    }

    // solve it
    auto result = paal::ir::bounded_degree_mst_iterative_rounding(
        g, degree_bounds, std::back_inserter(result_tree));

    // print result
    if (result.first == paal::lp::OPTIMAL) {
        std::cout << "Edges in the spanning tree" << std::endl;
        for (auto e : result_tree) {
            std::cout << "Edge " << e << std::endl;
        }
        std::cout << "Cost of the solution: " << *(result.second) << std::endl;
    } else {
        std::cout << "The instance is infeasible" << std::endl;
    }
    paal::lp::glp::free_env();
    return 0;
}
    //! [Bounded-Degree Minimum Spanning Tree Example]


/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "precomp.hpp"

// Eigen
#include <Eigen/Core>

// OpenCV
#include <opencv2/core/eigen.hpp>
#include <opencv2/sfm/numeric.hpp>

// libmv headers
#include "libmv/numeric/numeric.h"

#include <iostream>

namespace cv
{
namespace sfm
{

template<typename T>
void
meanAndVarianceAlongRows( const Mat_<T> &A,
                          Mat_<T> mean,
                          Mat_<T> variance )
{
  const int n = A.rows, m = A.cols;

  for( int i = 0; i < n; ++i )
  {
    mean(i) = 0;
    variance(i) = 0;

    for( int j = 0; j < m; ++j )
    {
      T x = A(i,j);
      mean(i) += x;
      variance(i) += x*x;
    }
  }

  mean /= m;
  for (int i = 0; i < n; ++i) {
    variance(i) = variance(i) / m - (mean(i)*mean(i));
  }
}

void
meanAndVarianceAlongRows( InputArray _A,
                          OutputArray _mean,
                          OutputArray _variance )
{
  const Mat A = _A.getMat();
  const int depth = A.depth();
  CV_Assert( depth == CV_32F || depth == CV_64F );

  _mean.create(A.rows, 1, depth);
  _variance.create(A.rows, 1, depth);

  Mat mean = _mean.getMat(), variance = _variance.getMat();

  if( depth == CV_32F )
  {
    meanAndVarianceAlongRows<float>( A, mean, variance );
  }
  else
  {
    meanAndVarianceAlongRows<double>( A, mean, variance );
  }
}

//template<typename T>
//inline Mat
//skewMatMinimal( const Mat_<T> &x )
//{
//  Mat_<T> skew(2,3);
//  skew << 0, -1,  x(1),
//          1,  0, -x(0);
//  return skew;
//}
//
//Mat
//skewMatMinimal( InputArray _x )
//{
//  Mat x = _x.getMat();
//  CV_Assert( x.rows == 3 && x.cols == 1 );
//
//  int depth = x.depth();
//  if( depth == CV_32F )
//  {
//    return skewMatMinimal<float>(x);
//  }
//  else
//  {
//    return skewMatMinimal<double>(x);
//  }
//}

template<typename T>
Mat
skewMat( const Mat_<T> &x )
{
  Mat_<T> skew(3,3);
  skew <<   0 , -x(2),  x(1),
          x(2),    0 , -x(0),
         -x(1),  x(0),    0;

  return skew;
}

Mat
skew( InputArray _x )
{
  const Mat x = _x.getMat();
  const int depth = x.depth();
  CV_Assert( x.size() == Size(3,1) || x.size() == Size(1,3) );
  CV_Assert( depth == CV_32F || depth == CV_64F );

  Mat skewMatrix;
  if( depth == CV_32F )
  {
    skewMatrix = skewMat<float>(x);
  }
  else if( depth == CV_64F )
  {
    skewMatrix = skewMat<double>(x);
  }
  else
  {
    //CV_Error(CV_StsBadArg, "The DataType must be CV_32F or CV_64F");
  }

  return skewMatrix;
}

} /* namespace sfm */
} /* namespace cv */