lslboost::numeric::ublas::vector<T> solve(
const lslboost::numeric::ublas::matrix<T>& A_,
const lslboost::numeric::ublas::vector<T>& b_)
{
//BOOST_ASSERT(A_.size() == b_.size());
lslboost::numeric::ublas::matrix<T> A(A_);
lslboost::numeric::ublas::vector<T> b(b_);
lslboost::numeric::ublas::permutation_matrix<> piv(b.size());
lu_factorize(A, piv);
lu_substitute(A, piv, b);
//
// iterate to reduce error:
//
lslboost::numeric::ublas::vector<T> delta(b.size());
for(unsigned i = 0; i < 1; ++i)
{
noalias(delta) = prod(A_, b);
delta -= b_;
lu_substitute(A, piv, delta);
b -= delta;
T max_error = 0;
for(unsigned i = 0; i < delta.size(); ++i)
{
T err = fabs(delta[i] / b[i]);
if(err > max_error)
max_error = err;
}
//std::cout << "Max change in LU error correction: " << max_error << std::endl;
}
return b;
}
ParameterEstimatorImpl::Parameters ParameterEstimatorImpl::solve(const ublas::matrix<double>& A , const ublas::vector<double>& y) const
{
// solve Ax = y
ublas::matrix<double> A_factorized = A;
ublas::permutation_matrix<size_t> pm(e_.parameterCount());
int singular = lu_factorize(A_factorized, pm);
if (singular)
throw runtime_error("[ParameterEstimatorImpl::solve()] A is singular.");
ublas::vector<double> result(y);
lu_substitute(A_factorized, pm, result);
return result;
}
vector_type solve(const matrix_type& A,
const vector_type& y)
{
namespace ublas = boost::numeric::ublas;
matrix_type A_factorized = A;
ublas::permutation_matrix<size_t> pm(y.size());
int singular = lu_factorize(A_factorized, pm);
if (singular) throw std::runtime_error("[LinearSolver<LU>::solve()] A is singular.");
vector_type result(y);
lu_substitute(A_factorized, pm, result);
return result;
}
bool LS::Regression::InvertMatrix (const ublas::matrix<double>& input, ublas::matrix<double>& inverse)
{
typedef ublas::permutation_matrix<std::size_t> pmatrix;
// create a working copy of the input
ublas::matrix<double> A(input);
// create a permutation matrix for the LU-factorization
pmatrix pm(A.size1());
// perform LU-factorization
int res = lu_factorize(A,pm);
if( res != 0 )
return false;
// create identity matrix of "inverse"
inverse.assign(ublas::identity_matrix<double>(A.size1()));
// backsubstitute to get the inverse
lu_substitute(A, pm, inverse);
return true;
}
void Energy::train()
{
try{
cout<<"A:\n"<<A<<endl;
cout<<"b:\n"<<b<<endl;
/*** linear least squares ***/
la::matrix<double> AT = la::trans(A);
la::matrix<double> ATA = la::prod(AT,A);
la::vector<double> ATb = la::prod(AT,b);
la::permutation_matrix<int> pm(ATA.size1());
la::lu_factorize(ATA,pm);
la::vector<double> c(ATb);
lu_substitute(ATA, pm, c);
la::vector<double> est = la::prod(A,c);
/*** set coeffficients ***/
set_coefficients(c);
/* cout<<"A:\n"<<print_matrix(A)<<endl;
cout<<"b:\n"<<print_matrix(b)<<endl;
cout<<"AT:\n"<<print_matrix(AT)<<endl;
cout<<"ATA:\n"<<print_matrix(ATA)<<endl;
cout<<"ATb:\n"<<print_matrix(ATb)<<endl;
cout<<"ATA - after:\n"<<print_matrix(ATA)<<endl;
cout<<"c:\n"<<print_matrix(c)<<endl;
cout<<"est:\n"<<print_matrix(est)<<endl;
*/
cout<<"TRAINING DONE"<<endl;
}
catch(...)
{
cout<<"WARNING: Training fail, will stil write matices to octave file"<<endl;
}
cout<<"Done writting to octave file"<<endl;
}
void MSC::train()
{
/*** linear least squares ***/
try{
la::matrix<double> AT = la::trans(A);
la::matrix<double> ATA = la::prod(AT,A);
la::vector<double> ATb = la::prod(AT,b);
la::permutation_matrix<int> pm(ATA.size1());
la::lu_factorize(ATA,pm);
la::vector<double> c(ATb);
lu_substitute(ATA, pm, c);
la::vector<double> est = la::prod(A,c);
/*** set coeffficients ***/
set_coefficients(c);
cout<<"A:\n"<<print_matrix(A)<<endl;
cout<<"b:\n"<<print_matrix(b)<<endl;
cout<<"AT:\n"<<print_matrix(AT)<<endl;
cout<<"ATA:\n"<<print_matrix(ATA)<<endl;
cout<<"ATb:\n"<<print_matrix(ATb)<<endl;
cout<<"ATA - after:\n"<<print_matrix(ATA)<<endl;
cout<<"c:\n"<<print_matrix(c)<<endl;
cout<<"est:\n"<<print_matrix(est)<<endl;
cout<<"TRAINING DONE"<<endl;
}
catch(...)
{
cout<<"WARNING: Training failed! Writting matrices to octave file\n";
}
return;
}
ThinPlateSpline(std::vector<boost::array<WorkingType, PosDim> > positions,
std::vector<boost::array<WorkingType, ValDim> > values)
{
boost::numeric::ublas::matrix<WorkingType> L;
refPositions = positions;
const int numPoints((int)refPositions.size());
const int WaLength(numPoints + PosDim + 1);
L = boost::numeric::ublas::matrix<WorkingType> (WaLength, WaLength);
// Calculate K and store in L
for(int i(0); i < numPoints; i++)
{
L(i,i) = 0.0;
// K is symmetrical so no point in calculating things twice
int j(i + 1);
for(; j < numPoints; ++j)
{
L(i,j) = L(j,i) = radialbasis<WorkingType, WorkingType, PosDim>(refPositions[i], refPositions[j]);
}
// construct P and store in K
L(j,i) = L(i,j) = 1.0;
++j;
for(int posElm(0); j < WaLength; ++posElm, ++j)
L(j,i) = L(i,j) = positions[i][posElm];
}
// O
for(int i(numPoints); i < WaLength; i++)
for(int j(numPoints); j < WaLength; j++)
L(i,j) = 0.0;
// Solve L^-1 Y = W^T
typedef boost::numeric::ublas::permutation_matrix<std::size_t> pmatrix;
boost::numeric::ublas::matrix<WorkingType> A(L);
pmatrix pm(A.size1());
int res = (int)lu_factorize(A, pm);
if(res != 0)
{
;//TODO catch this error
}
boost::numeric::ublas::matrix<WorkingType> invL(boost::numeric::ublas::identity_matrix<WorkingType>(A.size1()));
lu_substitute(A, pm, invL);
Wa = boost::numeric::ublas::matrix<WorkingType>(WaLength, ValDim );
boost::numeric::ublas::matrix<WorkingType> Y(WaLength, ValDim);
int i(0);
for(; i < numPoints; i++)
for(int j(0); j < ValDim; ++j)
Y(i, j) = values[i][j];
for(; i < WaLength; i++)
for(int j(0); j < ValDim; ++j)
Y(i, j) = 0.0;
Wa = prod(invL,Y);
}
template<typename MATRIX> MATRIX expm_pad(const MATRIX &H, typename type_traits<typename MATRIX::value_type>::real_type t = 1.0, const int p = 6){
typedef typename MATRIX::value_type value_type;
typedef typename MATRIX::size_type size_type;
typedef typename type_traits<value_type>::real_type real_value_type;
assert(H.size1() == H.size2());
assert(p >= 1);
const size_type n = H.size1();
const identity_matrix<value_type> I(n);
matrix<value_type> U(n,n),H2(n,n),P(n,n),Q(n,n);
real_value_type norm = 0.0;
// Calcuate Pade coefficients
vector<real_value_type> c(p+1);
c(0)=1;
for(size_type i = 0; i < (size_type) p; ++i)
c(i+1) = c(i) * ((p - i)/((i + 1.0) * (2.0 * p - i)));
// Calcuate the infinty norm of H, which is defined as the largest row sum of a matrix
for(size_type i=0; i<n; ++i) {
real_value_type temp = 0.0;
for(size_type j = 0; j < n; j++)
temp += std::abs(H(i, j));
norm = t * std::max<real_value_type>(norm, temp);
}
// If norm = 0, and all H elements are not NaN or infinity but zero,
// then U should be identity.
if (norm == 0.0) {
bool all_H_are_zero = true;
for(size_type i = 0; i < n; i++)
for(size_type j = 0; j < n; j++)
if( H(i,j) != value_type(0.0) )
all_H_are_zero = false;
if( all_H_are_zero == true ) return I;
// Some error happens, H has elements which are NaN or infinity.
std::cerr<<"Null input error in the template expm_pad.\n";
// std::cout << "Null INPUT : " << H <<"\n";
exit(0);
}
// Scaling, seek s such that || H*2^(-s) || < 1/2, and set scale = 2^(-s)
int s = 0;
real_value_type scale = 1.0;
if(norm > 0.5) {
s = std::max<int>(0, static_cast<int>((log(norm) / log(2.0) + 2.0)));
scale /= real_value_type(std::pow(2.0, s));
U.assign((scale * t) * H); // Here U is used as temp value due to that H is const
}
else
U.assign(H);
// Horner evaluation of the irreducible fraction, see the following ref above.
// Initialise P (numerator) and Q (denominator)
H2.assign( prod(U, U) );
Q.assign( c(p)*I );
P.assign( c(p-1)*I );
size_type odd = 1;
for( size_type k = p - 1; k > 0; --k) {
( odd == 1 ) ?
( Q = ( prod(Q, H2) + c(k-1) * I ) ) :
( P = ( prod(P, H2) + c(k-1) * I ) ) ;
odd = 1 - odd;
}
( odd == 1 ) ? ( Q = prod(Q, U) ) : ( P = prod(P, U) );
Q -= P;
// In origine expokit package, they use lapack ZGESV to obtain inverse matrix,
// and in that ZGESV routine, it uses LU decomposition for obtaing inverse matrix.
// Since in ublas, there is no matrix inversion template, I simply use the build-in
// LU decompostion package in ublas, and back substitute by myself.
// Implement Matrix Inversion
permutation_matrix<size_type> pm(n);
int res = lu_factorize(Q, pm);
if( res != 0) {
std::cerr << "Matrix inversion error in the template expm_pad.\n";
exit(0);
}
// H2 is not needed anymore, so it is temporary used as identity matrix for substituting.
H2.assign(I);
lu_substitute(Q, pm, H2);
(odd == 1) ?
( U.assign( -(I + real_value_type(2.0) * prod(H2, P))) ):
( U.assign( I + real_value_type(2.0) * prod(H2, P) ) );
// Squaring
for(size_type i = 0; i < (size_type) s; ++i)
U = (prod(U,U));
return U;
}
