#include <boost/numeric/ublas/matrix.hpp>

#include <boost/numeric/ublas/io.hpp>

#include <boost/numeric/ublas/lu.hpp>

#include <exception>

namespace ublas = boost::numeric::ublas;

template<typename T>

static bool invertMatrix(const ublas::matrix<T> &inputMatrix,

ublas::matrix<T> &outputInverseMatrix)

{

    using namespace boost::numeric::ublas;

    typedef permutation_matrix<std::size_t> pmatrix;

    try {

        matrix<T> A(inputMatrix);

        pmatrix pm(A.size1());

        int res = lu_factorize(A, pm);

        if (res != 0) {

            return false;

        }

        outputInverseMatrix.assign(ublas::identity_matrix<T>(A.size1()));

        lu_substitute(A, pm, outputInverseMatrix);

    } catch (std::exception &e) {

        std::cout<<"invertMatrix exception: "<<e.what()<<std::endl;

        return false;

    }

    return true;

}

/*

    ublas::matrix<double> h(2, 1);

    ublas::matrix<double> G(2, 2);

    ublas::matrix<double> Gt(2, 2);

    ublas::matrix<double> Delta(2, 1);

    ublas::matrix<double> T(2,2);

    ublas::matrix<double> T_inverse(2,2);

    ublas::matrix<double>  K(2,2);

    Gt = ublas::trans(G);

    T = ublas::prod(Gt,G); //Gt*G

    invertMatrix(T, T_inverse);/Gt*G inverse

    K = ublas::prod(T_inverse, Gt);//(Gt*G)-1 * Gt

    Delta = ublas::prod(K, h);//(Gt*G)-1 * Gt *h

*/

#include <iostream>

#include <Eigen/Dense>

#include <exception>

using namespace Eigen;

/*

    MatrixXd  h(2, 1);

    MatrixXd  G(2, 2);

    MatrixXd  Gt(2, 2);

    MatrixXd  Delta(2, 1);

    MatrixXd  T(2,2);

    MatrixXd  T_inverse(2,2);

    MatrixXd   K(2,2);

    Gt = G.transpose();

    T = Gt*G; //Gt*G

    T_inverse = T.inverse();

    K = T_inverse*Gt;//(Gt*G)-1 * Gt

    Delta = K*h;//(Gt*G)-1 * Gt *h

*/