void basisfield::Set(const volume<float>& pfield)
{
if (int(FieldSz_x()) != pfield.xsize() || int(FieldSz_y()) != pfield.ysize() || int(FieldSz_z()) != pfield.zsize()) {
throw BasisfieldException("basisfield::Set:: Matrix size mismatch beween basisfield class and supplied field");
}
if (Vxs_x() != pfield.xdim() || Vxs_y() != pfield.ydim() || Vxs_z() != pfield.zdim()) {
throw BasisfieldException("basisfield::Set:: Voxel size mismatch beween basisfield class and supplied field");
}
volume<float> volume_of_ones(pfield.xsize(),pfield.ysize(),pfield.zsize());
volume_of_ones.copyproperties(pfield);
volume_of_ones = 1.0;
double lambda = 0.001;
ColumnVector y = Jte(pfield,0);
boost::shared_ptr<MISCMATHS::BFMatrix> XtX = JtJ(volume_of_ones);
boost::shared_ptr<MISCMATHS::BFMatrix> BeEn = BendEnergyHess();
XtX->AddToMe(*BeEn,lambda);
ColumnVector coef_roof = XtX->SolveForx(y,SYM_POSDEF,1e-6,500);
SetCoef(coef_roof);
}
int levenberg_marquardt_nllsq_impl(Function f, JacobianFunction fill_jac,
InputVector& x, const OutputVector& y,
LinearSolver lin_solve, LimitFunction impose_limits, unsigned int max_iter,
T tau, T epsj, T epsx, T epsy)
{
typedef typename vect_traits<InputVector>::value_type ValueType;
typedef typename vect_traits<InputVector>::size_type SizeType;
/* Check if the problem is defined properly */
if (y.size() < x.size())
throw improper_problem("Levenberg-Marquardt requires M > N!");
mat<ValueType,mat_structure::rectangular> J(y.size(),x.size());
mat<ValueType,mat_structure::square> JtJ(x.size());
mat<ValueType,mat_structure::diagonal> diag_JtJ(x.size());
mat<ValueType,mat_structure::scalar> mu(x.size(),0.0);
InputVector Jte = x;
InputVector Dp = x; Dp -= x;
mat_vect_adaptor<InputVector> Dp_mat(Dp);
InputVector pDp = x;
impose_limits(x,Dp); // make sure the initial solution is feasible.
x += Dp;
if(tau <= 0.0)
tau = 1E-03;
if(epsj <= 0.0)
epsj = 1E-17;
if(epsx <= 0.0)
epsx = 1E-17;
ValueType epsx_sq = epsx * epsx;
if(epsy <= 0.0)
epsy = 1E-17;
if(max_iter <= 1)
max_iter = 2;
/* compute e=x - f(p) and its L2 norm */
OutputVector y_approx = f(x);
OutputVector e = y; e -= y_approx;
OutputVector e_tmp = e;
ValueType p_eL2 = e * e;
unsigned int nu = 2;
for(unsigned int k = 0; k < max_iter; ++k) {
if(p_eL2 < epsy)
return 1; //residual is too small.
fill_jac(J,x,y_approx);
/* J^T J, J^T e */
for(SizeType i = 0; i < J.get_col_count(); ++i) {
for(SizeType j = i; j < J.get_col_count(); ++j) {
ValueType tmp(0.0);
for(SizeType l = 0; l < J.get_row_count(); ++l)
tmp += J(l,i) * J(l,j);
JtJ(i,j) = JtJ(j,i) = tmp;
};
};
Jte = e * J;
ValueType p_L2 = x * x;
/* check for convergence */
if( norm_inf(mat_vect_adaptor<InputVector>(Jte)) < epsj)
return 2; //Jacobian is too small.
/* compute initial damping factor */
if( k == 0 ) {
ValueType tmp = std::numeric_limits<ValueType>::min();
for(SizeType i=0; i < JtJ.get_row_count(); ++i)
if(JtJ(i,i) > tmp)
tmp = JtJ(i,i); /* find max diagonal element */
mu = mat<ValueType,mat_structure::scalar>(x.size(), tau * tmp);
};
/* determine increment using adaptive damping */
while(true) {
/* solve augmented equations */
try {
lin_solve(make_damped_matrix(JtJ,mu),Dp_mat,mat_vect_adaptor<InputVector>(Jte),epsj);
impose_limits(x,Dp);
ValueType Dp_L2 = Dp * Dp;
pDp = x; pDp += Dp;
if(Dp_L2 < epsx_sq * p_L2) /* relative change in p is small, stop */
return 3; //steps are too small.
if( Dp_L2 >= (p_L2 + epsx) / ( std::numeric_limits<ValueType>::epsilon() * std::numeric_limits<ValueType>::epsilon() ) )
throw 42; //signal to throw a singularity-error (see below).
e_tmp = y; e_tmp -= f(pDp);
ValueType pDp_eL2 = e_tmp * e_tmp;
ValueType dL = mu(0,0) * Dp_L2 + Dp * Jte;
ValueType dF = p_eL2 - pDp_eL2;
if( (dL < 0.0) || (dF < 0.0) )
throw singularity_error("reject inc.");
// reduction in error, increment is accepted
ValueType tmp = ( ValueType(2.0) * dF / dL - ValueType(1.0));
tmp = 1.0 - tmp * tmp * tmp;
mu *= ( ( tmp >= ValueType(1.0 / 3.0) ) ? tmp : ValueType(1.0 / 3.0) );
nu = 2;
x = pDp;
y_approx = y; y_approx -= e_tmp;
e = e_tmp;
p_eL2 = pDp_eL2;
break; //the step is accepted and the loop is broken.
} catch(singularity_error&) {
//the increment must be rejected (either by singularity in damped matrix or no-redux by the step.
mu *= ValueType(nu);
nu <<= 1; // 2*nu;
if( nu == 0 ) /* nu has overflown. */
throw infeasible_problem("Levenberg-Marquardt method cannot reduce the function further, matrix damping has overflown!");
} catch(int i) {
if(i == 42)
throw singularity_error("Levenberg-Marquardt method has detected a near-singularity in the Jacobian matrix!");
else
throw i; //just in case there might be another integer thrown (very unlikely).
};
}; /* inner loop */
};
//if this point is reached, it means we have reached the maximum iterations.
throw maximum_iteration(max_iter);
};
