#include <iostream>
#include <fstream>
#include <cmath>
#include <string>

#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

class Interpolation {
public:
	// input:
	static constexpr int POINT_LIMIT = 30; // Maximum size of input point set
	int n; // Number of input points
	float x[POINT_LIMIT], y[POINT_LIMIT];

public:
	// output:
	static constexpr float LEFT_LIMIT = 0.f; // Left end of sampling interval
	static constexpr float RIGHT_LIMIT = 10.f; // Right end of sampling interval
	static constexpr float SAMPLE_RATE = 0.05f; // Distance of adjoining sample points
	static constexpr int SAMPLE_POINTS = static_cast<int>((RIGHT_LIMIT - LEFT_LIMIT) / SAMPLE_RATE) + 1; // Number of total sample points
	float out_x[SAMPLE_POINTS], out_y[SAMPLE_POINTS];

	Interpolation(int num, const float *ix, const float *iy) {
		n = num;
		memcpy_s(x, num * sizeof(float), ix, num * sizeof(float));
		memcpy_s(y, num * sizeof(float), iy, num * sizeof(float));
		memset(out_y, 0, sizeof(float) * SAMPLE_POINTS);
		for (int i = 0; i < SAMPLE_POINTS; ++i) {
			out_x[i] = (LEFT_LIMIT + i * SAMPLE_RATE);
		}
	}

	void outputToCSV(const char *filename, std::string title = "") { // output results to CSV
		std::ofstream csv(filename, std::ios::out | std::ios::app);
		csv << title << ",";
		for (int i = 0; i < SAMPLE_POINTS; ++i) {
			csv << out_y[i] << char(i == SAMPLE_POINTS - 1 ? '\n' : ',');
		}

		csv.close();
	}

	virtual void evaluate() = 0; // interpolate
};

class LinearInterpolation : public Interpolation {
public:
	LinearInterpolation(int num, const float *ix, const float *iy) : Interpolation(num, ix, iy) {}

	void evaluate() final {
		int k = -1;
		for (int i = 0; i < SAMPLE_POINTS; ++i) {
			while (k + 1 < n && x[k + 1] < out_x[i]) ++k;
			if (k == -1 || out_x[i] > x[n - 1]) {
				out_y[i] = 0.0f;
			}
			else {
				float fact = (out_x[i] - x[k]) / (x[k + 1] - x[k]);
				out_y[i] = fact * y[k + 1] + (1.0f - fact) * y[k];
			}
		}
	}
};

class LagrangeInterpolation : public Interpolation {
public:
	LagrangeInterpolation(int num, const float *ix, const float *iy) : Interpolation(num, ix, iy) {}

	void evaluate() final {
		for (int h = 0; h < SAMPLE_POINTS; ++h) {
			float res = 0.0f;
			for (int i = 0; i < n; ++i) {
				float tmp = y[i];
				for (int j = 0; j < n; ++j) {
					if (i != j) {
						tmp = tmp * (out_x[h] - x[j]);
						tmp = tmp / (x[i] - x[j]);
					}
				}

				res += tmp;
			}
			out_y[h] = res;
		}
	}
};

class GaussInterpolation : public Interpolation {
public:
	float b0, sigma;
	GaussInterpolation(int num, const float *ix, const float *iy,
		float _b0, float _sigma = 1.0f) : b0(_b0), sigma(_sigma), Interpolation(num, ix, iy) {}

	float gauss(float x, float x0) {
		return std::exp(-std::pow((x - x0) / sigma, 2.0f) / 2.0f);
	}
	void evaluate() final {
		MatrixXf A(n, n), B(n, 1);
		for (int i = 0; i < n; i++) {
			for (int j = 0; j < n; j++) {
				A(i, j) = gauss(x[i], x[j]);
			}
			B(i, 0) = y[i] - b0;
		}
		MatrixXf W = A.inverse() * B;
		for (int i = 0; i < SAMPLE_POINTS; ++i) {
			float res = b0;
			for (int j = 0; j < n; ++j) {
				res += W(j, 0) * gauss(out_x[i], x[j]);
			}
			out_y[i] = res;
		}
	}
};

class LeastSquareInterpolation : public Interpolation {
public:
	int m;
	LeastSquareInterpolation(int num, const float *ix, const float *iy, int _m)
		: m(_m), Interpolation(num, ix, iy) {}

	void evaluate() final {
		MatrixXf A(n, m + 1);
		for (int i = 0; i < n; ++i) {
			float pow = 1.0f;
			for (int j = m; j >= 0; --j) {
				A(i, j) = pow;
				pow *= x[i];
			}
		}

		MatrixXf B(n, 1);
		for (int i = 0; i < n; ++i) {
			B(i, 0) = y[i];
		}

		MatrixXf W = (A.transpose() * A).inverse() * A.transpose() * B;
		for (int i = 0; i < SAMPLE_POINTS; ++i) {
			float pow = 1.0f, res = 0.0f;
			for (int j = m; j >= 0; --j) {
				res += pow * W(j, 0);
				pow *= out_x[i];
			}
			out_y[i] = res;
		}
	}
};

class RidgeRegressionInterpolation : public Interpolation {
public:
	int m;
	float lambda;
	RidgeRegressionInterpolation(int num, const float *ix, const float *iy,
		int _m, float _lambda) : m(_m), lambda(_lambda), Interpolation(num, ix, iy) {}

	void evaluate() final {
		MatrixXf A(n, m + 1);
		for (int i = 0; i < n; ++i) {
			float pow = 1.0f;
			for (int j = m; j >= 0; --j) {
				A(i, j) = pow;
				pow *= x[i];
			}
		}

		MatrixXf B(n, 1);
		for (int i = 0; i < n; ++i) {
			B(i, 0) = y[i];
		}

		MatrixXf W = (A.transpose() * A + lambda * MatrixXf::Identity(m + 1, m + 1)).inverse() * A.transpose() * B;
		for (int i = 0; i <= m; i++) std::cout << W(i, 0) << " ";
		for (int i = 0; i < SAMPLE_POINTS; ++i) {
			float pow = 1.0f, res = 0.0f;
			for (int j = m; j >= 0; --j) {
				res += pow * W(j, 0);
				pow *= out_x[i];
			}
			out_y[i] = res;
		}
	}
};

int main() {
	std::ifstream dataflow("data.txt", std::ios::in);
	int num;
	float x[Interpolation::POINT_LIMIT], y[Interpolation::POINT_LIMIT];
	dataflow >> num;
	for (int i = 0; i < num; ++i) {
		dataflow >> x[i];
	}
	for (int i = 0; i < num; ++i) {
		dataflow >> y[i];
	}

	dataflow.close();

	LinearInterpolation inter0(num, x, y);
	LagrangeInterpolation inter1(num, x, y);
	GaussInterpolation inter2(num, x, y, 0.0f);
	LeastSquareInterpolation inter3(num, x, y, 2);
	RidgeRegressionInterpolation inter4(num, x, y, 5, 10000.0f);

	inter0.evaluate();
	inter1.evaluate();
	inter2.evaluate();
	inter3.evaluate();
	inter4.evaluate();

	inter0.outputToCSV("result.csv", "LinearInterpolation");
	inter1.outputToCSV("result.csv", "LagrangeInterpolation");
	inter2.outputToCSV("result.csv", "GaussInterpolation");
	inter3.outputToCSV("result.csv", "LeastSquareInterpolation");
	inter4.outputToCSV("result.csv", "RidgeRegressionInterpolation");
	return 0;
}


// To run this script, `cd` to the `./test/fixtures` directory and then execute in the terminal `runWandbox --file --compiler gcc-head --output output.json ./runner.cpp`.

#include <iostream>
#include <vector>
#include <boost/math/special_functions/zeta.hpp>

using namespace std;

vector<double> linspace( double start, double end, int num ) {
	double delta = (end - start) / (num - 1);
	vector<double> arr( num - 1 );
	for ( int i = 0; i < num - 1; ++i ){
		arr[ i ] = start + delta * i;
	}
	arr.push_back( end );
	return arr;
}

void print_vector( vector<double> vec, bool last = false ) {
	cout << "[";
	for ( vector<double>::iterator it = vec.begin(); it != vec.end(); ++it ) {
		if ( vec.end() != it+1 ) {
			cout << setprecision ( 18 ) << *it;
			cout << ",";
		} else {
			cout << setprecision ( 18 ) << *it;
			cout << "]";
			if ( last == false ) {
				cout << ",";
			}
		}
	}
	return;
}

void print_results( vector<double> x, vector<double> expected ) {
	cout << "{" << endl;
	cout << "  \"x\": ";
	print_vector( x );
	cout << "  \"expected\": ";
	print_vector( expected, true );
	cout << "}" << endl;
	return;
}

int main() {
	vector<double> x = linspace( -50.0, 50.0, 2000 );
	vector<double> expected;

	for ( int i = 0; i < 2000; i++  ) {
		expected.push_back( boost::math::zeta( x[ i ] ) );
	}
	print_results( x, expected );
	return 0;
}


// c libaries
#include <ctime>
// c++ libaries
#include <iostream>
// eigen libraries
#include <Eigen/Dense>
// ann - optimization
#include "src/opt/optimize.hpp"
// ann - string
#include "src/str/print.hpp"

//**********************************************************************
// Rosenberg function
//**********************************************************************

struct Rosen{
	double a,b;
	Rosen():a(1.0),b(100.0){};
	Rosen(double aa, double bb):a(aa),b(bb){};
	double operator()(const Eigen::VectorXd& x, Eigen::VectorXd& g){
		g[0]=-2.0*(a-x[0])-4.0*b*x[0]*(x[1]-x[0]*x[0]);
		g[1]=2.0*b*(x[1]-x[0]*x[0]);
		return (a-x[0])*(a-x[0])+b*(x[1]-x[0]*x[0])*(x[1]-x[0]*x[0]);
	};
};

void opt_rosen(Opt::Model& model){
	//rosenberg function
	Rosen rosen;
	//init data
	Opt::Data data(2);
	data.p()[0]=2.0*(1.0*std::rand()/RAND_MAX-0.5);
	data.p()[1]=2.0*(1.0*std::rand()/RAND_MAX-0.5);
	data.pOld()=data.p();
	data.max()=100000;
	data.nPrint()=100;
	data.algo()=model.algo();
	data.stop()=Opt::Stop::FABS;
	data.tol()=1e-8;
	//optimize
	for(int i=0; i<data.max(); ++i){
		//compute the value and gradient
		data.val()=rosen(data.p(),data.g());
		//compute the new position
		model.step(data);
		//calculate the difference
		data.dv()=std::fabs(data.val()-data.valOld());
		data.dp()=(data.p()-data.pOld()).norm();
		//check the break condition
		switch(data.stop()){
			case Opt::Stop::FREL: if(data.dv()<data.tol()) break;
			case Opt::Stop::XREL: if(data.dp()<data.tol()) break;
			case Opt::Stop::FABS: if(data.val()<data.tol()) break;
		}
		//print the status
		data.pOld().noalias()=data.p();//set "old" p value
		data.gOld().noalias()=data.g();//set "old" g value
		data.valOld()=data.val();//set "old" value
		//update step
		++data.step();
	}
	//print
	std::cout<<"n_steps = "<<data.step()<<"\n";
	std::cout<<"val     = "<<data.val()<<"\n";
	std::cout<<"x       = "<<data.p().transpose()<<"\n";
}


// C++ includes
#include <iostream>
using namespace std;

// Eigen includes
#include <Eigen/Core>
using namespace Eigen;

// autodiff include
#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>
using namespace autodiff;

// The vector function for which the Jacobian is needed
VectorXdual f(const VectorXdual& x)
{
    return x * x.sum();
}

int main()
{
    VectorXdual x(5);    // the input vector x with 5 variables
    x << 1, 2, 3, 4, 5;  // x = [1, 2, 3, 4, 5]

    VectorXdual F;  // the output vector F = f(x) evaluated together with Jacobian matrix below

    MatrixXd J = jacobian(f, wrt(x), at(x), F);  // evaluate the output vector F and the Jacobian matrix dF/dx

    cout << "F = \n" << F << endl;  // print the evaluated output vector F
    cout << "J = \n" << J << endl;  // print the evaluated Jacobian matrix dF/dx
}


#include "Utils/Math/pseudo_inverse.hpp"
#include <Eigen/LU>
#include <Eigen/SVD>
#include <stdio.h>
using namespace std;

namespace myUtils {

    void pseudoInverse(Eigen::MatrixXd const & matrix,
                       double sigmaThreshold,
                       Eigen::MatrixXd & invMatrix,
                       Eigen::VectorXd * opt_sigmaOut)    {

        if ((1 == matrix.rows()) && (1 == matrix.cols())) {
            // workaround for Eigen2
            invMatrix.resize(1, 1);
            if (matrix.coeff(0, 0) > sigmaThreshold) {
                invMatrix.coeffRef(0, 0) = 1.0 / matrix.coeff(0, 0);
            }
            else {
                invMatrix.coeffRef(0, 0) = 0.0;
            }
            if (opt_sigmaOut) {
                opt_sigmaOut->resize(1);
                opt_sigmaOut->coeffRef(0) = matrix.coeff(0, 0);
            }
            return;
        }

        Eigen::JacobiSVD<Eigen::MatrixXd> svd(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
        // not sure if we need to svd.sort()... probably not
        int const nrows(svd.singularValues().rows());
        Eigen::MatrixXd invS;
        invS = Eigen::MatrixXd::Zero(nrows, nrows);
        for (int ii(0); ii < nrows; ++ii) {
            if (svd.singularValues().coeff(ii) > sigmaThreshold) {
                invS.coeffRef(ii, ii) = 1.0 / svd.singularValues().coeff(ii);
            }
            else{
                // invS.coeffRef(ii, ii) = 1.0/ sigmaThreshold;
                // printf("sigular value is too small: %f\n", svd.singularValues().coeff(ii));
            }
        }
        invMatrix = svd.matrixV() * invS * svd.matrixU().transpose();
        if (opt_sigmaOut) {
            *opt_sigmaOut = svd.singularValues();
        }
    }

    Eigen::MatrixXd getNullSpace(const Eigen::MatrixXd & J,
                                 const double threshold) {

        Eigen::MatrixXd ret(J.cols(), J.cols());
        Eigen::MatrixXd J_pinv;
        pseudoInverse(J, threshold, J_pinv);
        ret = Eigen::MatrixXd::Identity(J.cols(), J.cols()) - J_pinv * J;
        return ret;
    }
    // JM modified threshold
    void weightedInverse(const Eigen::MatrixXd & J,
                         const Eigen::MatrixXd & Winv,
                         Eigen::MatrixXd & Jinv) {
            Eigen::MatrixXd lambda(J* Winv * J.transpose());
            Eigen::MatrixXd lambda_inv;
            myUtils::pseudoInverse(lambda, 0.0001, lambda_inv);
            //myUtils::pseudoInverse(lambda, 1e-6, lambda_inv);
            Jinv = Winv * J.transpose() * lambda_inv;
    }

}


#include "writer.hpp"
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <array>
#include <cmath>
#include <iostream>
#include <iostream>
#include <stdexcept>
#include <tuple>

//! Sparse Matrix type. Makes using this type easier.
typedef Eigen::SparseMatrix<double> SparseMatrix;

//! Used for filling the sparse matrix.
typedef Eigen::Triplet<double> Triplet;

//! Vector type
typedef Eigen::VectorXd Vector;

//! Our function pointer, typedef'd to make it easier to use
typedef double (*FunctionPointer)(double, double);

//----------------poissonBegin----------------
//! Create the Poisson matrix for 2D finite difference.
//! @param[out] A will be the Poisson matrix (as in the exercise)
//! @param[in] N number of elements in the x-direction
void createPorousMediaMatrix2D(SparseMatrix &A, FunctionPointer sigma, int N, double dx) {
	std::vector<Triplet> triplets;
	A.resize(N * N, N * N);
	triplets.reserve(5 * N * N - 4 * N);

	// Fill up triples

	// (write your solution here)
	A.setZero();
	std::array<double, 5> ss;
	auto s = [&ss](int i, int j) -> double {
		if (i == 0 && j == 0) {
			return ss[0];
		} else if (i == -1 && j == 0) {
			return ss[1];
		} else if (i == 1 && j == 0) {
			return ss[2];
		} else if (i == 0 && j == -1) {
			return ss[3];
		} else if (i == 0 && j == 1) {
			return ss[4];
		} else {
			throw std::runtime_error("s indices out of bounds.");
		}
	};

	for (int i = 0; i < N * N; i++) {
		int ii = i / N + 1;
		int jj = i % N + 1;
		ss     = {
		    sigma(ii * dx, jj * dx),
		    sigma((ii - 1) * dx, jj * dx),
		    sigma((ii + 1) * dx, jj * dx),
		    sigma(ii * dx, (jj - 1) * dx),
		    sigma(ii * dx, (jj + 1) * dx)};

		// u_{i,j}
		triplets.push_back(Triplet(i, i, 4 * s(0, 0)));
		if (i % N != 0) { // u_{i,j-1}
			triplets.push_back(Triplet(i, i - 1, -1 / 4.0 * (1 * s(0, -1) + 4 * s(0, 0) - s(0, 1))));
		}
		if (i % N != N - 1) { // u_{i,j+1}
			triplets.push_back(Triplet(i, i + 1, -1 / 4.0 * (-1 * s(0, -1) + 4 * s(0, 0) + s(0, 1))));
		}
		if (i >= N) { // u_{i-1,j}
			triplets.push_back(Triplet(i, i - N, -1 / 4.0 * (-1 * s(-1, 0) + 4 * s(0, 0) + s(1, 0))));
		}
		if (i < N * N - N) { // u_{i+1,j}
			triplets.push_back(Triplet(i, i + N, -1 / 4.0 * (1 * s(-1, 0) + 4 * s(0, 0) - s(1, 0))));
		}
	}

	A.setFromTriplets(triplets.begin(), triplets.end());
}
//----------------poissonEnd----------------

//----------------RHSBegin----------------
//! Create the Right hand side for the 2D finite difference
//! @param[out] rhs will at the end contain the right hand side
//! @param[in] f the right hand side function f
//! @param[in] N the number of points in the x direction
//! @param[in] dx the cell width
void createRHS(Vector &rhs, FunctionPointer f, int N, double dx) {
	rhs.resize(N * N);

	for (int i = 0; i < N; ++i) {
		for (int j = 0; j < N; ++j) {
			const double x = (i + 1) * dx;
			const double y = (j + 1) * dx;
			rhs[i * N + j] = dx * dx * f(x, y);
		}
	}
}
//----------------RHSEnd----------------

//----------------solveBegin----------------
//! Solve the Poisson equation in 2D
//! @param[in] f the function pointer to f
//! @param[in] N the number of points to use (in x direction)
//!
//! @returns the approximate solution u
Vector poissonSolve(FunctionPointer f, FunctionPointer sigma, int N) {
	Vector u;
	double dx = 1.0 / (N + 1);

	// Compute A, rhs and u
	// (write your solution here)
	SparseMatrix A;
	createPorousMediaMatrix2D(A, sigma, N, dx);

	Vector rhs;
	createRHS(rhs, f, N, dx);

	Eigen::SparseLU<SparseMatrix> solver;
	solver.compute(A);

	if (solver.info() != Eigen::Success) {
		throw std::runtime_error("Could not decompose the matrix.");
	}

	u.resize((N + 2) * (N + 2));
	u.setZero();

	Vector innerU = solver.solve(rhs);

	for (int i = 1; i < N + 1; i++) {
		for (int j = 1; j < N + 1; j++) {
			u[i * (N + 2) + j] = innerU[(i - 1) * N + (j - 1)];
		}
	}

	return u;
}
//----------------solveEnd----------------

//! Gives the Right hand side F at the point x, y
double F(double x, double y) {
	return 4 * M_PI * M_PI * sin(2 * M_PI * x) * sin(2 * M_PI * y) * (4 * cos(2 * M_PI * x) * cos(2 * M_PI * y) + M_PI);
}

//----------------convergenceBegin----------------
//! Gives the exact solution at the point x,y
double exactSolution(double x, double y) {
	// (write your solution here)
	return sin(2 * M_PI * x) * sin(2 * M_PI * y);
	//return 0; //remove when implemented
	//return 1.0/ (4 * M_PI * M_PI) * sin(2*M_PI*x); // constant solution
}

void convergeStudy(FunctionPointer F, FunctionPointer sigma) {
	const int startK = 3;
	const int endK   = 10;

	Vector errors(endK - startK);
	Vector resolutions(errors.size());
	for (int k = startK; k < endK; ++k) {
		const int N = 1 << k; // 2^k

		// (write your solution here)
		resolutions[k - startK] = N;
		Vector u;
		u            = poissonSolve(F, sigma, N);
		double error = 0;
		for (int i = 1; i < N + 2; i++) {
			for (int j = 1; j < N + 2; j++) {
				double e = abs(u[i * (N + 2) + j] - exactSolution(i / double(N + 2), j / double(N + 2)));
				if (e > error) {
					error = e;
				}
			}
		}
		errors[k - startK] = error;
	}

	writeToFile("errors.txt", errors);
	writeToFile("resolutions.txt", resolutions);
}
//----------------convergenceEnd----------------

int main(int, char **) {
	auto sigmaCos = [](double x, double y) {
		return M_PI / 2. + cos(2 * M_PI * x) * cos(2 * M_PI * y);
	};

	auto sigmaConstant = [](double x, double y) {
		std::ignore = x;
		std::ignore = y;
		return 1.0;
	};
	std::ignore = sigmaConstant;
	auto u      = poissonSolve(F, sigmaCos, 500);
	//auto u = poissonSolve(F, sigmaConstant, 500);
	writeToFile("u_fd.txt", u);

	convergeStudy(F, sigmaCos);
}


/** \file  main.cpp
    \brief Test and demonstration program.
           Copyright 2008, 2010 by Erik Schloegl
     */

#include <iostream>
#include <cstdlib>
#include <boost/math/distributions/normal.hpp>
#include <boost/bind.hpp>
#include <random/normal.h>
#include "MCGeneric.hpp"
#include "QFRandom.hpp"
//#include "MCGathererArray.hpp"
#include "BlackScholesAsset.hpp"

using namespace quantfin;

inline double positivePart(double x)
{
  return (x>0.0) ? x : 0.0;
}

class objective_class {
private:
  double initial_stock_price;
  double maturity;
  double interest_rate;
  double volatility;
  double strike;
  double discount_factor;
  double sqrt_maturity;
public:
  inline objective_class(double S,double T,double r,double sgm,double K)
	: initial_stock_price(S),maturity(T),interest_rate(r),volatility(sgm),strike(K),discount_factor(std::exp(-r*T)),sqrt_maturity(std::sqrt(maturity)) { };
  Array<double,1> func(double x);
};

// Return discounted payoff of call and put
Array<double,1> objective_class::func(double x)
{
  Array<double,1> payoff(2);
  payoff(0) = discount_factor * positivePart(initial_stock_price*std::exp((interest_rate-0.5*volatility*volatility)*maturity+volatility*sqrt_maturity*x)-strike);
  payoff(1) = discount_factor * positivePart(strike-initial_stock_price*std::exp((interest_rate-0.5*volatility*volatility)*maturity+volatility*sqrt_maturity*x));
  return payoff;
}

/** Test and demonstration for finite difference schemes.

    Command-line arguments:
      -# initial stock price.
         The default is 100.
      -# interest rate.
         The default is 5%.
      -# volatility.
         The default is 30%. If volatility is not constant, this is the volatility for the
         first time segment.
      -# maturity.
         The default is 1.5.
      -# moneyness.
         The default is 1 (at the money).
      -# minimum MC relative accuracy.
         The default is 1e-2.
      -# maximum MC relative accuracy.
         The default is 1e-2.
  */
int main(int argc,char* argv[])
{
  using std::cout;
  using std::endl;
  using std::flush;

  try {
    double S = 100.0;
    if (argc>1) S = atof(argv[1]);
    double r = 0.05;
    if (argc>2) r = atof(argv[2]);
    double sgm = 0.3;
    if (argc>3) sgm = atof(argv[3]);
    double mat = 1.5;
    if (argc>4) mat = atof(argv[4]);
    double K = 1.0;
    if (argc>5) K = atof(argv[5]);
    //K *= S*std::exp(r*mat);
    K *= S;
    double minacc = 1e-2;
    if (argc>6) minacc = atof(argv[6]);
    double maxacc = 1e-2;
    if (argc>7) maxacc = atof(argv[7]);

    ConstVol vol(sgm);
    BlackScholesAsset stock(&vol,S);
    cout << "S: " << S << "\nK: " << K << "\nr: " << r << "\nT: " << mat << "\nsgm: " << sgm << endl << endl;
    double CFcall = stock.option(mat,K,r);
    double CFput  = stock.option(mat,K,r,-1);
	ranlib::NormalUnit<double> normal_RNG;
	objective_class obj(S,mat,r,sgm,K);
	boost::function<Array<double,1> (double)> func = boost::bind(&objective_class::func,&obj,_1);
	boost::function<double (double)> antithetic = normal_antithetic<double>;
	MCGeneric<double,Array<double,1>,ranlib::NormalUnit<double> > mc(func,normal_RNG);
	MCGeneric<double,Array<double,1>,ranlib::NormalUnit<double> > mc_antithetic(func,normal_RNG,antithetic);
	MCGatherer<Array<double,1> > mcgatherer(2),mcgatherer_antithetic(2);
	double acc = minacc * CFcall;
	maxacc *= CFcall;
	cout << "Aiming for maximum accuracy of " << maxacc << endl;
	cout << "Method,Option,Required accuracy,Actual accuracy,Paths,Black/Scholes value,MC value,95% CI lower bound,95% CI upper bound,Difference in standard errors\n";
	boost::math::normal normal;
	double d = boost::math::quantile(normal,0.95);
	while (acc>=maxacc) {
	  double curr_acc = mc.simulate(mcgatherer,100,acc);
	  Array<double,1> mean(mcgatherer.mean());
	  Array<double,1> stddev(mcgatherer.stddev());
	  cout << "Standard,Call," << acc << ',' << curr_acc << ',' << mcgatherer.number_of_simulations() << ',' << CFcall << ',' << mean(0) << ',' << mean(0)-d*stddev(0) << ',' << mean(0)+d*stddev(0) << ',' << (mean(0)-CFcall)/stddev(0) << endl;
	  cout << "Standard,Put," << acc << ',' << curr_acc << ',' << mcgatherer.number_of_simulations() << ',' << CFput  << ',' << mean(1) << ',' << mean(1)-d*stddev(1) << ',' << mean(1)+d*stddev(1) << ',' << (mean(1)-CFput)/stddev(1) << endl;
	  double curr_acc_antithetic = mc_antithetic.simulate(mcgatherer_antithetic,100,acc);
	  Array<double,1> mean_antithetic(mcgatherer_antithetic.mean());
	  Array<double,1> stddev_antithetic(mcgatherer_antithetic.stddev());
	  cout << "Antithetic,Call," << acc << ',' << curr_acc_antithetic << ',' << mcgatherer_antithetic.number_of_simulations() << ',' << CFcall << ',' << mean_antithetic(0) << ',' << mean_antithetic(0)-d*stddev_antithetic(0) << ',' << mean_antithetic(0)+d*stddev_antithetic(0) << ',' << (mean_antithetic(0)-CFcall)/stddev_antithetic(0) << endl;
	  cout << "Antithetic,Put," << acc << ',' << curr_acc_antithetic << ',' << mcgatherer_antithetic.number_of_simulations() << ',' << CFput  << ',' << mean_antithetic(1) << ',' << mean_antithetic(1)-d*stddev_antithetic(1) << ',' << mean_antithetic(1)+d*stddev_antithetic(1) << ',' << (mean_antithetic(1)-CFput)/stddev_antithetic(1) << endl;
	  acc /= 2; }

	} // end of try block

  catch (std::logic_error xcpt) {
    std::cerr << xcpt.what() << endl; }
  catch (std::runtime_error xcpt) {
    std::cerr << xcpt.what() << endl; }
  catch (...) {
    std::cerr << "Other exception caught" << endl; }

  return 0;
}


// Copyright Christopher Kormanyos 2013.
// Copyright Paul A. Bristow 2013.
// Copyright John Maddock 2013.

// 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).

#ifdef _MSC_VER
#  pragma warning (disable : 4512) // assignment operator could not be generated.
#  pragma warning (disable : 4996) // assignment operator could not be generated.
#endif

#include <iostream>
#include <limits>
#include <vector>
#include <algorithm>
#include <iomanip>
#include <iterator>

// Weisstein, Eric W. "Bessel Function Zeros." From MathWorld--A Wolfram Web Resource.
// http://mathworld.wolfram.com/BesselFunctionZeros.html
// Test values can be calculated using [@wolframalpha.com WolframAplha]
// See also http://dlmf.nist.gov/10.21

//[bessel_zeros_example_1

/*`This example demonstrates calculating zeros of the Bessel and Neumann functions.
It also shows how Boost.Math and Boost.Multiprecision can be combined to provide
a many decimal digit precision. For 50 decimal digit precision we need to include
*/

  #include <boost/multiprecision/cpp_dec_float.hpp>

/*`and a `typedef` for `float_type` may be convenient
(allowing a quick switch to re-compute at built-in `double` or other precision)
*/
  typedef boost::multiprecision::cpp_dec_float_50 float_type;

//`To use the functions for finding zeros of the functions we need

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

//`This file includes the forward declaration signatures for the zero-finding functions:

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

/*`but more details are in the full documentation, for example at
[@http://www.boost.org/doc/libs/1_53_0/libs/math/doc/sf_and_dist/html/math_toolkit/special/bessel/bessel_over.html Boost.Math Bessel functions].
*/

/*`This example shows obtaining both a single zero of the Bessel function,
and then placing multiple zeros into a container like `std::vector` by providing an iterator.
*/
//] [/bessel_zeros_example_1]

/*The signature of the single value function is:

  template <class T>
  inline typename detail::bessel_traits<T, T, policies::policy<> >::result_type
    cyl_bessel_j_zero(
           T v,      // Floating-point value for Jv.
           int m);   // start index.

The result type is controlled by the floating-point type of parameter `v`
(but subject to the usual __precision_policy and __promotion_policy).

The signature of multiple zeros function is:

  template <class T, class OutputIterator>
  inline OutputIterator cyl_bessel_j_zero(
                                T v,                      // Floating-point value for Jv.
                                int start_index,          // 1-based start index.
                                unsigned number_of_zeros, // How many zeros to generate
                                OutputIterator out_it);   // Destination for zeros.

There is also a version which allows control of the __policy_section for error handling and precision.

  template <class T, class OutputIterator, class Policy>
  inline OutputIterator cyl_bessel_j_zero(
                                T v,                      // Floating-point value for Jv.
                                int start_index,          // 1-based start index.
                                unsigned number_of_zeros, // How many zeros to generate
                                OutputIterator out_it,    // Destination for zeros.
                                const Policy& pol);       // Policy to use.
*/

int main()
{
  try
  {
//[bessel_zeros_example_2

/*`[tip It is always wise to place code using Boost.Math inside try'n'catch blocks;
this will ensure that helpful error messages are shown when exceptional conditions arise.]

First, evaluate a single Bessel zero.

The precision is controlled by the float-point type of template parameter `T` of `v`
so this example has `double` precision, at least 15 but up to 17 decimal digits (for the common 64-bit double).
*/
//    double root = boost::math::cyl_bessel_j_zero(0.0, 1);
//    // Displaying with default precision of 6 decimal digits:
//    std::cout << "boost::math::cyl_bessel_j_zero(0.0, 1) " << root << std::endl; // 2.40483
//    // And with all the guaranteed (15) digits:
//    std::cout.precision(std::numeric_limits<double>::digits10);
//    std::cout << "boost::math::cyl_bessel_j_zero(0.0, 1) " << root << std::endl; // 2.40482555769577
/*`But note that because the parameter `v` controls the precision of the result,
`v` [*must be a floating-point type].
So if you provide an integer type, say 0, rather than 0.0, then it will fail to compile thus:
``
    root = boost::math::cyl_bessel_j_zero(0, 1);
``
with this error message
``
  error C2338: Order must be a floating-point type.
``

Optionally, we can use a policy to ignore errors, C-style, returning some value,
perhaps infinity or NaN, or the best that can be done. (See __user_error_handling).

To create a (possibly unwise!) policy `ignore_all_policy` that ignores all errors:
*/

  typedef boost::math::policies::policy<
    boost::math::policies::domain_error<boost::math::policies::ignore_error>,
    boost::math::policies::overflow_error<boost::math::policies::ignore_error>,
    boost::math::policies::underflow_error<boost::math::policies::ignore_error>,
    boost::math::policies::denorm_error<boost::math::policies::ignore_error>,
    boost::math::policies::pole_error<boost::math::policies::ignore_error>,
    boost::math::policies::evaluation_error<boost::math::policies::ignore_error>
              > ignore_all_policy;
 //`Examples of use of this `ignore_all_policy` are

    double inf = std::numeric_limits<double>::infinity();
    double nan = std::numeric_limits<double>::quiet_NaN();

    double dodgy_root = boost::math::cyl_bessel_j_zero(-1.0, 1, ignore_all_policy());
    std::cout << "boost::math::cyl_bessel_j_zero(-1.0, 1) " << dodgy_root << std::endl; // 1.#QNAN
    double inf_root = boost::math::cyl_bessel_j_zero(inf, 1, ignore_all_policy());
    std::cout << "boost::math::cyl_bessel_j_zero(inf, 1) " << inf_root << std::endl; // 1.#QNAN
    double nan_root = boost::math::cyl_bessel_j_zero(nan, 1, ignore_all_policy());
    std::cout << "boost::math::cyl_bessel_j_zero(nan, 1) " << nan_root << std::endl; // 1.#QNAN

/*`Another version of `cyl_bessel_j_zero`  allows calculation of multiple zeros with one call,
placing the results in a container, often `std::vector`.
For example, generate and display the first five `double` roots of J[sub v] for integral order 2,
as column ['J[sub 2](x)] in table 1 of
[@ http://mathworld.wolfram.com/BesselFunctionZeros.html Wolfram Bessel Function Zeros].
*/
    unsigned int n_roots = 5U;
    std::vector<double> roots;
    boost::math::cyl_bessel_j_zero(2.0, 1, n_roots, std::back_inserter(roots));
    std::copy(roots.begin(),
              roots.end(),
              std::ostream_iterator<double>(std::cout, "\n"));

/*`Or we can use Boost.Multiprecision to generate 50 decimal digit roots of ['J[sub v]]
for non-integral order `v= 71/19 == 3.736842`, expressed as an exact-integer fraction
to generate the most accurate value possible for all floating-point types.

We set the precision of the output stream, and show trailing zeros to display a fixed 50 decimal digits.
*/
    std::cout.precision(std::numeric_limits<float_type>::digits10); // 50 decimal digits.
    std::cout << std::showpoint << std::endl; // Show trailing zeros.

    float_type x = float_type(71) / 19;
    float_type r = boost::math::cyl_bessel_j_zero(x, 1); // 1st root.
    std::cout << "x = " << x << ", r = " << r << std::endl;

    r = boost::math::cyl_bessel_j_zero(x, 20U); // 20th root.
    std::cout << "x = " << x << ", r = " << r << std::endl;

    std::vector<float_type> zeros;
    boost::math::cyl_bessel_j_zero(x, 1, 3, std::back_inserter(zeros));

    std::cout << "cyl_bessel_j_zeros" << std::endl;
    // Print the roots to the output stream.
    std::copy(zeros.begin(), zeros.end(),
              std::ostream_iterator<float_type>(std::cout, "\n"));
//] [/bessel_zeros_example_2]
  }
  catch (std::exception const& ex)
  {
    std::cout << "Thrown exception " << ex.what() << std::endl;
  }

 } // int main()

 /*

 Output:

   Description: Autorun "J:\Cpp\big_number\Debug\bessel_zeros_example_1.exe"
  boost::math::cyl_bessel_j_zero(-1.0, 1) 3.83171
  boost::math::cyl_bessel_j_zero(inf, 1) 1.#QNAN
  boost::math::cyl_bessel_j_zero(nan, 1) 1.#QNAN
  5.13562
  8.41724
  11.6198
  14.796
  17.9598

  x = 3.7368421052631578947368421052631578947368421052632, r = 7.2731751938316489503185694262290765588963196701623
  x = 3.7368421052631578947368421052631578947368421052632, r = 67.815145619696290925556791375555951165111460585458
  cyl_bessel_j_zeros
  7.2731751938316489503185694262290765588963196701623
  10.724858308883141732536172745851416647110749599085
  14.018504599452388106120459558042660282427471931581

*/


//
//  Functions.h
//  Functor classes for activation and activation derivative functions.
//

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

#ifndef Cynapse_Functions_h
#define Cynapse_Functions_h

double sigmoid(double theta) {
    return 1.0 / (1.0 + exp(-theta));
}

double sigmoid_derivative(double z) {
    return sigmoid(z) * (1-sigmoid(z));
}

double heaviside(double theta) {
    return (theta >= 0.5 ? 1.0 : 0.0);
}

Eigen::MatrixXd quadratic_cost_derivative(Eigen::MatrixXd output, Eigen::MatrixXd input) {
    return output - input;
}

class FunctionFactory
{
public:
    static std::pair<std::function<double(double)>, std::function<double(double)> >
        create(const std::string& funcName) {

        std::function<double(double)> activation_func;
        std::function<double(double)> activation_deriv;

        if (funcName == "sigmoid") {
            activation_func = std::function<double(double)>(sigmoid);
            activation_deriv = std::function<double(double)>(sigmoid_derivative);
        }
        else if (funcName == "step")
        {
            activation_func = std::function<double(double)>(heaviside);
        }
        else
        {
            throw std::runtime_error("Function " + funcName + " is not supported.");
        }

        return std::pair<std::function<double(double)>, std::function<double(double)> >(activation_func, activation_deriv);
    }
};

#endif


#include <iostream>
#include <Eigen/Core>
#include <Eigen/QR>
#include <Eigen/Dense>

#include "gramschmidt.hpp"

using namespace std;
using namespace Eigen;

void test1(const MatrixXd& A)
{
    cout << "======================================" << endl;
    cout << "== Usual way to get null space of A ==" << endl;
    cout << "== A is m x n matrix (m >= n)       ==" << endl;
    cout << "======================================" << endl;

    HouseholderQR<MatrixXd> qr(A);
    MatrixXd R = qr.matrixQR().triangularView<Upper>();
    MatrixXd Q = qr.householderQ();

    cout << "=== A ===" << endl;
    cout << A << endl;
    cout << "=== Q ===" << endl;
    cout << Q << endl;
    cout << "=== R ===" << endl;
    cout << R << endl;

    cout << "=== Q*R ===" << endl;
    cout << Q*R << endl;

    MatrixXd Q1 = Q.block(0,0,A.rows(),A.cols());
    MatrixXd Q2 = Q.block(0,A.cols(),A.rows(),Q.cols()-A.cols());

    cout << "== Q1 ==" << endl;
    cout << Q1 << endl;
    cout << "== Q2 ==" << endl;
    cout << Q2 << endl;

    cout << "============================================" << endl;
    cout << "== Q2 is the basis of the Null Space of A ==" << endl;
    cout << "============================================" << endl;
    cout << "== A^T*Q2 ==" << endl;
    cout << A.transpose()*Q2 << endl;
}

void test2(const MatrixXd& A)
{
    cout << "======================================" << endl;
    cout << "== Usual way to get null space of B ==" << endl;
    cout << "== B is m x n matrix (m <= n)       ==" << endl;
    cout << "======================================" << endl;

    MatrixXd B;
    B = A.transpose();

    HouseholderQR<MatrixXd> qr(B);
    MatrixXd R = qr.matrixQR().triangularView<Upper>();
    MatrixXd Q = qr.householderQ();

    cout << "=== B ===" << endl;
    cout << B << endl;
    cout << "=== Q ===" << endl;
    cout << Q << endl;
    cout << "=== R ===" << endl;
    cout << R << endl;

    cout << "=== Q*R ===" << endl;
    cout << Q*R << endl;

    MatrixXd R1 = R.block(0,0,R.rows(),R.rows());
    MatrixXd R2 = R.block(0,R.rows(),R.rows(),R.cols()-R.rows());

    cout << "== R1 ==" << endl;
    cout << R1 << endl;
    cout << "== R2 ==" << endl;
    cout << R2 << endl;

    cout << "== N is following matrix ==" << endl;
    cout << "=====================" << endl;
    cout << "== | -R1^(-1)*R2 | ==" << endl;
    cout << "== |      I      | ==" << endl;
    cout << "=====================" << endl;
    MatrixXd N = MatrixXd::Zero(B.cols(),B.cols()-B.rows());
    N.block(0,0,B.rows(),B.cols()-B.rows()) = -R1.inverse()*R2;
    N.block(B.rows(),0,B.cols()-B.rows(),B.cols()-B.rows()) = MatrixXd::Identity(B.cols()-B.rows(),B.cols()-B.rows());
    cout << N << endl;
    cout << "==============================" << endl;
    cout << "== N is the Null space of B ==" << endl;
    cout << "==============================" << endl;
    cout << "== B*N ==" << endl;
    cout << B*N << endl;
}

void test3(const MatrixXd& A)
{
    cout << "==========================================" << endl;
    cout << "== Column pivot QR decomposition method ==" << endl;
    cout << "==========================================" << endl;
    MatrixXd B = A.transpose();

    ColPivHouseholderQR<MatrixXd> qr(B);
    MatrixXd Q = qr.matrixQ();
    MatrixXd R = qr.matrixR().triangularView<Upper>();
    MatrixXd P = qr.colsPermutation();

    cout << "=== B ===" << endl;
    cout << B << endl;
    cout << "=== Q ===" << endl;
    cout << Q << endl;
    cout << "=== R ===" << endl;
    cout << R << endl;
    cout << "=== P ===" << endl;
    cout << P << endl;

    cout << "=== Q*R*P^T ===" << endl;
    cout << Q*R*P.transpose() << endl;

    R *= P.transpose();

    MatrixXd R1 = R.block(0,0,R.rows(),R.rows());
    MatrixXd R2 = R.block(0,R.rows(),R.rows(),R.cols()-R.rows());

    cout << "== R1 ==" << endl;
    cout << R1 << endl;
    cout << "== R2 ==" << endl;
    cout << R2 << endl;

    cout << "== N is following matrix ==" << endl;
    cout << "=====================" << endl;
    cout << "== | -R1^(-1)*R2 | ==" << endl;
    cout << "== |      I      | ==" << endl;
    cout << "=====================" << endl;
    MatrixXd N = MatrixXd::Zero(B.cols(),B.cols()-B.rows());
    N.block(0,0,B.rows(),B.cols()-B.rows()) = -R1.inverse()*R2;
    N.block(B.rows(),0,B.cols()-B.rows(),B.cols()-B.rows()) = MatrixXd::Identity(B.cols()-B.rows(),B.cols()-B.rows());
    cout << N << endl;
    cout << "==============================" << endl;
    cout << "== N is the Null space of B ==" << endl;
    cout << "==============================" << endl;
    cout << "== B*N ==" << endl;
    cout << B*N << endl;

}

void test4(const MatrixXd& A)
{
    cout << "========================================" << endl;
    cout << "== Full pivot QR decomposition method ==" << endl;
    cout << "========================================" << endl;
    MatrixXd B = A.transpose();

    FullPivHouseholderQR<MatrixXd> qr(B);
    MatrixXd Q = qr.matrixQ();
    MatrixXd R = qr.matrixQR().triangularView<Upper>();
    MatrixXd P = qr.colsPermutation();

    cout << "=== B ===" << endl;
    cout << B << endl;
    cout << "=== Q ===" << endl;
    cout << Q << endl;
    cout << "=== R ===" << endl;
    cout << R << endl;
    cout << "=== P ===" << endl;
    cout << P << endl;

    cout << "=== Q*R*P^T ===" << endl;
    cout << Q*R*P.transpose() << endl;

    R *= P.transpose();

    MatrixXd R1 = R.block(0,0,R.rows(),R.rows());
    MatrixXd R2 = R.block(0,R.rows(),R.rows(),R.cols()-R.rows());

    cout << "== R1 ==" << endl;
    cout << R1 << endl;
    cout << "== R2 ==" << endl;
    cout << R2 << endl;

    cout << "== N is following matrix ==" << endl;
    cout << "=====================" << endl;
    cout << "== | -R1^(-1)*R2 | ==" << endl;
    cout << "== |      I      | ==" << endl;
    cout << "=====================" << endl;
    MatrixXd N = MatrixXd::Zero(B.cols(),B.cols()-B.rows());
    N.block(0,0,B.rows(),B.cols()-B.rows()) = -R1.inverse()*R2;
    N.block(B.rows(),0,B.cols()-B.rows(),B.cols()-B.rows()) = MatrixXd::Identity(B.cols()-B.rows(),B.cols()-B.rows());
    cout << N << endl;
    cout << "==============================" << endl;
    cout << "== N is the Null space of B ==" << endl;
    cout << "==============================" << endl;
    cout << "== B*N ==" << endl;
    cout << B*N << endl;
}

int main(void)
{
    // Set Matrix
    MatrixXd A(6,4);
    A <<
        1, -2, 4, -8,
        2, -1, 1, -1,
        3, 2, 0, 3,
        4, 1, 1, 1,
        7, 2, 4, 8,
        10, -4, 9, -2;

    test1(A);
    test2(A);
    test3(A);
    test4(A);

    return 0;
}


#include <sm/kinematics/rotations.hpp>
#include <sm/assert_macros.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/round.hpp>

namespace sm { namespace kinematics {

  // Euler angle rotations.
  Eigen::Matrix3d Rx(double radians){
    Eigen::Matrix3d C;
    double c = cos(radians);
    double s = sin(radians);
    C(0,0) = 1;   C(0,1) = 0.0; C(0,2) =  0;
    C(1,0) = 0.0; C(1,1) = c;   C(1,2) = -s;
    C(2,0) = 0.0; C(2,1) = s;   C(2,2) =  c;

    return C;
  }

  Eigen::Matrix3d Ry(double radians){
    Eigen::Matrix3d C;
    double c = cos(radians);
    double s = sin(radians);
    C(0,0) =  c;   C(0,1) = 0.0; C(0,2) =  s;
    C(1,0) =  0.0; C(1,1) = 1;   C(1,2) = 0.0;
    C(2,0) = -s;   C(2,1) = 0.0; C(2,2) =  c;

    return C;
  }
  Eigen::Matrix3d Rz(double radians){
    Eigen::Matrix3d C;
    double c = cos(radians);
    double s = sin(radians);
    C(0,0) = c;   C(0,1) = -s;  C(0,2) = 0.0;
    C(1,0) = s;   C(1,1) =  c;  C(1,2) = 0.0;
    C(2,0) = 0.0; C(2,1) = 0.0; C(2,2) =  1;

    return C;
  }

  Eigen::Matrix3d rph2R(double x, double y, double z){
    Eigen::Matrix3d C;
    double cx = cos(x);
    double sx = sin(x);
    double cy = cos(y);
    double sy = sin(y);
    double cz = cos(z);
    double sz = sin(z);
    //[cos(z)*cos(y), -sin(z)*cos(x)+cos(z)*sin(y)*sin(x),  sin(z)*sin(x)+cos(z)*sin(y)*cos(x)]
    //[sin(z)*cos(y),  cos(z)*cos(x)+sin(z)*sin(y)*sin(x), -cos(z)*sin(x)+sin(z)*sin(y)*cos(x)]
    //[      -sin(y),                       cos(y)*sin(x),                       cos(y)*cos(x)]
    C(0,0) = cz*cy; C(0,1) = -sz*cx+cz*sy*sx; C(0,2) =   sz*sx+cz*sy*cx;
    C(1,0) = sz*cy; C(1,1) =  cz*cx+sz*sy*sx; C(1,2) =  -cz*sx+sz*sy*cx;
    C(2,0) = -sy;   C(2,1) =           cy*sx; C(2,2) =            cy*cx;

    return C;
  }
  Eigen::Matrix3d rph2R(Eigen::Vector3d const & x){
    return rph2R(x[0],x[1],x[2]);
  }

  Eigen::Vector3d R2rph(Eigen::Matrix3d const & C){
    double phi = asin(C(2,0));
    double theta = atan2(C(2,1),C(2,2));
    double psi = atan2(C(1,0), C(0,0));

    Eigen::Vector3d ret;
    ret[0] = theta;
    ret[1] = -phi;
    ret[2] = psi;

    return ret;
  }

  //// Small angle approximation.
  template <typename Scalar_>
  Eigen::Matrix<Scalar_, 3, 3> crossMx(Scalar_ x, Scalar_ y, Scalar_ z){
    Eigen::Matrix<Scalar_, 3, 3> C;
    C(0,0) =  0.0; C(0,1) = -z;   C(0,2) =   y;
    C(1,0) =  z;   C(1,1) =  0.0; C(1,2) =  -x;
    C(2,0) = -y;   C(2,1) =  x;   C(2,2) =   0.0;
    return C;
  }
  template Eigen::Matrix<double, 3, 3> crossMx(double x, double y, double z);
  template Eigen::Matrix<float, 3, 3> crossMx(float x, float y, float z);

  template Eigen::Matrix<double, 3, 3> crossMx(Eigen::MatrixBase<Eigen::Matrix<double, 3, 1> > const &);
  template Eigen::Matrix<float, 3, 3> crossMx(Eigen::MatrixBase<Eigen::Matrix<float, 3, 1> > const &);

  template Eigen::Matrix<double, 3, 3> crossMx(Eigen::MatrixBase<Eigen::Matrix<double, Eigen::Dynamic, 1> > const &);
  template Eigen::Matrix<float, 3, 3> crossMx(Eigen::MatrixBase<Eigen::Matrix<float, Eigen::Dynamic, 1> > const &);

  // Axis Angle rotation.
  Eigen::Matrix3d axisAngle2R(double a, double ax, double ay, double az){

    SM_ASSERT_LT_DBG(std::runtime_error,fabs(sqrt(ax*ax + ay*ay + az*az) - 1.0), 1e-4, "The axis is not a unit vector. ||a|| = " << (sqrt(ax*ax + ay*ay + az*az)));

    if(a < 1e-12)
      return Eigen::Matrix3d::Identity();
    // e = [ax ay az]
    // e*(e') + (eye(3) - e*(e'))*cos(a) - crossMx(e) * sin(a) =
    //[         ax^2+ca*(1-ax^2), ax*ay-ca*ax*ay+sa*az, ax*az-ca*ax*az-sa*ay]
    //[ ax*ay-ca*ax*ay-sa*az,         ay^2+ca*(1-ay^2), ay*az-ca*ay*az+sa*ax]
    //[ ax*az-ca*ax*az+sa*ay, ay*az-ca*ay*az-sa*ax,         az^2+ca*(1-az^2)]
    double sa = sin(a);
    double ca = cos(a);
    double ax2 = ax*ax;
    double ay2 = ay*ay;
    double az2 = az*az;
    double const one = double(1);

    Eigen::Matrix3d C;
    C(0,0) =  ax2+ca*(one-ax2);       C(0,1) = ax*ay-ca*ax*ay+sa*az; C(0,2) = ax*az-ca*ax*az-sa*ay;
    C(1,0) =  ax*ay-ca*ax*ay-sa*az; C(1,1) = ay2+ca*(one-ay2);       C(1,2) = ay*az-ca*ay*az+sa*ax;
    C(2,0) =  ax*az-ca*ax*az+sa*ay; C(2,1) = ay*az-ca*ay*az-sa*ax; C(2,2) = az2+ca*(one-az2);

    return C;
  }
  Eigen::Matrix3d axisAngle2R(double x, double y, double z) {
    double a = sqrt(x*x + y*y + z*z);
    if(a < 1e-12)
      return Eigen::Matrix3d::Identity();

    double d = 1/a;
    return axisAngle2R(a,x*d, y*d, z*d);
  }

  Eigen::Matrix3d axisAngle2R(Eigen::Vector3d const & x){
    return axisAngle2R(x[0], x[1], x[2]);
  }

  Eigen::Vector3d R2AxisAngle(Eigen::Matrix3d const & C){
    // Sometimes, because of roundoff error, the value of tr ends up outside
    // the valid range of arccos. Truncate to the valid range.
    double tr = std::max(-1.0, std::min( (C(0,0) + C(1,1) + C(2,2) - 1.0) * 0.5, 1.0));
    double a = acos( tr ) ;

    Eigen::Vector3d axis;

    if(fabs(a) < 1e-10) {
      return Eigen::Vector3d::Zero();
    }

    axis[0] = (C(2,1) - C(1,2));
    axis[1] = (C(0,2) - C(2,0));
    axis[2] = (C(1,0) - C(0,1));
    double n2 = axis.norm();
    if(fabs(n2) < 1e-10)
      return Eigen::Vector3d::Zero();

    double scale = -a/n2;
    axis = scale * axis;

    return axis;

  }

  // Utility functions
  double angleMod(double radians){
    return (double)(radians - (SM_2PI * boost::math::round(radians / SM_2PI)));
  }
  double deg2rad(double degrees){
    return (double)(degrees * SM_DEG2RAD);
  }
  double rad2deg(double radians){
    return (double)(radians * SM_RAD2DEG);
  }

  Eigen::Matrix3d Cx(double radians)
  {
    return Rx(-radians);
  }
  Eigen::Matrix3d Cy(double radians)
  {
    return Ry(-radians);
  }
  Eigen::Matrix3d Cz(double radians)
  {
    return Rz(-radians);
  }

   Eigen::Matrix3d rph2C(double x, double y, double z)
   {
     return rph2R(-x,-y,-z);
   }

   Eigen::Matrix3d rph2C(Eigen::Vector3d const & x)
   {
     return rph2C(x[0],x[1],x[2]);
   }
   Eigen::Matrix3d rph2C(Eigen::VectorXd const & x)
   {
     SM_ASSERT_EQ_DBG(std::runtime_error,x.size(),3,"The input vector must have 3 components");
     return rph2C(x[0],x[1],x[2]);
   }

   Eigen::Matrix3d rph2C(Eigen::MatrixXd const & A, unsigned column)
   {
     SM_ASSERT_EQ_DBG(std::runtime_error,A.rows(),3,"The input matrix must have 3 rows");
     SM_ASSERT_LT_DBG(std::runtime_error,column,A.cols(),"The requested column is out of bounds");
     return rph2C(A(0,column),A(1,column),A(2,column));
   }

   Eigen::Vector3d C2rph(Eigen::MatrixXd const & C)
   {
     SM_ASSERT_EQ_DBG(std::runtime_error,C.rows(),3,"The input matrix must be 3x3");
     SM_ASSERT_EQ_DBG(std::runtime_error,C.cols(),3,"The input matrix must be 3x3");

     Eigen::Vector3d rph;

     rph[1] = asin(C(2,0));
     rph[2] = atan2(-C(1,0),C(0,0));
     rph[0] = atan2(-C(2,1),C(2,2));

     return rph;
   }

   Eigen::Vector3d C2rph(Eigen::Matrix3d const & C)
   {
     Eigen::Vector3d rph;

     rph[1] = asin(C(2,0));
     rph[2] = atan2(-C(1,0),C(0,0));
     rph[0] = atan2(-C(2,1),C(2,2));

     return rph;
   }

  }} // sm::kinematics


#include <iostream>
#include <cmath>

using namespace std;

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

using namespace Eigen;

// This program demonstrates how to use the Eigen geometry module

int main(int argc, char **argv) {

  // The Eigen/Geometry module provides a variety of rotation and translation representations
  // 3D rotation matrix directly using Matrix3d or Matrix3f
  Matrix3d rotation_matrix = Matrix3d::Identity();
  // The rotation vector uses AngleAxis, the underlying layer is not directly Matrix,
  // but the operation can be treated as a matrix (because the operator is overloaded)
  AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1)); // Rotate 45 degrees along the Z axis
  cout.precision(3);
  cout << "rotation matrix =\n" << rotation_vector.matrix() << endl; // convert to matrix with matrix()
  // can also be assigned directly
  rotation_matrix = rotation_vector.toRotationMatrix();
  // coordinate transformation with AngleAxis
  Vector3d v(1, 0, 0);
  Vector3d v_rotated = rotation_vector * v;
  cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
  // Or use a rotation matrix
  v_rotated = rotation_matrix * v;
  cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;

  // Euler angle: You can convert the rotation matrix directly into Euler angles
  Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX order, ie roll pitch yaw order
  cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

  // Euclidean transformation matrix using Eigen::Isometry
  Isometry3d T = Isometry3d::Identity(); // Although called 3d, it is essentially a 4∗4 matrix
  T.rotate(rotation_vector);             // Rotate according to rotation_vector
  T.pretranslate(Vector3d(1, 3, 4));     // Set the translation vector to (1,3,4)
  cout << "Transform matrix = \n" << T.matrix() << endl;

  // Use the transformation matrix for coordinate transformation
  Vector3d v_transformed = T * v; // Equivalent to R∗v+t
  cout << "v tranformed = " << v_transformed.transpose() << endl;

  // For affine and projective transformations, use Eigen::Affine3d and Eigen::Projective3d.

  // Quaternion
  // You can assign AngleAxis directly to quaternions, and vice versa
  Quaterniond q = Quaterniond(rotation_vector);
  cout << "quaternion from rotation vector = " << q.coeffs().transpose()
       << endl; // Note that the order of coeffs is (x, y, z, w), w is the real part, the first three are the imaginary part
  // can also assign a rotation matrix to it
  q = Quaterniond(rotation_matrix);
  cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
  // Rotate a vector with a quaternion and use overloaded multiplication
  v_rotated = q * v; // Note that the math is qvq^{−1}
  cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
  // expressed by regular vector multiplication, it should be calculated as follows
  cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

  return 0;
}


#include <iostream>
#include <cmath>
using namespace std;

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

/****************************
* This program demonstrates how to use Eigen geometry module
****************************/

int main ( int argc, char** argv )
{
    // Eigen/Geometry The module provides various representations of rotation and translation
    // 3D rotation matrix directly use Matrix3d or Matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // Rotation vector uses AngleAxis, its underlying layer is not directly Matrix,
    // but the operation can be treated as a matrix (because the operator is overloaded)
    //Rotate 45 degrees along the Z axis
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );
    cout .precision(3);
    //Use matrix() to convert to a matrix
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;
    // You can also assign directly
    rotation_matrix = rotation_vector.toRotationMatrix();
    // Coordinate transformation can be performed with AngleAxis
    Eigen::Vector3d v ( 1,0,0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    // Or use the rotation matrix
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    // Euler angle: The rotation matrix can be directly converted into Euler angle
    // ZYX order, that is, roll pitch yaw order
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 );
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    // Euclidean transformation matrix uses Eigen::Isometry
    // Although it is called 3d, it is essentially a 4*4 matrix
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();
    // Rotate according to rotation_vector
    T.rotate ( rotation_vector );
    // Set the translation vector to (1,3,4)
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );
    cout << "Transform matrix = \n" << T.matrix() <<endl;

    // Use transformation matrix for coordinate transformation
    // equivalent to R*v+t
    Eigen::Vector3d v_transformed = T*v;
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

    // For affine and projective transformations, use Eigen::Affine3d and Eigen::Projective3d.
    // Quaternion
    // You can directly assign AngleAxis to a quaternion, and vice versa
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    // Please note that the order of coeffs is (x,y,z,w), w is the real part, and
    // the first three are the imaginary part
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // You can also assign the rotation matrix to it
    q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // Use a quaternion to rotate a vector and use overloaded multiplication
    // Note that mathematically it is qvq^{-1}
    v_rotated = q*v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    return 0;
}


#include <iostream>
#include <armadillo>
#include <random>
#include "common.h"

using arma::mat;
using arma::svd;
using arma::vec;
using std::vector;

mat calculate_pseudoinverse(mat x)
{
    return x.t() * inv(x * x.t());
}

mat calculate_sum_signal(mat weigths, mat signals)
{
    return weigths * signals;
}

mat calculate_svd_inverse(mat x, int rank)
{
    // If rank = -1, use full rank (= smaller dimension of matrix x)
    if (rank == -1)
    {
        rank = std::min(x.n_cols, x.n_rows);
    }

    mat u, v;
    vec s;
    svd(u, s, v, x);
    mat s_inv = 1 / s;

    int lb = s_inv.n_elem - rank;
    int ub = s_inv.n_elem - 1;

    s_inv = s_inv.rows(lb, ub);
    v = v.cols(lb, ub);
    u = u.cols(lb, ub);
    mat sigma_inv = diagmat(s_inv);

    return v * sigma_inv * u.t();
}

mat low_rank_approximation(mat x, int rank)
{
    mat u, v;
    vec s;
    svd(u, s, v, x);

    s = s.rows(0, rank - 1);
    v = v.cols(0, rank - 1);
    u = u.cols(0, rank - 1);

    return u * diagmat(s) * v.t();
}

arma::mat trimmed_mean(arma::vec x, int n_points)
{
    int lb = n_points;
    int ub = x.n_elem - n_points - 1;
    arma::vec x_sorted = arma::sort(x, "ascend");
    return arma::mean(x_sorted.rows(lb, ub), 0);
}

arma::mat trimmed_mean(arma::vec x, float proportion)
{
    int n_points = round(proportion * x.n_elem);
    return trimmed_mean(x, n_points);
}

arma::mat rmse(arma::mat estimate_values, arma::mat true_values)
{
    arma::mat difference = estimate_values - true_values;
    return sqrt(sum(pow(difference, 2), 1) / difference.n_elem);
}

arma::mat mae(arma::mat estimate_values, arma::mat true_values)
{
    arma::mat difference = estimate_values - true_values;
    return arma::sum(arma::abs(difference), 1) / difference.n_elem;
}

arma::mat trimmed_mae(arma::vec estimate_values, arma::vec true_values, double rejection_threshold)
{
    arma::vec difference = estimate_values - true_values;
    arma::vec abs_difference = arma::abs(difference);
    int n_points = round(rejection_threshold * abs_difference.n_elem);
    arma::vec abs_difference_sorted = arma::sort(abs_difference, "descent");
    return arma::mean(abs_difference_sorted.rows(n_points, abs_difference_sorted.n_elem - 1), 0);
}

std::vector<int> sample_without_replacement(int lb, int ub, int n)
{
    std::vector<int> vec;
    for (size_t i = lb; i <= ub; i++)
    {
        vec.push_back(i);
    }
    std::random_device device;
    std::mt19937 generator(device());
    std::shuffle(vec.begin(), vec.end(), generator);
    std::vector<int> out(vec.begin(), vec.begin() + n);
    return out;
}


#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

/// This program demonstrates the basic usage of Sophus

int main(int argc, char **argv) {

  // Rotation matrix with 90 degrees along Z axis
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
  // or quaternion
  Quaterniond q(R);
  Sophus::SO3d SO3_R(R);              // Sophus::SO3d can be constructed from rotation matrix
  Sophus::SO3d SO3_q(q);              // or quaternion
  // they are equivalent of course
  cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
  cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;
  cout << "they are equal" << endl;

  // Use logarithmic map to get the Lie algebra
  Vector3d so3 = SO3_R.log();
  cout << "so3 = " << so3.transpose() << endl;
  // hat is from vector to skew−symmetric matrix
  cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << endl;
  // inversely from matrix to vector
  cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;

  // update by perturbation model
  Vector3d update_so3(1e-4, 0, 0); // this is a small update
  Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
  cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;

  cout << "*******************************" << endl;
  // Similar for SE(3)
  Vector3d t(1, 0, 0);           // translation 1 along X
  Sophus::SE3d SE3_Rt(R, t);     // construction SE3 from R,t
  Sophus::SE3d SE3_qt(q, t);     // or q,t
  cout << "SE3 from R,t= \n" << SE3_Rt.matrix() << endl;
  cout << "SE3 from q,t= \n" << SE3_qt.matrix() << endl;
  // Lie Algebra is 6d vector, we give a typedef
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  Vector6d se3 = SE3_Rt.log();
  cout << "se3 = " << se3.transpose() << endl;
  // The output shows Sophus puts the translation at first in se(3), then rotation.
  // Save as SO(3) wehave hat and vee
  cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;
  cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;

  // Finally the update
  Vector6d update_se3;
  update_se3.setZero();
  update_se3(0, 0) = 1e-4;
  Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
  cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;

  return 0;
}


#include <iostream>
#include <Eigen/Dense>
#include <vector>

template <typename FuncType, typename JacType>
void dampnewton(const FuncType &F, const JacType &DF,
                Eigen::VectorXd &x, double rtol = 1e-4,double atol = 1e-6)
{
    const int n = x.size();
    const double lmin = 1E-3; // Minimal damping factor
    double lambda = 1.0; // Initial and actual damping factor
    Eigen::VectorXd s(n),st(n); // Newton corrections
    Eigen::VectorXd xn(n);      // Tentative new iterate
    double sn,stn;    // Norms of Newton corrections

    do {
        auto jacfac = DF(x).lu(); // LU-factorize Jacobian

        s = jacfac.solve(F(x));   // Newton correction
        sn = s.norm();            // Norm of Newton correction
        lambda *= 2.0;
        do {
            lambda /= 2;
            if (lambda < lmin) throw "No convergence: lambda -> 0";
            xn = x-lambda*s;           // {\bf Tentative next iterate}
            st = jacfac.solve(F(xn));  // Simplified Newton correction
            stn = st.norm();
        }
        while (stn > (1-lambda/2)*sn); // {\bf Natural monotonicity test}
        x = xn; // Now: xn accepted as new iterate
        lambda = std::min(2.0*lambda,1.0); // Try to mitigate damping
    }
    // Termination based on simplified Newton correction
    while ((stn > rtol*x.norm()) && (stn > atol));
}

Nummerical Methods for CSE/PS14/solutions_ps14/dampnewton.hpp

Nummerical Methods for CSE/PS14/templates_ps14/dampnewton.hpp


#pragma once

#include <numeric>
#include <type_traits>
#include <utility>
#include <vector>

#include <opencv2/core/core.hpp>
#include <Eigen/Dense>

namespace pcv
{

template<class NumericType>
double getReprojectionError(
    const Eigen::Matrix3d &model,
    const std::pair<cv::Point_<NumericType>, cv::Point_<NumericType>>
        &correspondence)
{
    static_assert(std::is_arithmetic<NumericType>::value,
                  "Must have a numerical point type.");

    // Make the first correspondence a homogenous-coordinate column vector.
    Eigen::Vector3d homogenousFirst;
    homogenousFirst <<
        correspondence.first.x,
        correspondence.first.y,
        1.0;

    Eigen::Vector3d estimate = model * homogenousFirst;
    estimate /= estimate(2);

    return std::sqrt(  std::pow(estimate(0)-correspondence.second.x, 2)
                     + std::pow(estimate(1)-correspondence.second.y, 2));
}

template<class NumericType>
Eigen::Matrix3d
findHomographyWithLeastSquares(
    std::vector<std::pair<
        cv::Point_<NumericType>, cv::Point_<NumericType> >>
        correspondences)
{
    static_assert(std::is_arithmetic<NumericType>::value,
                  "Must have a numerical point type.");

    // Allocate appropriate amount of memory for Matrix.
    Eigen::MatrixXd joinedPoints(2*correspondences.size(), 9);

    for (std::size_t correspondenceIndex = 0; correspondenceIndex < correspondences.size(); ++correspondenceIndex)
    {
        const auto &pointPair = correspondences[correspondenceIndex];

        joinedPoints.row(2*correspondenceIndex) <<
            -pointPair.first.x,
            -pointPair.first.y,
            -1,
            0,
            0,
            0,
            pointPair.first.x*pointPair.second.x,
            pointPair.first.y*pointPair.second.x,
            pointPair.second.x;

        joinedPoints.row(2*correspondenceIndex+1) <<
            0,
            0,
            0,
            -pointPair.first.x,
            -pointPair.first.y,
            -1,
            pointPair.first.x*pointPair.second.y,
            pointPair.first.y*pointPair.second.y,
            pointPair.second.y;
    }

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(
        joinedPoints.transpose()*joinedPoints, Eigen::ComputeFullV);

    auto output =
        Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(svd.matrixV().col(8).data());

    return output / output(2, 2);
}

template<class NumericType>
Eigen::Matrix3d
findHomographyWithDirectLinearTransform(
    std::vector<std::pair<
        cv::Point_<NumericType>, cv::Point_<NumericType> >>
        correspondences)
{
    static_assert(std::is_arithmetic<NumericType>::value,
                  "Must have a numerical point type.");

    Eigen::Matrix<double, 8, 9> joinedPoints;

    std::array<Eigen::Matrix<double, 2, 9>, 4> points;

    std::transform(
        std::cbegin(correspondences),
        std::cend(correspondences),
        std::begin(points),
        [](std::pair<cv::Point2f, cv::Point2f> pointPair)
        {
            Eigen::Matrix<double, 2, 9> tmp;
            tmp <<
                -pointPair.first.x,
                -pointPair.first.y, -1, 0,
                0,
                0,
                pointPair.first.x*pointPair.second.x,
                pointPair.first.y*pointPair.second.x,
                pointPair.second.x,
                0,
                0,
                0,
                -pointPair.first.x,
                -pointPair.first.y,
                -1,
                pointPair.first.x*pointPair.second.y,
                pointPair.first.y*pointPair.second.y,
                pointPair.second.y;

            return tmp;
        });

    joinedPoints << points[0], points[1], points[2], points[3];

    Eigen::JacobiSVD<decltype(joinedPoints)> svd(joinedPoints, Eigen::ComputeFullV);

    auto output =
        Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(svd.matrixV().col(8).data());

    return output / output(2, 2);
}

} // end namespace pcv


#include <iostream>

//#define USING_MLOGD
#include "utility/tool.h"
#include <Eigen/Core>
//#include <Eigen/Geometry>  // Eigen 几何模块

/**
 * @brief       使用Gramy-Schmidt方法实现向量正交化,并单位化
 * @param  [in]  a       向量a
 * @param  [in]  b       向量b
 * @param  [in]  c       向量c
 * @param  [out]  An      标准化后的向量A
 * @param  [out]  Bn      标准化后的向量B
 * @param  [out]  Cn      标准化后的向量C
 * @retval
 * @note  a,b,c必须为线性无关组
 */
static void schmidtOrthogonalV3D(
                        Eigen::Vector3d a,
                        Eigen::Vector3d b,
                        Eigen::Vector3d c,
                        Eigen::Vector3d* An,
                        Eigen::Vector3d* Bn,
                        Eigen::Vector3d* Cn ){

    Eigen::Vector3d A,B,C;
    //A直接赋值为a
    A = a;
    //MLOGD("vector A: %.3lf %.3f %.3f", A.x(), A.y(), A.z() );
    //求B
    double xab = A.dot(b) / A.dot( A );
    B = b - xab * A;
    //MLOGD("vector B: %.3lf %.3f %.3f", B.x(), B.y(), B.z() );
    //求C
    double xac = A.dot( c ) / A.dot( A );
    double xbc = B.dot( c ) / B.dot( B );
    C = c - ( xac * A ) - ( xbc * B );
    //MLOGD("vector C: %.3lf %.3f %.3f", C.x(), C.y(), C.z() );
    //单位化并输出
    *An = A.normalized();
    *Bn = B.normalized();
    *Cn = C.normalized();
    //MLOGD("normalized vector A: %.3lf %.3f %.3f", An->x(), An->y(), An->z() );
    //MLOGD("normalized vector B: %.3lf %.3f %.3f", Bn->x(), Bn->y(), Bn->z() );
    //MLOGD("normalized vector C: %.3lf %.3f %.3f", Cn->x(), Cn->y(), Cn->z() );
}

//--------------------测试函数----------------------------
static void schmidtOrthogonalTest( void ) {
    //线性无关组1
    Eigen::Vector3d a(1,1.2,0);
    Eigen::Vector3d b(1,2,0);
    Eigen::Vector3d c(0,1,1);

    //线性无关组2
    Eigen::Vector3d v1(9,1.2,2.4);
    Eigen::Vector3d v2(1,2,6.7);
    Eigen::Vector3d v3(5,1.5,1);

    Eigen::Vector3d A,B,C;

    schmidtOrthogonalV3D( a, b, c, &A, &B, &C );
    //schmidtOrthogonalV3D( v1, v2, v3, &A, &B, &C );
    TPLOGI("normalized vector A: %.3lf %.3f %.3f", A.x(), A.y(), A.z() );
    TPLOGI("normalized vector B: %.3lf %.3f %.3f", B.x(), B.y(), B.z() );
    TPLOGI("normalized vector C: %.3lf %.3f %.3f", C.x(), C.y(), C.z() );
}


#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

int main()
{
  MatrixXd m = MatrixXd::Random(3,3);
  m = (m + MatrixXd::Constant(3,3,1.2)) * 50;
  cout << "m =" << endl << m << endl;
  VectorXd v(3);
  v << 1, 2, 3;
  cout << "m * v =" << endl << m * v << endl;
}


#include <armadillo>
#include <iostream>
using namespace arma;
int main()
{
    // arma::mat A(5, 5, arma::fill::randu);
    // arma::mat B(5, 5, arma::fill::randn);
    // std::cout << A * B << std::endl;
    mat X = randu<mat>(5,5);
    mat Y = X.t()*X;
    mat R1 = chol(Y);
    std::cout << R1 << std::endl;
}


/*
 * main.cpp
 *
 *  Created on: 2021/07/22
 *      Author: matsu
 */
#include <Eigen/Dense>
#include <cstdio>
#include <iostream>
#include <string>
#include <vector>

using namespace Eigen;
using namespace std;

void test_intro1();
void test_intro2_1();
void test_intro2_2();

int main( int argc, char** argv )
{
    vector< string > args( argv, argv + argc );

    if ( args.at( 1 ) == string( "intro1" ) )
    {
        test_intro1();
    }
    else if ( args.at( 1 ) == string( "intro2_1" ) )
    {
        test_intro2_1();
    }
    else if ( args.at( 1 ) == string( "intro2_2" ) )
    {
        test_intro2_2();
    }
    else
    {
        fprintf( stderr, "Matching test is not exist.\n" );
        exit( -1 );
    }

    return 0;
}

void test_intro1()
{
    MatrixXd m( 2, 2 );
    m( 0, 0 ) = 3;
    m( 1, 0 ) = 2.5;
    m( 0, 1 ) = -1;
    m( 1, 1 ) = m( 1, 0 ) + m( 0, 1 );
    std::cout << m << std::endl;
}

void test_intro2_1()
{
    MatrixXd m = MatrixXd::Random( 3, 3 );
    m = ( m + MatrixXd::Constant( 3, 3, 1.2 ) ) * 50;
    cout << "m =" << endl
         << m << endl;
    VectorXd v( 3 );
    v << 1, 2, 3;
    cout << "m * v =" << endl
         << m * v << endl;
}

void test_intro2_2()
{
    Matrix3d m = Matrix3d::Random();
    m = ( m + Matrix3d::Constant( 1.2 ) ) * 50;
    cout << "m =" << endl
         << m << endl;
    Vector3d v( 1, 2, 3 );

    cout << "m * v =" << endl
         << m * v << endl;
}


#include <Eigen/Dense>
#include <iostream>

using namespace std;
using namespace Eigen;

void rankoneinvit(const VectorXd & d, const double & tol, double & lmin) {
	MatrixXd M;
	VectorXd ev = d;
	lmin = 0.;
	double lnew = d.cwiseAbs().minCoeff();
	while (abs(lnew-lmin) > tol*lmin){
		lmin = lnew;
		M = d.asDiagonal(); M+=ev*ev.transpose();
		ev = M.lu().solve(ev);
		ev.normalize();
		lnew = ev.transpose()*M* ev;
		}
	lmin=lnew;
}

int main(){
    double tol=1e-3;
    double lmin;
    int n=10;

    VectorXd d=VectorXd::Random(n);
    rankoneinvit(d,tol,lmin);
    cout<<"lmin = "<<lmin<<endl;

    //Compare runtimes
    unsigned int repeats = 3;

    for(unsigned int p = 2; p <= 9; p++) {
        unsigned int n = pow(2,p);

        for(unsigned int r = 0; r < repeats; ++r) {

//             d = VectorXd::Random(n);
            d = VectorXd::LinSpaced(n,1,2);
            rankoneinvit(d,tol,lmin);

            }

    }
return 0;}

Nummerical Methods for CSE/PS3/problem1_a.cpp


#include <iostream>
#include <Eigen/Dense>
#include "timer.h"

void rankoneinvit(const Eigen::VectorXd & d, const double & tol, double & lmin){
	lmin=0.;
	Eigen::VectorXd ev;
	int n=d.size();
	Eigen::MatrixXd M;
	ev=d;
	double lnew = d.cwiseAbs().minCoeff();
	while(std::abs(lnew-lmin)> tol*lmin){
		lmin=lnew;
		M=d.asDiagonal();
		M+=ev*ev.transpose();
		ev= M.partialPivLu().solve(ev);
		ev.normalize();
		lnew= ev.transpose()*M*ev;
	}
	lmin=lnew;
}

using namespace std;
using namespace Eigen;
int main(){
    srand((unsigned int) time(0));
    double tol=1e-3;
    double lmin;
    int n=10;

    // Check correctedness of the fast version
    VectorXd d=VectorXd::Random(n);
    rankoneinvit(d,tol,lmin);
    cout<<"lmin = "<<lmin<<endl;
    //rankoneinvit_fast(d,tol,lmin);
    //cout<<"lmin = "<<lmin<<endl;

    //Compare runtimes
    unsigned int repeats = 3;
    /*
    for(unsigned int p = 2; p <= 9; p++) {
        tm_slow.reset();
        tm_fast.reset();
        unsigned int n = pow(2,p);

        for(unsigned int r = 0; r < repeats; ++r) {

//             d = VectorXd::Random(n);
            d = VectorXd::LinSpaced(n,1,2);
            rankoneinvit(d,tol,lmin);

            rankoneinvit_fast(d,tol,lmin);
        }

        cout << "The slow method took:    " <<  tm_slow.min().count() / 1000000. << " ms for n = " <<n<<endl;
        cout << "The fast method took:    " <<  tm_fast.min().count() / 1000000. << " ms for n = " <<n<<endl;
    }*/
}

Nummerical Methods for CSE/PS3/rankoneinvit.cpp


/**
 * @brief Partial Least Squares Regression with Eigen
 *
 * Same source as LDA, \cite friedman2001elements
 *
 * @file pls-eigen.hpp
 * @author François-David Collin <Francois-David.Collin@umontpellier.fr>
 * @brief
 * @version 0.1
 * @date 2018-11-08
 *
 * @copyright Copyright (c) 2018
 *
 */
#pragma once

#include "various.hpp"
#include <Eigen/Dense>
#include <list>
#include <algorithm>
#include <range/v3/all.hpp>

using namespace Eigen;
using namespace std;
using namespace ranges;

template<class Derived, class OtherDerived>
VectorXd pls(const MatrixBase<Derived>& x,
         const MatrixBase<OtherDerived>& y,
         size_t ncomp,
         MatrixXd& Projection,
         RowVectorXd& mean,
         RowVectorXd& std,
         bool stopping = false)
{
    auto n = x.rows();
    auto p = x.cols();
    mean = x.colwise().mean();
    std = ((x.rowwise() - mean).array().square().colwise().sum() / (x.rows() - 1)).sqrt();;
    MatrixXd X = (x.rowwise() - mean).array().rowwise() / std.array();
    MatrixXd Xo = X;
    MatrixXd XX = X.transpose() * X;
    MatrixXd Y = MatrixXd::Zero(n, ncomp + 1);
    MatrixXd P(p,ncomp);
    MatrixXd R(p,ncomp);
    VectorXd XY(n);
    VectorXd w(p);
    VectorXd r(p);
    VectorXd t(p);
    VectorXd res(ncomp);
    size_t window_size = std::max(2_z,ncomp/10_z);

    std::list<unsigned char> stopping_criterium(window_size,0);
    // Y.col(0) = y.rowwise() - y.colwise().mean();
    double ymean = y.mean();
    Y.col(0).array() = ymean;
    double SSTO = (y.array() - ymean).array().square().sum();
    int m = 0;
    // for (auto m = 0; m < ncomp; m++)
    while (m < ncomp)
    {
        XY = X.transpose() * y;
        SelfAdjointEigenSolver<MatrixXd> es( XY.transpose() * XY );
        auto abs_eigenvalues = es.eigenvalues().array().abs();
        size_t max_eigenvalue_indice = ranges::distance(abs_eigenvalues.begin(),ranges::max_element(abs_eigenvalues));
        auto q = es.eigenvectors().col(max_eigenvalue_indice);
        w = XY * q;
        w /= sqrt((w.transpose()*w)(0,0));
        r=w;
        for (auto j=0; j<=m-1;j++)
        {
            r -= (P.col(j).transpose()*w)(0,0)*R.col(j);
        }
        R.col(m) = r;
        t = Xo * r;
        P.col(m) = XX *r/(t.transpose()*t)(0,0);
        VectorXd Zm = X * XY;
        double Znorm = Zm.array().square().sum();
        double Thetam = Zm.dot(y) / Znorm;
        Y.col(m + 1) = Y.col(m) + Thetam * Zm;
        X -= Zm.rowwise().replicate(p) * ((Zm/Znorm).transpose() * X).asDiagonal();
        res(m) = (Y.col(m + 1).array() - ymean).array().square().sum() / SSTO;
        if ((m >= 2) && stopping) {
            auto lastdiff = res(m) - res(m-1);
            auto lastmean = (res(m) + res(m-1))/2.0;
            size_t remains = ncomp - m;
            stopping_criterium.pop_front();
            stopping_criterium.push_back(lastmean >= 0.99 * remains * lastdiff);
            auto wcrit = ranges::accumulate(stopping_criterium,0);
            if (wcrit == window_size) break;
        }
        m++;
    }
    if (m < ncomp) {
        m--;
        res = res(seq(0,m)).eval();
    }
    Projection = R;
//    VectorXd res = (Y.block(0,1,n,ncomp).array() - ymean).array().square().colwise().sum() / SSTO;
    return res;
}


#pragma once
#include "dirichlet_boundary.hpp"
#include "load_vector_assembly.hpp"
#include "stiffness_matrix_assembly.hpp"
#include <Eigen/Core>
#include <igl/readSTL.h>
#include <igl/slice.h>
#include <igl/slice_into.h>
#include <string>

typedef Eigen::VectorXd Vector;

//----------------solveBegin----------------
//! Solve the FEM system.
//!
//! @param[out] u will at the end contain the FEM solution.
//! @param[in] vertices list of triangle vertices for the mesh
//! @param[in] triangles list of triangles (described by indices)
//! @param[in] f the RHS f (as in the exercise)
//! @param[in] g the boundary value (as in the exercise)
//! @param[in] sigma the function sigma as in the exercise
//! @param[in] r the parameter r from the lecture notes
//! return number of degrees of freedom (without the boundary dofs)
int solveFiniteElement(Vector &               u,
                       const Eigen::MatrixXd &vertices,
                       const Eigen::MatrixXi &triangles,
                       const std::function<double(double, double)> &f,
                       const std::function<double(double, double)> &sigma,
                       const std::function<double(double, double)> &g,
                       double r) {
	SparseMatrix A;
	// (write your solution here)
	assembleStiffnessMatrix(A, vertices, triangles, sigma, r);

	Vector F;
	// (write your solution here)
	assembleLoadVector(F, vertices, triangles, f);

	u.resize(vertices.rows());
	u.setZero();
	Eigen::VectorXi interiorVertexIndices;

	// set Dirichlet Boundary conditions
	// (write your solution here)
	setDirichletBoundary(u, interiorVertexIndices, vertices, triangles, g);
	F -= A * u;

	SparseMatrix AInterior;

	igl::slice(A, interiorVertexIndices, interiorVertexIndices, AInterior);
	Eigen::SimplicialLDLT<SparseMatrix> solver;

	Vector FInterior;

	igl::slice(F, interiorVertexIndices, FInterior);

	//initialize solver for AInterior
	// (write your solution here)
	//// NPDE_START_TEMPLATE
	solver.compute(AInterior);

	if (solver.info() != Eigen::Success) {
		throw std::runtime_error("Could not decompose the matrix");
	}
	//// NPDE_END_TEMPLATE

	//solve interior system
	// (write your solution here)
	//// NPDE_START_TEMPLATE
	Vector uInterior = solver.solve(FInterior);
	igl::slice_into(uInterior, interiorVertexIndices, u);
	//// NPDE_END_TEMPLATE

	return interiorVertexIndices.size();
}
//----------------solveEnd----------------


#include <Eigen/Core>
#include <iostream>
#include <LBFGS.h>

using Eigen::VectorXf;
using Eigen::MatrixXf;
using namespace LBFGSpp;

class Rosenbrock
{
private:
    int n;
public:
    Rosenbrock(int n_) : n(n_) {}
    float operator()(const VectorXf& x, VectorXf& grad)
    {
        float fx = 0.0;
        for(int i = 0; i < n; i += 2)
        {
            float t1 = 1.0 - x[i];
            float t2 = 10 * (x[i + 1] - x[i] * x[i]);
            grad[i + 1] = 20 * t2;
            grad[i]     = -2.0 * (x[i] * grad[i + 1] + t1);
            fx += t1 * t1 + t2 * t2;
        }
        return fx;
    }
};

int main()
{
    const int n = 10;
    LBFGSParam<float> param;
    LBFGSSolver<float> solver(param);
    Rosenbrock fun(n);

    VectorXf x = VectorXf::Zero(n);
    float fx;
    int niter = solver.minimize(fun, x, fx);

    std::cout << niter << " iterations" << std::endl;
    std::cout << "x = \n" << x.transpose() << std::endl;
    std::cout << "f(x) = " << fx << std::endl;

    return 0;
}


// https://stackoverflow.com/questions/21412169/creating-a-rotation-matrix-with-pitch-yaw-roll-using-eigen
#include <Eigen/Dense>
#include <iostream>
#include <math.h>

Eigen::Matrix3d rotation_from_euler(double roll, double pitch, double yaw)
{
    // roll and pitch and yaw in radians
    double su = sin(roll);
    double cu = cos(roll);
    double sv = sin(pitch);
    double cv = cos(pitch);
    double sw = sin(yaw);
    double cw = cos(yaw);
    Eigen::Matrix3d Rot_matrix(3, 3);
    Rot_matrix(0, 0) = cv * cw;
    Rot_matrix(0, 1) = su * sv * cw - cu * sw;
    Rot_matrix(0, 2) = su * sw + cu * sv * cw;
    Rot_matrix(1, 0) = cv * sw;
    Rot_matrix(1, 1) = cu * cw + su * sv * sw;
    Rot_matrix(1, 2) = cu * sv * sw - su * cw;
    Rot_matrix(2, 0) = -sv;
    Rot_matrix(2, 1) = su * cv;
    Rot_matrix(2, 2) = cu * cv;
    return Rot_matrix;
}

int main()
{
    Eigen::Matrix3d rot_mat = rotation_from_euler(0, 0, 0.5 * M_PI);
    std::cout << rot_mat << std::endl;
    return 0;
}


#include "sphere.hpp"

#include "intersection_info.hpp"

#include <math/point3d.hpp>
#include <math/ray3d.hpp>
#include <math/utility.hpp>
#include <math/vector3d.hpp>

#include <boost/optional.hpp>
#include <boost/none.hpp>

#include <cmath>

namespace shapes
{

// Check for ray-sphere intersection using geometric test.
//
// Note:
//  If ray origin is on the surface of the sphere and ray direction points
//  to the outside, no intersection is found.
bool intersects(shapes::sphere shape, math::ray3d ray)
{
  const auto L = math::make_vector3d(ray.origin, shape.center);
  const auto tPX = math::dot(L, ray.direction);

  const auto orig_center_sqrd_dist = math::squared_length(L);
  const auto squared_radius = shape.radius * shape.radius;

  // Ray origin is inside sphere.
  if (orig_center_sqrd_dist < squared_radius) { return true; }

  // Sphere center is behind ray origin.
  if (tPX <= 0.0f) { return false; }

  auto dsq = orig_center_sqrd_dist - tPX * tPX;
  return dsq <= squared_radius;
}

// Returns the closest intersection point between ray and sphere.
//
// Note:
//  If ray origin is on the surface of the sphere and ray direction points
//  to the outside, no intersection is found.
boost::optional<shapes::intersection_info> closest_intersection(shapes::sphere shape, math::ray3d ray)
{
  const auto L = math::make_vector3d(ray.origin, shape.center);
  const auto tPX = math::dot(L, ray.direction);

  const auto orig_center_sqrd_dist = math::squared_length(L);
  const auto squared_radius = shape.radius * shape.radius;
  const auto origin_inside_sphere = orig_center_sqrd_dist < squared_radius;

  // Sphere center is behind ray origin.
  if (tPX <= 0.0f && origin_inside_sphere == false) { return boost::none; }

  const auto dsq = orig_center_sqrd_dist - tPX * tPX;
  if (dsq > squared_radius) { return boost::none; }

  float t;
  if (tPX > 0.0f && origin_inside_sphere == false)
  {
    t = tPX - std::sqrt(squared_radius - dsq);
  }
  else
  {
    t = tPX + std::sqrt(squared_radius - dsq);
  }

  shapes::intersection_info info;
  info.point = math::translated(ray.origin, math::scaled(ray.direction, t));
  info.normal = math::normalized(math::make_vector3d(shape.center, info.point));
  return info;
}

}


#include <iostream>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

// Generic functor
template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{
    // Information that tells the caller the numeric type (eg. double) and size (input / output dim)
    typedef _Scalar Scalar;
    enum {
        InputsAtCompileTime = NX,
        ValuesAtCompileTime = NY
};
// Tell the caller the matrix sizes associated with the input, output, and jacobian
typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

// Local copy of the number of inputs
int m_inputs, m_values;

// Two constructors:
Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

// Get methods for users to determine function input and output dimensions
int inputs() const { return m_inputs; }
int values() const { return m_values; }

};

///////////////////////// Levenberg Marquardt Example (1) f(x) = a x + b ///////////////////////////////

typedef std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > Point2DVector;

Point2DVector GeneratePoints();

struct SimpleLineFunctor : Functor<double>
{
    int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const
    {
        // "a" in the model is z(0), and "b" is z(1)
        double x_i,y_i,a,b;
        for(unsigned int i = 0; i < this->Points.size(); ++i)
        {
            y_i=this->Points[i](1);
            x_i=this->Points[i](0);
            a=z(0);
            b=z(1);
            fvec(i) =y_i-(a*x_i +b);
        }

        return 0;
    }

  Point2DVector Points;

  int inputs() const { return 2; } // There are two parameters of the model
  int values() const { return this->Points.size(); } // The number of observations
};

struct SimpleLineFunctorNumericalDiff : Eigen::NumericalDiff<SimpleLineFunctor> {};

Point2DVector GeneratePoints(const unsigned int numberOfPoints)
{
    Point2DVector points;
    // Model y = 2*x + 5 with some noise (meaning that the resulting minimization should be about (2,5)
    for(unsigned int i = 0; i < numberOfPoints; ++i)
    {
        double x = static_cast<double>(i);
        Eigen::Vector2d point;
        point(0) = x;
        point(1) = 2.0 * x + 5.0 + drand48()/10.0;
        points.push_back(point);
    }

  return points;
}

void testSimpleLineFunctor()
{
    std::cout << "Testing f(x) = a x + b function..." << std::endl;

    unsigned int numberOfPoints = 50;
    Point2DVector points = GeneratePoints(numberOfPoints);

    Eigen::VectorXd x(2);
    x.fill(2.0f);

    SimpleLineFunctorNumericalDiff functor;
    functor.Points = points;
    Eigen::LevenbergMarquardt<SimpleLineFunctorNumericalDiff> lm(functor);

    Eigen::LevenbergMarquardtSpace::Status status = lm.minimize(x);
    std::cout << "status: " << status << std::endl;
    std::cout << "x that minimizes the function: " << std::endl << x << std::endl;
    std::cout <<"lm.parameters.epsfcn: " <<lm.parameters.epsfcn << std::endl;
    std::cout <<"lm.parameters.factor: " <<lm.parameters.factor << std::endl;
    std::cout <<"lm.parameters.ftol: " << lm.parameters.ftol << std::endl;
    std::cout <<"lm.parameters.gtol: " <<lm.parameters.gtol << std::endl;
    std::cout <<"lm.parameters.maxfev: " <<lm.parameters.maxfev << std::endl;
    std::cout <<"lm.parameters.xtol: " <<lm.parameters.xtol << std::endl;

    std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;

}

///////////////////////// Levenberg Marquardt Example (2) f(x) = ax² + bx + c ///////////////////////////////

// Ref: https://medium.com/@sarvagya.vaish/levenberg-marquardt-optimization-part-2-5a71f7db27a0

struct QuadraticFunctor
{
    // Compute 'm' errors, one for each data point, for the given parameter values in 'x'
    int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
    {
        // 'x' has dimensions n x 1
        // It contains the current estimates for the parameters.

        // 'fvec' has dimensions m x 1
        // It will contain the error for each data point.

        double aParam = x(0);
        double bParam = x(1);
        double cParam = x(2);

        for (int i = 0; i < values(); i++)
        {
            double xValue = measuredValues(i, 0);
            double yValue = measuredValues(i, 1);

            fvec(i) = yValue - (aParam * xValue * xValue + bParam * xValue + cParam);
        }
    }

    // Compute the jacobian of the errors
    int df(const Eigen::VectorXd &x, Eigen::MatrixXd &fjac) const
    {
        // 'x' has dimensions n x 1
        // It contains the current estimates for the parameters.

        // 'fjac' has dimensions m x n
        // It will contain the jacobian of the errors, calculated numerically in this case.

        float epsilon;
        epsilon = 1e-5f;

        for (int i = 0; i < x.size(); i++)
        {
            Eigen::VectorXd xPlus(x);
            xPlus(i) += epsilon;
            Eigen::VectorXd xMinus(x);
            xMinus(i) -= epsilon;

            Eigen::VectorXd fvecPlus(values());
            operator()(xPlus, fvecPlus);

            Eigen::VectorXd fvecMinus(values());
            operator()(xMinus, fvecMinus);

            Eigen::VectorXd fvecDiff(values());
            fvecDiff = (fvecPlus - fvecMinus) / (2.0f * epsilon);

            fjac.block(0, i, values(), 1) = fvecDiff;
        }
    }

    // Number of data points, i.e. values.
    int m;

    // Returns 'm', the number of values.
    int values() const { return m; }

    // The number of parameters, i.e. inputs.
    int n;

    // Returns 'n', the number of inputs.
    int inputs() const { return n; }

    Eigen::MatrixXd measuredValues;
};

void quadraticPointsGenerator(std::vector<double> &x_values, std::vector<double> &y_values,double a=-1, double b=0.6, double c=2, unsigned int numberOfPoints=50 )
{
    //point are x_start=-4, x_end=3
    double x_start=-4;
    double x_end=3;
    double x,y;

    for(unsigned int i = 0; i < numberOfPoints; ++i)
    {
        x = x_start+ static_cast<double>(i)* ( x_end- x_start)/numberOfPoints;
        y = a*pow(x,2)+b*x+c + drand48()/10.0;
        x_values.push_back(x);
        y_values.push_back(y);
    }

}

void testQuadraticFunctor()
{
    std::cout << "Testing the f(x) = ax² + bx + c function..." << std::endl;
    std::vector<double> x_values;
    std::vector<double> y_values;

    double a=-1;
    double b=0.6;
    double c=2;

    quadraticPointsGenerator(x_values, y_values,a,b,c);

    // 'm' is the number of data points.
    int m = x_values.size();

    // Move the data into an Eigen Matrix.
    // The first column has the input values, x. The second column is the f(x) values.
    Eigen::MatrixXd measuredValues(m, 2);
    for (int i = 0; i < m; i++) {
        measuredValues(i, 0) = x_values[i];
        measuredValues(i, 1) = y_values[i];
    }

    // 'n' is the number of parameters in the function.
    // f(x) = a(x^2) + b(x) + c has 3 parameters: a, b, c
    int n = 3;

    // 'parameters' is vector of length 'n' containing the initial values for the parameters.
    // The parameters 'x' are also referred to as the 'inputs' in the context of LM optimization.
    // The LM optimization inputs should not be confused with the x input values.
    Eigen::VectorXd parameters(n);
    parameters(0) = 0.0;             // initial value for 'a'
    parameters(1) = 0.0;             // initial value for 'b'
    parameters(2) = 0.0;             // initial value for 'c'

    //
    // Run the LM optimization
    // Create a LevenbergMarquardt object and pass it the functor.
    //

    QuadraticFunctor functor;
    functor.measuredValues = measuredValues;
    functor.m = m;
    functor.n = n;

    Eigen::LevenbergMarquardt<QuadraticFunctor, double> lm(functor);
    int status = lm.minimize(parameters);
    std::cout << "LM optimization status: " << status << std::endl;

    //
    // Results
    // The 'x' vector also contains the results of the optimization.
    //
    std::cout << "Optimization results" << std::endl;
    std::cout << "\ta: " << parameters(0) << std::endl;
    std::cout << "\tb: " << parameters(1) << std::endl;
    std::cout << "\tc: " << parameters(2) << std::endl;

    std::cout << "Actual values are" << std::endl;
    std::cout << "\ta: " << a << std::endl;
    std::cout << "\tb: " << b << std::endl;
    std::cout << "\tc: " << c << std::endl;
    std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;

}

///////////////////////// Levenberg Marquardt Example (3) f(x,y) = (x + 2*y -7)^2 + (2*x + y - 5)^2 ///////////////////////////////

// https://en.wikipedia.org/wiki/Test_functions_for_optimization
// Booth Function
// Implement f(x,y) = (x + 2*y -7)^2 + (2*x + y - 5)^2
struct BoothFunctor : Functor<double>
{
    // Simple constructor
    BoothFunctor(): Functor<double>(2,2) {}

    // Implementation of the objective function
    int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const
    {
        double x = z(0);   double y = z(1);
        /*
        * Evaluate the Booth function.
        * Important: LevenbergMarquardt is designed to work with objective functions that are a sum
        * of squared terms. The algorithm takes this into account: do not do it yourself.
        * In other words: objFun = sum(fvec(i)^2)
        */
        fvec(0) = x + 2*y - 7;
        fvec(1) = 2*x + y - 5;
        return 0;
    }
};

void testBoothFunctor() {
    std::cout << "Testing the f(x,y) = (x + 2*y -7)^2 + (2*x + y - 5)^2 function..." << std::endl;
    Eigen::VectorXd zInit(2); zInit << 1.87, 2.032;
    std::cout << "zInit: " << zInit.transpose() << std::endl;
    Eigen::VectorXd zSoln(2); zSoln << 1.0, 3.0;
    std::cout << "zSoln: " << zSoln.transpose() << std::endl;

    BoothFunctor functor;
    Eigen::NumericalDiff<BoothFunctor> numDiff(functor);
    Eigen::LevenbergMarquardt<Eigen::NumericalDiff<BoothFunctor>,double> lm(numDiff);
    lm.parameters.maxfev = 1000;
    lm.parameters.xtol = 1.0e-10;
    std::cout << "max fun eval: " << lm.parameters.maxfev << std::endl;
    std::cout << "x tol: " << lm.parameters.xtol << std::endl;

    Eigen::VectorXd z = zInit;
    int ret = lm.minimize(z);
    std::cout << "iter count: " << lm.iter << std::endl;
    std::cout << "return status: " << ret << std::endl;
    std::cout << "zSolver: " << z.transpose() << std::endl;
    std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
}

///////////////////////// Levenberg Marquardt Example (4) f(x,y) = (x^2 + y - 11)^2 + (x + y^2 - 7)^2 ///////////////////////////////

// https://en.wikipedia.org/wiki/Test_functions_for_optimization
// Himmelblau's Function
// Implement f(x,y) = (x^2 + y - 11)^2 + (x + y^2 - 7)^2
struct HimmelblauFunctor : Functor<double>
{
    // Simple constructor
    HimmelblauFunctor(): Functor<double>(2,2) {}

    // Implementation of the objective function
    int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const
    {
        double x = z(0);   double y = z(1);
        /*
        * Evaluate Himmelblau's function.
        * Important: LevenbergMarquardt is designed to work with objective functions that are a sum
        * of squared terms. The algorithm takes this into account: do not do it yourself.
        * In other words: objFun = sum(fvec(i)^2)
        */
        fvec(0) = x * x + y - 11;
        fvec(1) = x + y * y - 7;
        return 0;
    }
};

void testHimmelblauFunctor()
{
    std::cout << "Testing f(x,y) = (x^2 + y - 11)^2 + (x + y^2 - 7)^2 function..." << std::endl;
    // Eigen::VectorXd zInit(2); zInit << 0.0, 0.0;  // soln 1
    // Eigen::VectorXd zInit(2); zInit << -1, 1;  // soln 2
    // Eigen::VectorXd zInit(2); zInit << -1, -1;  // soln 3
    Eigen::VectorXd zInit(2); zInit << 1, -1;  // soln 4
    std::cout << "zInit: " << zInit.transpose() << std::endl;
    std::cout << "soln 1: [3.0, 2.0]" << std::endl;
    std::cout << "soln 2: [-2.805118, 3.131312]" << std::endl;
    std::cout << "soln 3: [-3.77931, -3.28316]" << std::endl;
    std::cout << "soln 4: [3.584428, -1.848126]" << std::endl;

    HimmelblauFunctor functor;
    Eigen::NumericalDiff<HimmelblauFunctor> numDiff(functor);
    Eigen::LevenbergMarquardt<Eigen::NumericalDiff<HimmelblauFunctor>,double> lm(numDiff);
    lm.parameters.maxfev = 1000;
    lm.parameters.xtol = 1.0e-10;
    std::cout << "max fun eval: " << lm.parameters.maxfev << std::endl;
    std::cout << "x tol: " << lm.parameters.xtol << std::endl;

    Eigen::VectorXd z = zInit;
    int ret = lm.minimize(z);
    std::cout << "iter count: " << lm.iter << std::endl;
    std::cout << "return status: " << ret << std::endl;
    std::cout << "zSolver: " << z.transpose() << std::endl;
    std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
}

int main()
{
    testSimpleLineFunctor();
    testQuadraticFunctor();
    testBoothFunctor();
    testHimmelblauFunctor();
}

/*

Refs
https://github.com/daviddoria/Examples/blob/master/c%2B%2B/Eigen/LevenbergMarquardt/CurveFitting.cpp
https://stackoverflow.com/questions/18509228/how-to-use-the-eigen-unsupported-levenberg-marquardt-implementation
https://stackoverflow.com/questions/48213584/understanding-levenberg-marquardt-enumeration-returns
https://www.ultimatepp.org/reference$Eigen_demo$en-us.html
https://ethz-adrl.github.io/ct/ct_doc/doc/html/core_tut_linearization.html
https://robotics.stackexchange.com/questions/20673/why-with-the-pseudo-inverse-it-is-possible-to-invert-the-jacobian-matrix-even-in
http://users.ics.forth.gr/~lourakis/
https://mathoverflow.net/questions/257699/gauss-newton-vs-gradient-descent-vs-levenberg-marquadt-for-least-squared-method
https://math.stackexchange.com/questions/1085436/gauss-newton-versus-gradient-descent
https://stackoverflow.com/questions/34701160/how-to-set-levenberg-marquardt-damping-using-eigen
http://www.netlib.org/minpack/lmder.f
https://en.wikipedia.org/wiki/Test_functions_for_optimization
https://github.com/daviddoria/Examples/blob/master/c%2B%2B/Eigen/LevenbergMarquardt/NumericalDerivative.cpp
https://stackoverflow.com/questions/18509228/how-to-use-the-eigen-unsupported-levenberg-marquardt-implementation
*/


#include <Eigen/Core>
#include <Eigen/LU>
#include <vector>
#include <iostream>
#include "writer.hpp"
#include <assert.h>

/// Uses the explicit Euler method to compute y from time 0 to time T
/// where y is a 2x1 vector solving the linear system of ODEs as in the exercise
///
/// @param[out] yT at the end of the call, this will have vNext
/// @param[in] y0 the initial conditions
/// @param[in] zeta the damping factor (see exercise)
/// @param[in] h the step size
/// @param[in] T the final time at which to compute the solution.
///
/// The first component of y (the position) will be stored to y1, the second component (the velocity) to y2. The i-th entry of y1 (resp. y2) will contain the first (resp. second) component of y at time i*h.
///

//----------------explicitEulerBegin----------------
void explicitEuler(std::vector<double> & y1, std::vector<double> &y2, std::vector<double> & time,
	const Eigen::Vector2d& y0,
		   double zeta, double h, double T) {

	y1.push_back(y0(0));
	y2.push_back(y0(1));
	time.push_back(0);

	for(int k = 0; k < (T / h); k++) {
		double y1_k = y1.back();
		double y2_k = y2.back();

		//y1[k + 1] = y1_k + h * y2_k;
		//y2[k + 1] = y2_k + h * (-y1_k - (2 * zeta * y2_k));

		//time[k + 1] = time[k] + h;

		y1.push_back(y1_k + h * y2_k);
		y2.push_back(y2_k + h * (-y1_k - (2 * zeta * y2_k)));

		time.push_back(time.back() + h);
	}
}
//----------------explicitEulerEnd----------------

// Implements the implicit Euler. Analogous to explicit Euler, same input and output parameters
//----------------implicitEulerBegin----------------
void implicitEuler(std::vector<double> & y1, std::vector<double> & y2, std::vector<double> & time,
	const Eigen::Vector2d& y0,
		   double zeta, double h, double T) {

	int nSteps = T / h;
	//y1.resize(nSteps + 1);
	//y2.resize(nSteps + 1);
	//time.resize(nSteps + 1);

	y1[0] = y0(0);
	y2[0] = y0(1);
	time[0] = 0;

	Eigen::MatrixXd A(2, 2);
	A << 1, -h,
			 h, (1 + 2*zeta*h);

	Eigen::FullPivLU<Eigen::MatrixXd> lu = A.fullPivLu();

	for(int k = 0; k < nSteps + 1; k++) {
		Eigen::Vector2d y_prev = { y1[k], y2[k] };
		Eigen::VectorXd y_next = lu.solve(y_prev);
		y1[k + 1] = y_next(0);
		y2[k + 1] = y_next(1);
		time[k + 1] = time[k] + h;
	}
}
//----------------implicitEulerEnd----------------

//----------------energyBegin----------------
// Energy computation given the velocity. Assume the energy vector to be already initialized with the correct size.
void Energy(const std::vector<double> & v, std::vector<double> & energy)
{
  assert(v.size()==energy.size());

	for(int i = 0; i < v.size(); i++) {
		energy[i] = 0.5 * v[i] * v[i];
	}
}
//----------------energyEnd----------------

int main() {

	double T = 20.0;
	double h = 0.5; // Change this for explicit / implicit time stepping comparison
	const Eigen::Vector2d y0(1,0);
	double zeta=0.2;
	std::vector<double> y1;
	std::vector<double> y2;
	std::vector<double> time;
	explicitEuler(y1,y2,time,y0,zeta,h,T);
	writeToFile("position_expl.txt", y1);
	writeToFile("velocity_expl.txt",y2);
	writeToFile("time_expl.txt",time);
	std::vector<double> energy(y2.size());
	Energy(y2,energy);
	writeToFile("energy_expl.txt",energy);
	y1.assign(y1.size(),0);
	y2.assign(y2.size(),0);
	time.assign(time.size(),0);
	energy.assign(energy.size(),0);
	implicitEuler(y1,y2,time,y0,zeta,h,T);
	writeToFile("position_impl.txt", y1);
	writeToFile("velocity_impl.txt",y2);
	writeToFile("time_impl.txt",time);
	Energy(y2,energy);
	writeToFile("energy_impl.txt",energy);

	return 0;
}