#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 <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

*/