/// \brief Copy constructor.
///
/// We need an explicit copy constructor, because otherwise the
/// default copy constructor would override the generic matrix
/// view "copy constructor" below.
Matrix (const Matrix& in) :
nrows_ (in.nrows()),
ncols_ (in.ncols()),
A_ (verified_alloc_size (in.nrows(), in.ncols()))
{
if (! in.empty())
copy_matrix (nrows(), ncols(), get(), lda(), in.get(), in.lda());
}
void WPCA(const Matrix<double>& data,
const Vector<double>& A,
Vector<double>& mu,
Matrix<double>& trans,
double var/*=0.0*/)
{
int d_out = 0;
bool use_variance = (var>0);
int d_in = data.nrows();
int N = data.ncols();
if(mu.size() != d_in)
mu.reallocate(d_in);
if(use_variance and var>1)
throw MatVecError("WPCA : var must be between 0 and 1\n");
if(!use_variance){
d_out = trans.nrows();
}
//compute weighted mean
for(int i=0;i<d_in;i++)
{
mu(i)=0.0;
for(int j=0;j<N;j++)
mu(i) += A(j) * data(i,j);
}
mu /= A.SumElements();
//M is a centered and weighted version of data
Matrix<double> M(d_in,N);
for(int i=0;i<N;i++){
M.col(i) = sqrt(A(i))*(data.col(i)-mu);
}
//Find trans with an SVD
SVD M_SVD(M,true,true); //overwrite M
if(use_variance){
//compute d_out from SVD
double total=0;
double Sum = M_SVD.S.SumElements();
d_out = 0;
while(total<Sum){
total+=M_SVD.S(d_out)/Sum;
d_out++;
}
}
if(trans.ncols() != d_in || trans.nrows() != d_out)
trans.reallocate(d_out,d_in);
for(size_t i=0;i<d_out;i++){
trans.row(i) = M_SVD.U.col(i);
}
}
bool CFeasibilityMap::SolveLP(Matrix &A, ColumnVector &b) {
lprec *lp ;
int n_row = A.nrows(); int n_col = A.ncols();
lp = make_lp(0,n_col) ;
double *input_row = new double[1+n_col];
for (int i_row=1; i_row<=n_row; i_row++){
input_row[0] = 0 ;
for (int j=1; j<=n_col; j++){
input_row[j] = A(i_row,j) ;
}
add_constraint(lp, input_row, LE, b(i_row)) ;
}
delete [] input_row;
double *input_obj = new double[1+n_col]; // The first zero is for matrix form
input_obj[0] = 0 ;
for (int j=1; j<=n_col; j++){
input_obj[j] = 1 ;
}
set_obj_fn(lp, input_obj) ;
delete [] input_obj;
set_verbose(lp, IMPORTANT); // NEUTRAL (0), IMPORTANT (3), NORMAL (4), FULL (6)
bool is_feasible = (solve(lp)==0); // 0: feasible solution found, 2: not found
// solution for minimizing objective function
delete_lp(lp);
return is_feasible;
}
// Matrix A's first n columns are orthonormal
// so A.Columns(1,n).t() * A.Columns(1,n) is the identity matrix.
// Fill out the remaining columns of A to make them orthonormal
// so A.t() * A is the identity matrix
void extend_orthonormal(Matrix& A, int n)
{
REPORT
Tracer et("extend_orthonormal");
int nr = A.nrows(); int nc = A.ncols();
if (nc > nr) Throw(IncompatibleDimensionsException(A));
if (n > nc) Throw(IncompatibleDimensionsException(A));
ColumnVector SSR;
{ Matrix A1 = A.Columns(1,n); SSR = A1.sum_square_rows(); }
for (int i = n; i < nc; ++i)
{
// pick row with smallest SSQ
int k; SSR.minimum1(k);
// orthogonalise column with 1 at element k, 0 elsewhere
// next line is rather inefficient
ColumnVector X = - A.Columns(1, i) * A.SubMatrix(k, k, 1, i).t();
X(k) += 1.0;
// normalise
X /= sqrt(X.SumSquare());
// update row sums of squares
for (k = 1; k <= nr; ++k) SSR(k) += square(X(k));
// load new column into matrix
A.Column(i+1) = X;
}
}
void PCA(const Matrix<double>& data,
Vector<double>& mu,
Matrix<double>& trans,
double var/*=0.0*/)
{
Vector<double> A(data.ncols(),1.0);
WPCA(data,A,mu,trans,var);
}
bool q_rigidaffine_compute_affinematrix_3D(const vector<Point3D64f> &vec_A,const vector<Point3D64f> &vec_B,Matrix &x4x4_affinematrix)
{
if(vec_A.size()<4 || vec_A.size()!=vec_B.size())
{
fprintf(stderr,"ERROR: Invalid input parameters! \n");
return false;
}
if(x4x4_affinematrix.nrows()!=4 || x4x4_affinematrix.ncols()!=4)
{
x4x4_affinematrix.ReSize(4,4);
}
vector<Point3D64f> vec_A_norm,vec_B_norm;
Matrix x4x4_normalize_A(4,4),x4x4_normalize_B(4,4);
vec_A_norm=vec_A; vec_B_norm=vec_B;
q_normalize_points_3D(vec_A,vec_A_norm,x4x4_normalize_A);
q_normalize_points_3D(vec_B,vec_B_norm,x4x4_normalize_B);
int n_point=vec_A.size();
Matrix A(3*n_point,13);
int row=1;
for(int i=0;i<n_point;i++)
{
A(row,1)=vec_A_norm[i].x; A(row,2)=vec_A_norm[i].y; A(row,3)=vec_A_norm[i].z; A(row,4)=1.0;
A(row,5)=0.0; A(row,6)=0.0; A(row,7)=0.0; A(row,8)=0.0;
A(row,9)=0.0; A(row,10)=0.0; A(row,11)=0.0; A(row,12)=0.0;
A(row,13)=-vec_B_norm[i].x;
A(row+1,1)=0.0; A(row+1,2)=0.0; A(row+1,3)=0.0; A(row+1,4)=0.0;
A(row+1,5)=vec_A_norm[i].x; A(row+1,6)=vec_A_norm[i].y; A(row+1,7)=vec_A_norm[i].z; A(row+1,8)=1.0;
A(row+1,9)=0.0; A(row+1,10)=0.0; A(row+1,11)=0.0; A(row+1,12)=0.0;
A(row+1,13)=-vec_B_norm[i].y;
A(row+2,1)=0.0; A(row+2,2)=0.0; A(row+2,3)=0.0; A(row+2,4)=0.0;
A(row+2,5)=0.0; A(row+2,6)=0.0; A(row+2,7)=0.0; A(row+2,8)=0.0;
A(row+2,9)=vec_A_norm[i].x; A(row+2,10)=vec_A_norm[i].y;A(row+2,11)=vec_A_norm[i].z;A(row+2,12)=1.0;
A(row+2,13)=-vec_B_norm[i].z;
row+=3;
}
DiagonalMatrix D(13);
Matrix U(3*n_point,13),V(13,13);
SVD(A,D,U,V);
Matrix h=V.column(13);
for(int i=1;i<=13;i++)
h(i,1) /= h(13,1);
x4x4_affinematrix(1,1)=h(1,1); x4x4_affinematrix(1,2)=h(2,1); x4x4_affinematrix(1,3)=h(3,1); x4x4_affinematrix(1,4)=h(4,1);
x4x4_affinematrix(2,1)=h(5,1); x4x4_affinematrix(2,2)=h(6,1); x4x4_affinematrix(2,3)=h(7,1); x4x4_affinematrix(2,4)=h(8,1);
x4x4_affinematrix(3,1)=h(9,1); x4x4_affinematrix(3,2)=h(10,1); x4x4_affinematrix(3,3)=h(11,1); x4x4_affinematrix(3,4)=h(12,1);
x4x4_affinematrix(4,1)=0.0; x4x4_affinematrix(4,2)=0.0; x4x4_affinematrix(4,3)=0.0; x4x4_affinematrix(4,4)=1.0;
x4x4_affinematrix=x4x4_normalize_B.i()*x4x4_affinematrix*x4x4_normalize_A;
return true;
}
// Print out a matrix, row by row, to a given precision
void Logger::print(const Matrix& m, int digits) const
{
// Print out row by row
Vector temp(m.ncols());
for (int i = 0; i < m.nrows(); i++){
temp = m.rowAsVector(i);
print (temp, digits, false);
}
}
double SMACOF::calculateSigma( const Matrix<double>& Weights, const Matrix<double>& Distances, const Matrix<double>& InitialZ, Matrix<double>& dists ) {
unsigned M = Distances.nrows(); double sigma=0;
for(unsigned i=1; i<M; ++i) {
for(unsigned j=0; j<i; ++j) {
double dlow=0; for(unsigned k=0; k<InitialZ.ncols(); ++k) { double tmp=InitialZ(i,k) - InitialZ(j,k); dlow+=tmp*tmp; }
dists(i,j)=dists(j,i)=sqrt(dlow); double tmp3 = Distances(i,j) - dists(i,j);
sigma += Weights(i,j)*tmp3*tmp3;
}
}
return sigma;
}
void SMACOF::run( const Matrix<double>& Weights, const Matrix<double>& Distances, const double& tol, const unsigned& maxloops, Matrix<double>& InitialZ ) {
unsigned M = Distances.nrows();
// Calculate V
Matrix<double> V(M,M); double totalWeight=0.;
for(unsigned i=0; i<M; ++i) {
for(unsigned j=0; j<M; ++j) {
if(i==j) continue;
V(i,j)=-Weights(i,j);
if( j<i ) totalWeight+=Weights(i,j);
}
for(unsigned j=0; j<M; ++j) {
if(i==j)continue;
V(i,i)-=V(i,j);
}
}
// And pseudo invert V
Matrix<double> mypseudo(M, M); pseudoInvert(V, mypseudo);
Matrix<double> dists( M, M ); double myfirstsig = calculateSigma( Weights, Distances, InitialZ, dists ) / totalWeight;
// initial sigma is made up of the original distances minus the distances between the projections all squared.
Matrix<double> temp( M, M ), BZ( M, M ), newZ( M, InitialZ.ncols() );
for(unsigned n=0; n<maxloops; ++n) {
if(n==maxloops-1) plumed_merror("ran out of steps in SMACOF algorithm");
// Recompute BZ matrix
for(unsigned i=0; i<M; ++i) {
for(unsigned j=0; j<M; ++j) {
if(i==j) continue; //skips over the diagonal elements
if( dists(i,j)>0 ) BZ(i,j) = -Weights(i,j)*Distances(i,j) / dists(i,j);
else BZ(i,j)=0.;
}
//the diagonal elements are -off diagonal elements BZ(i,i)-=BZ(i,j) (Equation 8.25)
BZ(i,i)=0; //create the space memory for the diagonal elements which are scalars
for(unsigned j=0; j<M; ++j) {
if(i==j) continue;
BZ(i,i)-=BZ(i,j);
}
}
mult( mypseudo, BZ, temp); mult(temp, InitialZ, newZ);
//Compute new sigma
double newsig = calculateSigma( Weights, Distances, newZ, dists ) / totalWeight;
//Computing whether the algorithm has converged (has the mass of the potato changed
//when we put it back in the oven!)
if( fabs( newsig - myfirstsig )<tol ) break;
myfirstsig=newsig;
InitialZ = newZ;
}
}
ReturnMatrix Helmert_transpose(const Matrix& Y, bool full)
{
REPORT
Tracer et("Helmert_transpose * Matrix ");
int m = Y.nrows(); int n = Y.ncols(); if (!full) ++m;
Matrix X(m, n);
for (int j = 1; j <= n; ++j)
{
ColumnVector CV = Y.Column(j);
X.Column(j) = Helmert_transpose(CV, full);
}
X.release(); return X.for_return();
}
BSpline::BSpline(const Matrix mat)
{
int N, j, i;
RowVector first(2), last(2);
// specify default parameters in case the matrix
// passed as argument is incorrect
_d = 3;
_k = 0;
_done = false;
draw_init();
// check that the matrix has two rows
if (mat.ncols() != 2) {
cerr << "BSpline::BSpline(): Error: Knot matrix must have two columns" << endl
<< "BSpline::BSpline(): Creating empty B-spline" << endl;
return;
}
// check that the number of interior knot intervals is a power of two
N = mat.nrows();
j = (int) ceil(log((double)N-2*_d-1)/log(2.0));
if ((N-2*_d-1) != (1<<j)) {
cerr << "BSpline::BSpline(): Error: Incorrect number of interior knots" << endl
<< "BSpline::BSpline(): Creating empty B-spline" << endl;
return;
}
// check that the first and last d+1 columns are identical
first = mat.Row(1);
last = mat.Row(N);
for (i=1; i<_d; i++) {
if ((first(1) != mat(i+1,1)) ||
(first(2) != mat(i+1,2)) ||
(last(1) != mat(N-i,1)) ||
(last(2) != mat(N-i,2))) {
cerr << "BSpline::BSpline(): Error: Matrix is not an endpoint-interpolating bspline"
<< endl << "BSpline::BSpline(): Creating empty B-spline" << endl;
return;
}
}
// matrix is in correct format, so create the knots
for (i=1; i<=N; i++)
add_knot((int)rint(mat(i,1)), (int)rint(mat(i,2)));
// update the internal bspline parameters
_j = j;
_done = true;
update_matrix();
}
double cross_validation(Matrix<double> x, Vector<double> y, Kernel *k, double c, int cross = -1)
{
// 交差検定
int group = cross;
if (group <= 0) group = 1 + log2(x.nrows());
int size = x.nrows() / group;
double accuracy = 0.0;
for (int i = 0; i < group; i++) {
size_t si = i * size, sz = size + (i == group - 1 ? (x.nrows() % size) : 0);
Matrix<double> samplex(x.nrows() - sz, x.ncols()), testx(sz, x.ncols());
Vector<double> sampley(y.size() - sz), testy(sz);
for (size_t j = 0; j < x.nrows(); j++) {
if (j < si) {
samplex.setRow(j, x.extractRow(j));
sampley[j] = y[j];
}
else if (j < si + sz) {
testx.setRow(j - si, x.extractRow(j));
testy[j - si] = y[j];
}
else {
samplex.setRow(j - sz, x.extractRow(j));
sampley[j - sz] = y[j];
}
}
SVM svm(samplex, sampley, k, c);
int pass = 0;
for (size_t j = 0; j < sz; j++)
if (svm.discriminant(testx.extractRow(j)) * testy[j] > 0.0) pass++;
double acc = pass / (double)sz * 100.0;
printf("loop #%d :: %f\n", i, acc);
accuracy += acc / group;
}
printf("total accuracy :: %f\n", accuracy);
return accuracy;
}
ReturnMatrix Helmert(const Matrix& X, bool full)
{
REPORT
Tracer et("Helmert * Matrix");
int m = X.nrows(); int n = X.ncols();
if (m == 0) Throw(ProgramException("Matrix has 0 rows ", X));
Matrix Y;
if (full) Y.resize(m,n); else Y.resize(m-1, n);
for (int j = 1; j <= n; ++j)
{
ColumnVector CV = X.Column(j);
Y.Column(j) = Helmert(CV, full);
}
Y.release(); return Y.for_return();
}
bool CFeasibilityMap::SolveLP(Matrix &A, ColumnVector &b, ColumnVector &x) {
lprec *lp ;
int n_row = A.nrows(); int n_col = A.ncols();
x = ColumnVector(n_col); x = 0;
lp = make_lp(0,n_col) ;
double *input_row = new double[1+n_col];
for (int i_row=1; i_row<=n_row; i_row++){
input_row[0] = 0 ; // The first zero is for matrix form
for (int j=1; j<=n_col; j++){
input_row[j] = A(i_row,j) ;
}
add_constraint(lp, input_row, LE, b(i_row)) ;
}
delete [] input_row;
double *input_obj = new double[1+n_col]; // The first zero is for matrix form
input_obj[0] = 0 ;
for (int j=1; j<=n_col; j++){
input_obj[j] = 1 ;
}
set_obj_fn(lp, input_obj) ;
delete [] input_obj;
set_verbose(lp, IMPORTANT); // NEUTRAL (0), IMPORTANT (3), NORMAL (4), FULL (6)
bool is_feasible = (solve(lp)==0); // 0: feasible solution found, 2: not found
// solution for minimizing objective function
double* x_min = new double[n_col];
double* x_max = new double[n_col];
if (is_feasible) {
get_variables(lp, x_min);
set_maxim(lp);
is_feasible = (solve(lp)==0); // 0: feasible solution found, 2: not found
if (is_feasible) {
get_variables(lp, x_max);
for (int i = 0; i < n_col; i++) {
x(i+1) = (x_min[i] + x_max[i]) / 2.0;
}
}
}
delete [] x_min;
delete [] x_max;
delete_lp(lp);
return is_feasible;
}
bool q_rigidaffine_compute_rigidmatrix_3D(const vector<Point3D64f> &vec_A,const vector<Point3D64f> &vec_B,Matrix &x4x4_rigidmatrix)
{
if(vec_A.size()<4 || vec_A.size()!=vec_B.size())
{
fprintf(stderr,"ERROR: Invalid input parameters! \n");
return false;
}
if(x4x4_rigidmatrix.nrows()!=4 || x4x4_rigidmatrix.ncols()!=4)
{
x4x4_rigidmatrix.ReSize(4,4);
}
int n_point=vec_A.size();
vector<Point3D64f> vec_A_norm,vec_B_norm;
Matrix x4x4_normalize_A(4,4),x4x4_normalize_B(4,4);
vec_A_norm=vec_A; vec_B_norm=vec_B;
q_normalize_points_3D(vec_A,vec_A_norm,x4x4_normalize_A);
q_normalize_points_3D(vec_B,vec_B_norm,x4x4_normalize_B);
Matrix x3xn_A(3,n_point),x3xn_B(3,n_point);
for(long i=0;i<n_point;i++)
{
x3xn_A(1,i+1)=vec_A_norm[i].x; x3xn_A(2,i+1)=vec_A_norm[i].y; x3xn_A(3,i+1)=vec_A_norm[i].z;
x3xn_B(1,i+1)=vec_B_norm[i].x; x3xn_B(2,i+1)=vec_B_norm[i].y; x3xn_B(3,i+1)=vec_B_norm[i].z;
}
DiagonalMatrix D;
Matrix U,V;
SVD(x3xn_A*x3xn_B.t(),D,U,V);
Matrix R=V*U.t();
x4x4_rigidmatrix(1,1)=R(1,1); x4x4_rigidmatrix(1,2)=R(1,2); x4x4_rigidmatrix(1,3)=R(1,3); x4x4_rigidmatrix(1,4)=0.0;
x4x4_rigidmatrix(2,1)=R(2,1); x4x4_rigidmatrix(2,2)=R(2,2); x4x4_rigidmatrix(2,3)=R(2,3); x4x4_rigidmatrix(2,4)=0.0;
x4x4_rigidmatrix(3,1)=R(3,1); x4x4_rigidmatrix(3,2)=R(3,2); x4x4_rigidmatrix(3,3)=R(3,3); x4x4_rigidmatrix(3,4)=0.0;
x4x4_rigidmatrix(4,1)=0.0; x4x4_rigidmatrix(4,2)=0.0; x4x4_rigidmatrix(4,3)=0.0; x4x4_rigidmatrix(4,4)=1.0;
x4x4_rigidmatrix=x4x4_normalize_B.i()*x4x4_rigidmatrix*x4x4_normalize_A;
return true;
}
void print_matrix(const char* name, const Matrix<double>& A, int n, int m)
{
std::ostringstream s;
std::string t;
if (n == -1)
n = A.nrows();
if (m == -1)
m = A.ncols();
s << name << ": " << std::endl;
for (int i = 0; i < n; i++)
{
s << " ";
for (int j = 0; j < m; j++)
s << A[i][j] << ", ";
s << std::endl;
}
t = s.str();
t = t.substr(0, t.size() - 3); // To remove the trailing space, comma and newline
std::cout << t << std::endl;
}
int SparseEigenSolver(
CSR3_sym_matrix<double, std::int32_t>& A,
CSR3_sym_matrix<double, std::int32_t>& B,
Matrix<double, M_PROP::GE, M_SHAPE::RE, M_ORD::COL>& Z,
Vector<double>& L,
Vector<double>& eigen_error,
double eigenv_max,
int toll_exp,
int max_iter,
std::ostream& log)
{
typedef MML3_INT_TYPE int_t;
int len=100;
char buf[100];
//MKLGetVersionString(buf, len);
mkl_get_version_string(buf, len);
log << "\tSolutore modale per matrici sparse basato su:\n"
<< "\tMKL-FEAST: " << std::string(buf,len);
log << "\n\tCaratteristiche del sistema:"
<< "\n\t\tN : " << A.nrows()
<< "\n\t\tNNZ(A) : " << A.nonzeros()
<< "\n\t\tNNZ(B) : " << B.nonzeros() << std::endl;
if (A.nrows() != B.nrows())
{
log << "LinearEigenSolver: dimensioni di A e B incompatibili \n";
return -1;
}
Vector<int_t> fpm(128);
feastinit(fpm.data());
fpm(1) = 1; // manda in output messAGGI a runtime
fpm(2) = 8; // numero di punti di quadratura (default=)8
if (toll_exp)
fpm(3) =toll_exp; // tolleranza 10^-fpm(3)
if (max_iter)
fpm(4) = max_iter;
fpm(64) = 0; // use PARDISO
int_t n = A.nrows();
double epsout = 0.;
int_t loop = 0;
double emin = 0.;
int_t subspace_dim = std::min(Z.ncols(),n); // m0 in feast routine
int_t founded_eig = 0; // feast m
int_t info = 0;
const char uplo = 'U';
log << "\n\tParmetri algoritmici:"
<< "\n\t\tintervallo autovalori : [" << emin << "," << eigenv_max << "]"
<< "\n\t\tdimensione sottospazio : " << subspace_dim
<< "\n\t\ttolleranza : 10E-" << fpm(3)
<< "\n\t\titerazioni massime : " << fpm(4)
<< std::endl;
log << "\n ................ Inizio messaggi solutore .................\n\n";
dfeast_scsrgv(&uplo, &n,
A.column_value(), A.row_pos(), A.column_index(),
B.column_value(), B.row_pos(), B.column_index(),
fpm.data(), &epsout, &loop, &emin, &eigenv_max,
&subspace_dim, L.data(), Z.data(), &founded_eig, eigen_error.data(),
&info);
log << "\n ................ Fine messaggi solutore ...................\n\n";
if (info != 0)
return info;
Vector<double> tmp_L(L.data(), founded_eig);
L.swap(tmp_L);
return info;
}
int LinearSparseSolver( static_sparse_CSR_Matrix<double, std::int32_t, M_PROP::SYM>& A,
Matrix<double, M_PROP::GE, M_SHAPE::RE, M_ORD::COL>& B,
Matrix<double, M_PROP::GE, M_SHAPE::RE, M_ORD::COL>& X,
std::ostream& log,
int threads)
{
typedef MML3_INT_TYPE int_t;
int len=100;
char buf[100];
//MKLGetVersionString(buf, len);
mkl_get_version_string(buf, len);
log << "Solutore lineare per matrici sparse basato su:\n"
<< "\tMKL-PARDISO: " << std::string(buf,len);
log << "\nCaratteristiche del sistema:"
<< "\n\t N =" << A.nrows()
<< "\n\t NNZ =" << A.nonzeros()
<< "\n\t NRHS=" << B.ncols() << std::endl;
// pardiso parameters
MML3::Vector<int_t> pardiso_pt(64);
pardiso_pt=0;
int_t pardiso_maxfct = 1; // una sola matrice da fattorizzare
int_t pardiso_mnum = 1; // la matrice numero 1
int_t pardiso_mtype = 2; // matrici simmetriche definite positive
int_t pardiso_phase = 0;
int_t pardiso_n = A.nrows();
double* pardiso_a=A.column_value();
int_t* pardiso_ia = A.row_pos();
int_t* pardiso_ja = A.column_index();
MML3::Vector<int_t> pardiso_perm(pardiso_n);
int_t pardiso_nrhs = 0;
MML3::Vector<int_t> pardiso_iparam(64);
pardiso_iparam = 0;
int_t pardiso_msglvl = 0; // 1=messaggi a run time, 0=no messaggi
double* pardiso_b=0;
double* pardiso_x=0;
int_t pardiso_error=0;
pardiso_iparam(1) = 1; /* No solver default*/
pardiso_iparam(2) = 2; /* Fill-in reordering from METIS, 1 for AMD */
pardiso_iparam(3) = threads; /* Numbers of thread processors */
pardiso_iparam(4) = 0; /* No iterative-direct algorithm */
pardiso_iparam(5) = 0; /* No user fill-in reducing permutation */
pardiso_iparam(6) = 0; /* Write solution into x */
pardiso_iparam(7) = 16; /* Default logical fortran unit number for output */
pardiso_iparam(8) = 0; /* Max numbers of iterative refinement steps */
pardiso_iparam(9) = 0; /* Not in use*/
pardiso_iparam(10) = 13; /* Perturb the pivot elements with 1E-13 */
pardiso_iparam(11) = 0; /* Use nonsymmetric permutation and scaling MPS */
pardiso_iparam(12) = 0; /* Not in use*/
pardiso_iparam(13) = 0; /* Not in use*/
pardiso_iparam(14) = 0; /* Output: Number of perturbed pivots */
pardiso_iparam(15) = 0; /* Not in use*/
pardiso_iparam(16) = 0; /* Not in use*/
pardiso_iparam(17) = 0; /* Not in use*/
pardiso_iparam(18) = -1; /* Output: Number of nonzeros in the factor LU */
pardiso_iparam(19) = -1; /* Output: Mflops for LU factorization */
pardiso_iparam(20) = 0; /* Output: Numbers of CG Iterations */
// --------------------------------------------------------------------
// Reordering and Symbolic Factorization. This step also allocates
// all memory that is necessary for the factorization.
// --------------------------------------------------------------------
pardiso_phase = 11;
PARDISO( pardiso_pt.begin(), &pardiso_maxfct, &pardiso_mnum, &pardiso_mtype, &pardiso_phase,
&pardiso_n, pardiso_a, pardiso_ia, pardiso_ja, pardiso_perm.begin(), &pardiso_nrhs,
pardiso_iparam.begin(), &pardiso_msglvl, 0, 0, &pardiso_error);
if (pardiso_error != 0)
{
log << "riordino e fattorizzazione simbolica falliti, codice: " << pardiso_error << "\n";
return -1;
}
log << "\tRiordino e fattorizzazione simbolica completata"
<< "\n\t\tFactor NNZ=" << pardiso_iparam(18)
<< "\n\t\tMFLOPS =" << pardiso_iparam(19);
// --------------------------------------------------------------------
// Numerical factorization.
// --------------------------------------------------------------------
pardiso_phase = 22;
PARDISO( pardiso_pt.begin(), &pardiso_maxfct, &pardiso_mnum, &pardiso_mtype, &pardiso_phase,
&pardiso_n, pardiso_a, pardiso_ia, pardiso_ja, pardiso_perm.begin(), &pardiso_nrhs,
pardiso_iparam.begin(), &pardiso_msglvl, 0, 0, &pardiso_error);
if (pardiso_error != 0)
{
log << " fattorizzazione numerica fallita, codice pardiso=" << pardiso_error << "\n";
return -1;
}
// --------------------------------------------------------------------*/
// Back substitution and iterative refinement. */
// --------------------------------------------------------------------*/
pardiso_phase = 33;
pardiso_nrhs=B.nrows();
pardiso_iparam(6) = 0; // Write solution into x. Attenzione, anche se lo pongo ad 1 in modo che la soluzione sovrascriva F, X viene comunque usato
PARDISO( pardiso_pt.begin(), &pardiso_maxfct, &pardiso_mnum, &pardiso_mtype, &pardiso_phase,
&pardiso_n, pardiso_a, pardiso_ia, pardiso_ja, pardiso_perm.begin(), &pardiso_nrhs,
pardiso_iparam.begin(), &pardiso_msglvl, B.begin(), X.begin(), &pardiso_error);
if (pardiso_error != 0)
{
log << "Errore nella fase di sostituzione all'indietro e rifinitura iteritava, codice: " << pardiso_error << "\n";
return -1;
}
// --------------------------------------------------------------------
// Termination and release of memory.
// --------------------------------------------------------------------
pardiso_phase = -1; /* Release internal memory. */
PARDISO( pardiso_pt.begin(), &pardiso_maxfct, &pardiso_mnum, &pardiso_mtype, &pardiso_phase,
&pardiso_n, pardiso_a, pardiso_ia, pardiso_ja, pardiso_perm.begin(), &pardiso_nrhs,
pardiso_iparam.begin(), &pardiso_msglvl, 0, 0, &pardiso_error);
return 1;
}
//centrilize and scale the point set
// xn = T*x;
// x: every column represent a point [3*N]
bool q_normalize_points(const vector<Coord3D_PCM> vec_input,vector<Coord3D_PCM> &vec_output,Matrix &x4x4_normalize)
{
//check parameters
if(vec_input.size()<=0)
{
fprintf(stderr,"ERROR: Input array is null! \n");
return false;
}
if(!vec_output.empty())
vec_output.clear();
vec_output=vec_input;
if(x4x4_normalize.nrows()!=4 || x4x4_normalize.ncols()!=4)
{
x4x4_normalize.ReSize(4,4);
}
//compute the centriod of input point set
Coord3D_PCM cord_centroid;
int n_point=vec_input.size();
for(int i=0;i<n_point;i++)
{
cord_centroid.x+=vec_input[i].x;
cord_centroid.y+=vec_input[i].y;
cord_centroid.z+=vec_input[i].z;
}
cord_centroid.x/=n_point;
cord_centroid.y/=n_point;
cord_centroid.z/=n_point;
//center the point set
for(int i=0;i<n_point;i++)
{
vec_output[i].x-=cord_centroid.x;
vec_output[i].y-=cord_centroid.y;
vec_output[i].z-=cord_centroid.z;
}
//compute the average distance of every point to the origin
double d_point2o=0,d_point2o_avg=0;
for(int i=0;i<n_point;i++)
{
d_point2o=sqrt(vec_output[i].x*vec_output[i].x+vec_output[i].y*vec_output[i].y+vec_output[i].z*vec_output[i].z);
d_point2o_avg+=d_point2o;
}
d_point2o_avg/=n_point;
//compute the scale factor
double d_scale_factor=1.0/d_point2o_avg;
//scale the point set
for(int i=0;i<n_point;i++)
{
vec_output[i].x*=d_scale_factor;
vec_output[i].y*=d_scale_factor;
vec_output[i].z*=d_scale_factor;
}
//compute the transformation matrix
// 1 row
x4x4_normalize(1,1)=d_scale_factor;
x4x4_normalize(1,2)=0;
x4x4_normalize(1,3)=0;
x4x4_normalize(1,4)=-d_scale_factor*cord_centroid.x;
// 2 row
x4x4_normalize(2,1)=0;
x4x4_normalize(2,2)=d_scale_factor;
x4x4_normalize(2,3)=0;
x4x4_normalize(2,4)=-d_scale_factor*cord_centroid.y;
// 3 row
x4x4_normalize(3,1)=0;
x4x4_normalize(3,2)=0;
x4x4_normalize(3,3)=d_scale_factor;
x4x4_normalize(3,4)=-d_scale_factor*cord_centroid.z;
// 4 row
x4x4_normalize(4,1)=0;
x4x4_normalize(4,2)=0;
x4x4_normalize(4,3)=0;
x4x4_normalize(4,4)=1;
return true;
}
bool q_gridnodes_dT_3D(const double *p_img64f_tar,const double *p_img64f_sub,const long sz_img[4],
const long sz_gridwnd,const Matrix &x_bsplinebasis,
vector< vector< vector< vector<double> > > > &vec4D_grid_dT)
{
if(p_img64f_tar==0 || p_img64f_sub==0)
{
printf("ERROR: Invalid input image pointer!\n");
return false;
}
if(sz_img[0]<=0 || sz_img[1]<=0 || sz_img[2]<=0 || sz_img[3]!=1)
{
printf("ERROR: Invalid input image size!\n");
return false;
}
if(sz_gridwnd<=1)
{
printf("ERROR: Invalid sz_gridwnd, it should >1!\n");
return false;
}
if(x_bsplinebasis.nrows()!=pow(sz_gridwnd,3) || x_bsplinebasis.ncols()!=pow(4,3))
{
printf("ERROR: Invalid input x_bsplinebasis size!\n");
return false;
}
if(vec4D_grid_dT.size()!=0)
{
printf("WARNNING: Output vec4D_grid_gradient is not empty, original data will be cleared!\n");
vec4D_grid_dT.clear();
}
clock_t start;
double ****p_img64f_tar_4d=0,****p_img64f_sub_4d=0;
if(!new4dpointer(p_img64f_tar_4d,sz_img[0],sz_img[1],sz_img[2],sz_img[3],p_img64f_tar) ||
!new4dpointer(p_img64f_sub_4d,sz_img[0],sz_img[1],sz_img[2],sz_img[3],p_img64f_sub))
{
printf("ERROR: Fail to allocate memory for the 4d pointer of image.\n");
if(p_img64f_tar_4d) {delete4dpointer(p_img64f_tar_4d,sz_img[0],sz_img[1],sz_img[2],sz_img[3]);}
if(p_img64f_sub_4d) {delete4dpointer(p_img64f_sub_4d,sz_img[0],sz_img[1],sz_img[2],sz_img[3]);}
return false;
}
vector< vector< vector< vector<double> > > > vec4D_img_gradient;//4D: row,col,z,dxdydz
vec4D_img_gradient.assign(sz_img[1],vector< vector< vector<double> > >(sz_img[0],vector< vector<double> >(sz_img[2],vector<double>(3,0))));
if(SHOWTIME) start=clock();
for(long z=0;z<sz_img[2];z++)
for(long y=0;y<sz_img[1];y++)
for(long x=0;x<sz_img[0];x++)
{
long x_s,x_b,y_s,y_b,z_s,z_b;
x_s=x-1; x_b=x+1;
y_s=y-1; y_b=y+1;
z_s=z-1; z_b=z+1;
x_s = x_s<0 ? 0:x_s; x_b = x_b>=sz_img[0] ? sz_img[0]-1:x_b;
y_s = y_s<0 ? 0:y_s; y_b = y_b>=sz_img[1] ? sz_img[1]-1:y_b;
z_s = z_s<0 ? 0:z_s; z_b = z_b>=sz_img[2] ? sz_img[2]-1:z_b;
double Ix,Iy,Iz;
if(p_img64f_sub_4d[0][z][y][x_b]<0 || p_img64f_sub_4d[0][z][y][x_s]<0 ||
p_img64f_sub_4d[0][z][y_b][x]<0 || p_img64f_sub_4d[0][z][y_s][x]<0 ||
p_img64f_sub_4d[0][z_b][y][x]<0 || p_img64f_sub_4d[0][z_s][y][x]<0 ||
p_img64f_tar_4d[0][z][y][x]<0 || p_img64f_sub_4d[0][z][y][x]<0)
{
vec4D_img_gradient[y][x][z][0]=0;
vec4D_img_gradient[y][x][z][1]=0;
vec4D_img_gradient[y][x][z][2]=0;
}
else
{
Ix=p_img64f_sub_4d[0][z][y][x_b]-p_img64f_sub_4d[0][z][y][x_s];
Iy=p_img64f_sub_4d[0][z][y_b][x]-p_img64f_sub_4d[0][z][y_s][x];
Iz=p_img64f_sub_4d[0][z_b][y][x]-p_img64f_sub_4d[0][z_s][y][x];
double I_Itar=p_img64f_sub_4d[0][z][y][x]-p_img64f_tar_4d[0][z][y][x];
vec4D_img_gradient[y][x][z][0]=I_Itar*Ix;
vec4D_img_gradient[y][x][z][1]=I_Itar*Iy;
vec4D_img_gradient[y][x][z][2]=I_Itar*Iz;
}
}
if(SHOWTIME) printf("\t\t\t\t>>compute dT time consume: %.2f s\n",(clock()-start)/100.0);
if(SHOWTIME) start=clock();
if(!q_bspline_field2grid_3D(vec4D_img_gradient,sz_gridwnd,x_bsplinebasis,vec4D_grid_dT))
{
printf("ERROR: q_bspline_field2grid_3D() return false!\n");
if(p_img64f_tar_4d) {delete4dpointer(p_img64f_tar_4d,sz_img[0],sz_img[1],sz_img[2],sz_img[3]);}
if(p_img64f_sub_4d) {delete4dpointer(p_img64f_sub_4d,sz_img[0],sz_img[1],sz_img[2],sz_img[3]);}
return false;
}
if(SHOWTIME) printf("\t\t\t\t>>compute dT-dM time consume: %.2f s\n",(clock()-start)/100.0);
if(p_img64f_tar_4d) {delete4dpointer(p_img64f_tar_4d,sz_img[0],sz_img[1],sz_img[2],sz_img[3]);}
if(p_img64f_sub_4d) {delete4dpointer(p_img64f_sub_4d,sz_img[0],sz_img[1],sz_img[2],sz_img[3]);}
return true;
}
double QuadProg::solve_quadprog(Matrix<double>& G, Vector<double>& g0,
const Matrix<double>& CE, const Vector<double>& ce0,
const Matrix<double>& CI, const Vector<double>& ci0,
Vector<double>& x)
{
std::ostringstream msg;
{
//Ensure that the dimensions of the matrices and vectors can be
//safely converted from unsigned int into to int without overflow.
unsigned mx = std::numeric_limits<int>::max();
if(G.ncols() >= mx || G.nrows() >= mx ||
CE.nrows() >= mx || CE.ncols() >= mx ||
CI.nrows() >= mx || CI.ncols() >= mx ||
ci0.size() >= mx || ce0.size() >= mx || g0.size() >= mx){
msg << "The dimensions of one of the input matrices or vectors were "
<< "too large." << std::endl
<< "The maximum allowable size for inputs to solve_quadprog is:"
<< mx << std::endl;
throw std::logic_error(msg.str());
}
}
int n = G.ncols(), p = CE.ncols(), m = CI.ncols();
if ((int)G.nrows() != n)
{
msg << "The matrix G is not a square matrix (" << G.nrows() << " x "
<< G.ncols() << ")";
throw std::logic_error(msg.str());
}
if ((int)CE.nrows() != n)
{
msg << "The matrix CE is incompatible (incorrect number of rows "
<< CE.nrows() << " , expecting " << n << ")";
throw std::logic_error(msg.str());
}
if ((int)ce0.size() != p)
{
msg << "The vector ce0 is incompatible (incorrect dimension "
<< ce0.size() << ", expecting " << p << ")";
throw std::logic_error(msg.str());
}
if ((int)CI.nrows() != n)
{
msg << "The matrix CI is incompatible (incorrect number of rows "
<< CI.nrows() << " , expecting " << n << ")";
throw std::logic_error(msg.str());
}
if ((int)ci0.size() != m)
{
msg << "The vector ci0 is incompatible (incorrect dimension "
<< ci0.size() << ", expecting " << m << ")";
throw std::logic_error(msg.str());
}
x.resize(n);
register int i, j, k, l; /* indices */
int ip; // this is the index of the constraint to be added to the active set
Matrix<double> R(n, n), J(n, n);
Vector<double> s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n), u_old(m + p);
double f_value, psi, c1, c2, sum, ss, R_norm;
double inf;
if (std::numeric_limits<double>::has_infinity)
inf = std::numeric_limits<double>::infinity();
else
inf = 1.0E300;
double t, t1, t2; /* t is the step lenght, which is the minimum of the partial step length t1
* and the full step length t2 */
Vector<int> A(m + p), A_old(m + p), iai(m + p);
int q, iq, iter = 0;
Vector<bool> iaexcl(m + p);
/* p is the number of equality constraints */
/* m is the number of inequality constraints */
q = 0; /* size of the active set A (containing the indices of the active constraints) */
#ifdef TRACE_SOLVER
std::cout << std::endl << "Starting solve_quadprog" << std::endl;
print_matrix("G", G);
print_vector("g0", g0);
print_matrix("CE", CE);
print_vector("ce0", ce0);
print_matrix("CI", CI);
print_vector("ci0", ci0);
#endif
/*
* Preprocessing phase
*/
/* compute the trace of the original matrix G */
c1 = 0.0;
for (i = 0; i < n; i++)
{
c1 += G[i][i];
}
/* decompose the matrix G in the form L^T L */
cholesky_decomposition(G);
#ifdef TRACE_SOLVER
print_matrix("G", G);
#endif
/* initialize the matrix R */
for (i = 0; i < n; i++)
{
d[i] = 0.0;
for (j = 0; j < n; j++)
R[i][j] = 0.0;
}
R_norm = 1.0; /* this variable will hold the norm of the matrix R */
/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
c2 = 0.0;
for (i = 0; i < n; i++)
{
d[i] = 1.0;
forward_elimination(G, z, d);
for (j = 0; j < n; j++)
J[i][j] = z[j];
c2 += z[i];
d[i] = 0.0;
}
#ifdef TRACE_SOLVER
print_matrix("J", J);
#endif
/* c1 * c2 is an estimate for cond(G) */
/*
* Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
* this is a feasible point in the dual space
* x = G^-1 * g0
*/
cholesky_solve(G, x, g0);
for (i = 0; i < n; i++)
x[i] = -x[i];
/* and compute the current solution value */
f_value = 0.5 * scalar_product(g0, x);
#ifdef TRACE_SOLVER
std::cout << "Unconstrained solution: " << f_value << std::endl;
print_vector("x", x);
#endif
/* Add equality constraints to the working set A */
iq = 0;
for (i = 0; i < p; i++)
{
for (j = 0; j < n; j++)
np[j] = CE[j][i];
compute_d(d, J, np);
update_z(z, J, d, iq);
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
print_matrix("R", R, n, iq);
print_vector("z", z);
print_vector("r", r, iq);
print_vector("d", d);
#endif
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
t2 = 0.0;
if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np);
/* set x = x + t2 * z */
for (k = 0; k < n; k++)
x[k] += t2 * z[k];
/* set u = u+ */
u[iq] = t2;
for (k = 0; k < iq; k++)
u[k] -= t2 * r[k];
/* compute the new solution value */
f_value += 0.5 * (t2 * t2) * scalar_product(z, np);
A[i] = -i - 1;
if (!add_constraint(R, J, d, iq, R_norm))
{
// Equality constraints are linearly dependent
throw std::runtime_error("Constraints are linearly dependent");
return f_value;
}
}
/* set iai = K \ A */
for (i = 0; i < m; i++)
iai[i] = i;
l1: iter++;
#ifdef TRACE_SOLVER
print_vector("x", x);
#endif
/* step 1: choose a violated constraint */
for (i = p; i < iq; i++)
{
ip = A[i];
iai[ip] = -1;
}
/* compute s[x] = ci^T * x + ci0 for all elements of K \ A */
ss = 0.0;
psi = 0.0; /* this value will contain the sum of all infeasibilities */
ip = 0; /* ip will be the index of the chosen violated constraint */
for (i = 0; i < m; i++)
{
iaexcl[i] = true;
sum = 0.0;
for (j = 0; j < n; j++)
sum += CI[j][i] * x[j];
sum += ci0[i];
s[i] = sum;
psi += std::min(0.0, sum);
}
#ifdef TRACE_SOLVER
print_vector("s", s, m);
#endif
if (fabs(psi) <= m * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
{
/* numerically there are not infeasibilities anymore */
q = iq;
return f_value;
}
/* save old values for u and A */
for (i = 0; i < iq; i++)
{
u_old[i] = u[i];
A_old[i] = A[i];
}
/* and for x */
for (i = 0; i < n; i++)
x_old[i] = x[i];
l2: /* Step 2: check for feasibility and determine a new S-pair */
for (i = 0; i < m; i++)
{
if (s[i] < ss && iai[i] != -1 && iaexcl[i])
{
ss = s[i];
ip = i;
}
}
if (ss >= 0.0)
{
q = iq;
return f_value;
}
/* set np = n[ip] */
for (i = 0; i < n; i++)
np[i] = CI[i][ip];
/* set u = [u 0]^T */
u[iq] = 0.0;
/* add ip to the active set A */
A[iq] = ip;
#ifdef TRACE_SOLVER
std::cout << "Trying with constraint " << ip << std::endl;
print_vector("np", np);
#endif
l2a:/* Step 2a: determine step direction */
/* compute z = H np: the step direction in the primal space (through J, see the paper) */
compute_d(d, J, np);
update_z(z, J, d, iq);
/* compute N* np (if q > 0): the negative of the step direction in the dual space */
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
std::cout << "Step direction z" << std::endl;
print_vector("z", z);
print_vector("r", r, iq + 1);
print_vector("u", u, iq + 1);
print_vector("d", d);
print_vector("A", A, iq + 1);
#endif
/* Step 2b: compute step length */
l = 0;
/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
t1 = inf; /* +inf */
/* find the index l s.t. it reaches the minimum of u+[x] / r */
for (k = p; k < iq; k++)
{
if (r[k] > 0.0)
{
if (u[k] / r[k] < t1)
{
t1 = u[k] / r[k];
l = A[k];
}
}
}
/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = -s[ip] / scalar_product(z, np);
else
t2 = inf; /* +inf */
/* the step is chosen as the minimum of t1 and t2 */
t = std::min(t1, t2);
#ifdef TRACE_SOLVER
std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
#endif
/* Step 2c: determine new S-pair and take step: */
/* case (i): no step in primal or dual space */
if (t >= inf)
{
/* QPP is infeasible */
// FIXME: unbounded to raise
q = iq;
return inf;
}
/* case (ii): step in dual space */
if (t2 >= inf)
{
/* set u = u + t * [-r 1] and drop constraint l from the active set A */
for (k = 0; k < iq; k++)
u[k] -= t * r[k];
u[iq] += t;
iai[l] = l;
delete_constraint(R, J, A, u, n, p, iq, l);
#ifdef TRACE_SOLVER
std::cout << " in dual space: "
<< f_value << std::endl;
print_vector("x", x);
print_vector("z", z);
print_vector("A", A, iq + 1);
#endif
goto l2a;
}
/* case (iii): step in primal and dual space */
/* set x = x + t * z */
for (k = 0; k < n; k++)
x[k] += t * z[k];
/* update the solution value */
f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]);
/* u = u + t * [-r 1] */
for (k = 0; k < iq; k++)
u[k] -= t * r[k];
u[iq] += t;
#ifdef TRACE_SOLVER
std::cout << " in both spaces: "
<< f_value << std::endl;
print_vector("x", x);
print_vector("u", u, iq + 1);
print_vector("r", r, iq + 1);
print_vector("A", A, iq + 1);
#endif
if (fabs(t - t2) < std::numeric_limits<double>::epsilon())
{
#ifdef TRACE_SOLVER
std::cout << "Full step has taken " << t << std::endl;
print_vector("x", x);
#endif
/* full step has taken */
/* add constraint ip to the active set*/
if (!add_constraint(R, J, d, iq, R_norm))
{
iaexcl[ip] = false;
delete_constraint(R, J, A, u, n, p, iq, ip);
#ifdef TRACE_SOLVER
print_matrix("R", R);
print_vector("A", A, iq);
print_vector("iai", iai);
#endif
for (i = 0; i < m; i++)
iai[i] = i;
for (i = p; i < iq; i++)
{
A[i] = A_old[i];
u[i] = u_old[i];
iai[A[i]] = -1;
}
for (i = 0; i < n; i++)
x[i] = x_old[i];
goto l2; /* go to step 2 */
}
else
iai[ip] = -1;
#ifdef TRACE_SOLVER
print_matrix("R", R);
print_vector("A", A, iq);
print_vector("iai", iai);
#endif
goto l1;
}
/* a patial step has taken */
#ifdef TRACE_SOLVER
std::cout << "Partial step has taken " << t << std::endl;
print_vector("x", x);
#endif
/* drop constraint l */
iai[l] = l;
delete_constraint(R, J, A, u, n, p, iq, l);
#ifdef TRACE_SOLVER
print_matrix("R", R);
print_vector("A", A, iq);
#endif
/* update s[ip] = CI * x + ci0 */
sum = 0.0;
for (k = 0; k < n; k++)
sum += CI[k][ip] * x[k];
s[ip] = sum + ci0[ip];
#ifdef TRACE_SOLVER
print_vector("s", s, m);
#endif
goto l2a;
}
bool q_bspline_field2grid_3D(const vector< vector< vector< vector<double> > > > &vec4D_grid_int,
const long sz_gridwnd,const Matrix &x_bsplinebasis,
vector< vector< vector< vector<double> > > > &vec4D_grid)
{
if(vec4D_grid_int.size()==0 || vec4D_grid_int[0].size()==0 || vec4D_grid_int[0][0].size()==0 || vec4D_grid_int[0][0][0].size()!=3)
{
printf("ERROR: Invalid input grid size!\n");
return false;
}
if(sz_gridwnd<=1)
{
printf("ERROR: Invalid sz_gridwnd, it should >1!\n");
return false;
}
if(x_bsplinebasis.nrows()!=pow(sz_gridwnd,3) || x_bsplinebasis.ncols()!=pow(4,3))
{
printf("ERROR: Invalid input x_bsplinebasis size!\n");
return false;
}
if(vec4D_grid.size()!=0)
{
printf("WARNNING: Output vec4D_grid is not empty, original data will be cleared!\n");
vec4D_grid.clear();
}
long sz_grid_int[3];
sz_grid_int[1]=vec4D_grid_int.size();
sz_grid_int[0]=vec4D_grid_int[0].size();
sz_grid_int[2]=vec4D_grid_int[0][0].size();
long sz_grid[3];
for(long i=0;i<3;i++)
sz_grid[i]=sz_grid_int[i]/sz_gridwnd+3;
vec4D_grid.assign(sz_grid[1],vector< vector< vector<double> > >(sz_grid[0],vector< vector<double> >(sz_grid[2],vector<double>(3,0))));
for(long x=0;x<sz_grid[0]-3;x++)
for(long y=0;y<sz_grid[1]-3;y++)
for(long z=0;z<sz_grid[2]-3;z++)
for(long xyz=0;xyz<3;xyz++)
{
Matrix x1D_transblock(sz_gridwnd*sz_gridwnd*sz_gridwnd,1);
long ind=1;
for(long zz=0;zz<sz_gridwnd;zz++)
for(long xx=0;xx<sz_gridwnd;xx++)
for(long yy=0;yy<sz_gridwnd;yy++)
{
x1D_transblock(ind,1)=vec4D_grid_int[y*sz_gridwnd+yy][x*sz_gridwnd+xx][z*sz_gridwnd+zz][xyz];
ind++;
}
Matrix x1D_grid=x_bsplinebasis.t()*x1D_transblock;
ind=1;
for(long dep=z;dep<z+4;dep++)
for(long col=x;col<x+4;col++)
for(long row=y;row<y+4;row++)
{
vec4D_grid[row][col][dep][xyz]+=x1D_grid(ind,1);
ind++;
}
}
return true;
}
void IRWPCA(const Matrix<double>& data,
Vector<double>& weights,
Vector<double>& mu,
Matrix<double>& trans,
const double tol/*=1E-8*/,
const int max_iter/*=1000*/,
const bool quiet/* = true*/)
{
int d_in = data.nrows();
int d_out = trans.nrows();
int N = data.ncols();
//check on input data
if (mu.size() != d_in)
mu.reallocate(d_in);
if (trans.ncols() != d_in)
trans.reallocate(d_out,d_in);
if (weights.size() != N)
weights.reallocate(N,1.0);
else
weights.SetAllTo(1.0);
if (max_iter<1)
throw MatVecError("max_iter must be positive");
//data needed for procedure
Matrix<double,SYM> transcov(d_in,d_in);
Vector<double> V;
Vector<double> weights_old;
Vector<double> V_minus_mu;
Matrix<double> trans_old;
PCA(data,mu,trans);
//Now we iterate and find the optimal weights,
// mean, and transformation
for(int count=1;count<max_iter;count++)
{
weights_old.deep_copy(weights);
trans_old.deep_copy(trans);
transcov = trans.Transpose() * trans;
//find errors and new weights
for(int i=0;i<N;i++)
{
V.deep_copy( data.col(i) );
V -= mu;
V_minus_mu.deep_copy(V);
V -= transcov * V_minus_mu;
weights(i) = pow( blas_NRM2(V), 2 );
}
make_weights(weights);
WPCA(data,weights,mu,trans);
//check for convergence using residuals between
// new weights and old weights
weights_old -= weights;
double Wres = blas_NRM2(weights_old);
// and new trans and old trans
//trans_old -= trans;
//double Tres = blas_NRM2(trans_old);
if ( ( Wres/sqrt(N) <tol) )
{
if (!quiet)
std::cout << "IRWPCA: converged to tol = " << tol
<< " in " << count << " iterations\n";
return;
}
}//end for
//if tolerance was not reached, return an error message
throw IterException(max_iter,tol);
}
