ConvexObjectivePtr CostFromFunc::convex(const vector<double>& xin) {
VectorXd x = getVec(xin, vars_);
ConvexObjectivePtr out(new ConvexObjective());
QuadExpr quad;
if (!full_hessian_) {
double val;
VectorXd grad,hess;
calcGradAndDiagHess(*f_, x, epsilon_, val, grad, hess);
hess = hess.cwiseMax(VectorXd::Zero(hess.size()));
//QuadExpr& quad = out->quad_;
quad.affexpr.constant = val - grad.dot(x) + .5*x.dot(hess.cwiseProduct(x));
quad.affexpr.vars = vars_;
quad.affexpr.coeffs = toDblVec(grad - hess.cwiseProduct(x));
quad.vars1 = vars_;
quad.vars2 = vars_;
quad.coeffs = toDblVec(hess*.5);
}
else {
double val;
VectorXd grad;
MatrixXd hess;
calcGradHess(f_, x, epsilon_, val, grad, hess);
MatrixXd pos_hess = MatrixXd::Zero(x.size(), x.size());
Eigen::SelfAdjointEigenSolver<MatrixXd> es(hess);
VectorXd eigvals = es.eigenvalues();
MatrixXd eigvecs = es.eigenvectors();
for (size_t i=0, end = x.size(); i != end; ++i) { //tricky --- eigen size() is signed
if (eigvals(i) > 0) pos_hess += eigvals(i) * eigvecs.col(i) * eigvecs.col(i).transpose();
}
//QuadExpr& quad = out->quad_;
quad.affexpr.constant = val - grad.dot(x) + .5*x.dot(pos_hess * x);
quad.affexpr.vars = vars_;
quad.affexpr.coeffs = toDblVec(grad - pos_hess * x);
int nquadterms = (x.size() * (x.size()-1))/2;
quad.coeffs.reserve(nquadterms);
quad.vars1.reserve(nquadterms);
quad.vars2.reserve(nquadterms);
for (size_t i=0, end = x.size(); i != end; ++i) { //tricky --- eigen size() is signed
quad.vars1.push_back(vars_[i]);
quad.vars2.push_back(vars_[i]);
quad.coeffs.push_back(pos_hess(i,i)/2);
for (size_t j=i+1; j != end; ++j) { //tricky --- eigen size() is signed
quad.vars1.push_back(vars_[i]);
quad.vars2.push_back(vars_[j]);
quad.coeffs.push_back(pos_hess(i,j));
}
}
}
out->addQuadExpr(quad);
return out;
}
bool ConjugateGradientType::improve_energy(bool verbose) {
iter++;
//printf("I am running ConjugateGradient::improve_energy\n");
const double E0 = energy();
if (E0 != E0) {
// There is no point continuing, since we're starting with a NaN!
// So we may as well quit here.
if (verbose) {
printf("The initial energy is a NaN, so I'm quitting early from ConjugateGradientType::improve_energy.\n");
f.print_summary("has nan:", E0);
fflush(stdout);
}
return false;
}
double gdotd;
{
const VectorXd g = -grad();
// Let's immediately free the cached gradient stored internally!
invalidate_cache();
// Note: my notation vaguely follows that of
// [wikipedia](http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method).
// I use the Polak-Ribiere method, with automatic direction reset.
// Note that we could save some memory by using Fletcher-Reeves, and
// it seems worth implementing that as an option for
// memory-constrained problems (then we wouldn't need to store oldgrad).
double beta = g.dot(g - oldgrad)/oldgradsqr;
oldgrad = g;
if (beta < 0 || beta != beta || oldgradsqr == 0) beta = 0;
oldgradsqr = oldgrad.dot(oldgrad);
direction = g + beta*direction;
gdotd = oldgrad.dot(direction);
if (gdotd < 0) {
direction = oldgrad; // If our direction is uphill, reset to gradient.
if (verbose) printf("reset to gradient...\n");
gdotd = oldgrad.dot(direction);
}
}
Minimizer lm = linmin(f, gd, kT, x, direction, -gdotd, &step);
for (int i=0; i<100 && lm.improve_energy(verbose); i++) {
if (verbose) lm.print_info("\t");
}
if (verbose) {
//lm->print_info();
print_info();
printf("grad*dir/oldgrad*dir = %g\n", grad().dot(direction)/gdotd);
}
return (energy() < E0);
}
void SOGP::predict(const VectorXd &state, VectorXd &prediction,
VectorXd &prediction_variance) {
//check if we have initialised the system
if (!this->initialized) {
throw OTLException("SOGP not yet initialised");
}
double kstar = kernel->eval(state,state);
//check if we not been trained
if (this->current_size == 0) {
prediction = VectorXd::Zero(this->output_dim);
prediction_variance = VectorXd::Ones(this->output_dim)*
(kstar + this->noise);
return;
}
VectorXd k;
kernel->eval(state, this->basis_vectors, k);
//std::cout << "K: \n" << k << std::endl;
//std::cout << "alpha: \n" << this->alpha.block(0,0,this->current_size, this->output_dim) << std::endl;
prediction = k.transpose() *this->alpha.block(0,0,this->current_size, this->output_dim);
prediction_variance = VectorXd::Ones(this->output_dim)*
(k.dot(this->C.block(0,0, this->current_size, this->current_size)*k)
+ kstar + this->noise);
return;
}
double cosangle(VectorXd a, VectorXd b) {
if (a.norm() < 1e-6 || b.norm() < 1e-6) {
return 1;
} else {
return a.dot(b) / (a.norm() * b.norm());
}
}
VectorXd Optimizer::compute_dog_leg(double alpha, const VectorXd& h_sd,
const VectorXd& h_gn, double delta, double& gain_ratio_denominator) {
if (h_gn.norm() <= delta) {
gain_ratio_denominator = current_SSE_at_linpoint;
return h_gn;
}
double h_sd_norm = h_sd.norm();
if ((alpha * h_sd_norm) >= delta) {
gain_ratio_denominator = delta * (2 * alpha * h_sd_norm - delta)
/ (2 * alpha);
return (delta / h_sd_norm) * h_sd;
} else {
// complicated case: calculate intersection of trust region with
// line between Gauss-Newton and steepest descent solutions
VectorXd a = alpha * h_sd;
VectorXd b = h_gn;
double c = a.dot(b - a);
double b_a_norm2 = (b - a).squaredNorm();
double a_norm2 = a.squaredNorm();
double delta2 = delta * delta;
double sqrt_term = sqrt(c * c + b_a_norm2 * (delta2 - a_norm2));
double beta;
if (c <= 0) {
beta = (-c + sqrt_term) / b_a_norm2;
} else {
beta = (delta2 - a_norm2) / (c + sqrt_term);
}
gain_ratio_denominator = .5 * alpha * (1 - beta) * (1 - beta) * h_sd_norm
* h_sd_norm + beta * (2 - beta) * current_SSE_at_linpoint;
return (alpha * h_sd + beta * (h_gn - alpha * h_sd));
}
}
bool NewtonMinimizerGradHessian::zoom(const double& wolfe1, const double& wolfe2, double& lo, double& hi, VectorXd& try_grad, VectorXd& direction, double& grad0_dir, double& val0, double& val_lo, VectorXd& init_params, VectorXd& try_params, unsigned int max_iter, double& result)
{
double tryval = val0;
double alpha = tryval;
double temp1 = tryval;
double temp2 = tryval;
double temp3 = tryval;
bool bounds = true;
unsigned int counter = 1;
while(true)
{
alpha = lo + hi;
alpha*=0.5;
try_params = direction;
try_params *= alpha;
try_params += init_params;
bounds = function->calcValGrad(try_params, tryval, try_grad);
for(unsigned int i=0;i<fixparameter.size();++i)
{
if(fixparameter[i] != 0)
{
try_grad[i] = 0.;
}
}
temp1 = wolfe1*alpha;
temp1 *= grad0_dir;
temp1 += val0;
if( ( tryval > temp1 ) || ( tryval >= val_lo ) || (bounds == false) )
{
// if( (fabs((tryval - val_lo)/(tryval)) < 1.0e-4) && (tryval < val_lo) ){result = alpha;return true;}
if( (fabs((tryval - val_lo)/(tryval)) < 1.0e-4) ){result = alpha;return true;}
hi = alpha;
}
else
{
temp1 = try_grad.dot(direction);
temp2 = -wolfe2*grad0_dir;
temp3 = fabs(temp1);
if( temp3 <= fabs(temp2) )
{
result = alpha;
return bounds;
}
temp3 = hi - lo;
temp1 *= temp3;
if( temp1 >= 0.)
{
hi = lo;
}
lo = alpha;
val_lo = tryval;
}
counter++;
if(counter > max_iter){return false;}
}
}
AffExpr affFromValGrad(double y, const VectorXd& x, const VectorXd& dydx, const VarVector& vars) {
AffExpr aff;
aff.constant = y - dydx.dot(x);
aff.coeffs = toDblVec(dydx);
aff.vars = vars;
aff = cleanupAff(aff);
return aff;
}
double ObjectiveMLS::eval(const VectorXd& x) const
{
double obj = 0.0;
for(int i = 0; i < a_.cols(); ++i)
obj -= logsig(-x.dot(a_.col(i)) - b_(i));
obj /= (double)a_.cols();
return obj;
}
MatrixXd HessianMEL::eval(const VectorXd& x) const
{
MatrixXd hess = MatrixXd::Zero(x.rows(), x.rows());
for(int i = 0; i < a_.cols(); ++i)
hess += a_.col(i) * a_.col(i).transpose() * exp(x.dot(a_.col(i)) + b_(i));
hess /= (double)a_.cols();
return hess;
}
VectorXd GradientMEL::eval(const VectorXd& x) const
{
VectorXd grad = VectorXd::Zero(x.rows());
for(int i = 0; i < a_.cols(); ++i)
grad += a_.col(i) * exp(x.dot(a_.col(i)) + b_(i));
grad /= (double)a_.cols();
return grad;
}
double ObjectiveMEL::eval(const VectorXd& x) const
{
double obj = 0.0;
for(int i = 0; i < a_.cols(); ++i)
obj += exp(x.dot(a_.col(i)) + b_(i));
obj /= (double)a_.cols();
return obj;
}
VectorXd GradientMLSNoCopy::eval(const VectorXd& x) const
{
VectorXd grad = VectorXd::Zero(x.rows());
for(int i = 0; i < A_->cols(); ++i)
grad += A_->col(i) * sigmoid(x.dot(A_->col(i)) + b_->coeff(i));
grad /= (double)A_->cols();
return grad;
}
double Triangle<ConcreteShape>::integrateField(const VectorXd &field) {
double val = 0;
Matrix<double,2,2> inverse_Jacobian;
double detJ;
std::tie(inverse_Jacobian,detJ) = ConcreteShape::inverseJacobian(mVtxCrd);
val = detJ*field.dot(mIntegrationWeights);
return val;
}
double Spectral::kernel(const VectorXd& a, const VectorXd& b){
switch(kernel_type){
case 2 :
return(pow(a.dot(b)+constant,order));
default :
return(exp(-gamma*((a-b).squaredNorm())));
}
}
void denseFisherMetric::bounceP(const VectorXd& normal)
{
mAuxVector = mGL.solve(normal);
double C = -2.0 * mP.dot(mAuxVector);
C /= normal.dot(mAuxVector);
mP += C * normal;
}
void PositionConstraint::fillObjGrad(std::vector<double>& dG) {
VectorXd dP = evalCon();
for(int dofIndex = 0; dofIndex < mNode->getNumDependentDofs(); dofIndex++) {
int i = mNode->getDependentDof(dofIndex);
const Var* v = mVariables[i];
double w = v->mWeight;
VectorXd J = xformHom(mNode->getDerivWorldTransform(dofIndex), mOffset);
J /= w;
dG[i] += 2 * dP.dot(J);
}
}
double dEda(const VectorXd &XY,
const VectorXd &s0,
const vector<spring> &springlist,
const vector<vector<int>> &springpairs,
double kappa,
const double g11,
const double g12,
const double g22)
{
double out;
out=s0.dot(HarmonicGradient(springlist,XY,g11,g12,g22)+BendingGrad(springpairs,springlist,XY,kappa,g11,g12,g22));
return out;
}
NaiveBayesClassifier::NaiveBayesClassifier(const vector<VectorXd>& x,
const vector<int>& y) :
k(0), d(0), p(), mu(), var() {
// n is the number of points
unsigned n = x.size();
assert(n > 0);
assert(y.size() == n);
// d is the dimensionality
d = x[0].size();
for (const VectorXd& v : x)
assert(v.size() == d);
// number of classes
k = *(std::max_element(y.cbegin(), y.cend())) + 1;
for (int i = 0; i < k; ++i) {
// find all points in class i
vector<VectorXd> xi;
for (unsigned j = 0; j < n; ++j)
if (y[j] == i)
xi.push_back(x[j]);
// ni is the number of points in class i
int ni = xi.size();
assert(ni > 0);
// prior probability
p.push_back((double)ni / (double)n);
// class mean
VectorXd m = VectorXd::Zero(d);
for (const VectorXd& v : xi)
m += v;
m /= ni;
mu.push_back(m);
// centered data matrix
MatrixXd z(d, ni);
for (int j = 0; j < ni; ++j)
z.col(j) = xi[j] - m;
// class-specific attribute variances
VectorXd variance(d);
for (int j = 0; j < d; ++j) {
VectorXd zj = z.row(j);
variance(j) = (1.0/ni) * zj.dot(zj);
}
var.push_back(variance);
}
}
double BacktrackingLineSearch::step_size(std::function<double (const VectorXd&)> f,
const VectorXd &dfx,
const VectorXd &x,
const VectorXd &direction) const {
auto m = direction.dot(dfx);
auto t = -_c * m;
auto fx = f(x);
auto step = _alpha;
for (unsigned int i = 0; i < _niter; i++) {
VectorXd new_x = x + direction*step;
if ((fx - f(new_x)) > step * t) {
break;
}
step = step * _tau;
}
return step;
}
void doConjStep(VectorXd &XY,
VectorXd &s0,
VectorXd &gradE,
const vector<spring> &springlist,
const vector<vector<int>> &springpairs,
int bendingOn,
double kappa,
int conjsteps,
double g11,
double g12,
double g22)
{
double a1=0.0;
double a2=1.0;
double betan;
VectorXd gradEn(gradE.size());
VectorXd sn(s0.size());
functor network(XY,s0,springlist,springpairs,kappa,g11,g12,g22);
doBracketfind(a1,a2,network);
if(doBracketfind(a1,a2,network))
{
// double an=doFalsePosition(network,a1,a2);
//double an=Ridder(network,a1,a2);
double an=Brent(network,a1,a2,1e-12);
//Update the positions.
XY=XY+an*s0;
if(bendingOn==0){
gradEn=HarmonicGradient(springlist,XY,g11,g12,g22);
} else{
gradEn=HarmonicGradient(springlist,XY,g11,g12,g22)+BendingGrad(springpairs,springlist,XY,kappa,g11,g12,g22);
}
betan=(gradEn-gradE).dot(gradEn)/(gradE.dot(gradE));
//Did not find bracket, reset CG-method
} else{
betan=0.0;
if(bendingOn==0){
gradE=HarmonicGradient(springlist,XY,g11,g12,g22);
} else{
gradE=HarmonicGradient(springlist,XY,g11,g12,g22)+BendingGrad(springpairs,springlist,XY,kappa,g11,g12,g22);
}
s0=-gradE;
//cout<<"Bracket failed, Reset CG"<<endl;
return;
}
if(conjsteps%5 ==0.0) betan=0.0;
if(betan<0.0) betan=0; //max(betan,0)
if(abs(gradEn.dot(gradE))>.5*gradE.dot(gradE)) betan=0.0;
if(-2*gradE.dot(gradE)>gradE.dot(s0) && gradE.dot(s0) >-.2*gradE.dot(gradE)) betan=0.0;
//cout<<"\r"<<"** Beta "<<betan<<flush;
sn=-gradEn+betan*s0;
gradE=gradEn;
s0=sn;
}
double line_search(fcn_Rn_to_R f, VectorXd x0, VectorXd v0, VectorXd Dfx0 ) {
// Performs a line search. Takes in a function (f), initial point (x), and a
// direction (v). Tries to find a scalar value (a) such that f(x + av) is
// minimized.
//
double alpha = 1;
double beta = 0.2;
double tau = 0.5;
double fval = f(x0);
double stepsize = beta * Dfx0.dot(v0);
while ( f(x0 + alpha*v0) > fval + alpha*stepsize ) {
alpha = tau*alpha;
//cout << "Step size : " << alpha << endl;
}
return alpha;
}
// [[Rcpp::export()]]
VectorXd sample_delta_c_Eigen(
VectorXd delta,
VectorXd tauh,
Map<MatrixXd> Lambda_prec,
double delta_1_shape,
double delta_1_rate,
double delta_2_shape,
double delta_2_rate,
Map<MatrixXd> randg_draws, // all done with rate = 1;
Map<MatrixXd> Lambda2
) {
int times = randg_draws.rows();
int k = tauh.size();
MatrixXd scores_mat = Lambda_prec.cwiseProduct(Lambda2);
VectorXd scores = scores_mat.colwise().sum();
double rate,delta_old;
for(int i = 0; i < times; i++){
delta_old = delta(0);
rate = delta_1_rate + 0.5 * (1/delta(0)) * tauh.dot(scores);
delta(0) = randg_draws(i,0) / rate;
// tauh = cumprod(delta);
tauh *= delta(0)/delta_old; // replaces re-calculating cumprod
for(int h = 1; h < k-1; h++) {
delta_old = delta(h);
rate = delta_2_rate + 0.5*(1/delta(h))*tauh.tail(k-h).dot(scores.tail(k-h));
delta(h) = randg_draws(i,h) / rate;
// tauh = cumprod(delta);
tauh.tail(k-h) *= delta(h)/delta_old; // replaces re-calculating cumprod
// Rcout << (tauh - cumprod(delta)).sum() << std::endl;
}
}
return(delta);
}
/*
* frame_offset: used to place HxBj in right place in H
*/
bool MSCKF::getResidualH(VectorXd& ri, MatrixXd& Hi, Vector3d feature_pose, MatrixXd measure, MatrixXd pose_mtx, int frame_offset)
{
int num_frame = (int)pose_mtx.cols();
int errorStateLength = (int)fullErrorCovariance.rows();
ri = VectorXd::Zero(2 * num_frame);
Hi = MatrixXd::Zero(2 * num_frame, errorStateLength); // Hi cols == error state length
MatrixXd HxBj, Hc;
MatrixXd Mij, tmp39;
MatrixXd Hfi;
MatrixXd Hf; // use double precision to increase numerial result
Hfi = MatrixXd::Zero(2 * num_frame, 3);
for(int j = 0; j < num_frame; j++)
{
cout << "frame is " << frame_offset+j << endl;
Matrix3d R_gb = quaternion_to_R(pose_mtx.block<4, 1>(0, j));
//double xx = pts[i * 3 + 0] - position(0);
//double yy = pts[i * 3 + 1] - position(1);
//double zz = pts[i * 3 + 2] - position(2);
//Vector3d local_point = Ric.inverse() * (quat.inverse() * Vector3d(xx, yy, zz) - Tic);
Vector3d feature_in_c = R_cb * R_gb.transpose() * (feature_pose - pose_mtx.block<3, 1>(4, j)) + fullNominalState.segment(16, 3);
//Vector2d projPtr = projectPoint(feature_pose, R_gb, pose_mtx.block<3, 1>(4, j), fullNominalState.segment(16, 3));
//zij = cam.h(R_cb * R_gb.transpose() * (feature_pose - p_gb) + p_cb);
Vector2d projPtr = projectCamPoint(feature_in_c);
//cout << "projPtr projectPoint" << projPtr.transpose() << endl;
/*
if (feature_in_c(2) < 1e-4)
{
projPtr = measure.col(j);
return false;
}
else
{
projPtr = projectCamPoint(feature_in_c);
}*/
//if (projPtr(0)<0 || projPtr(1)>800 || projPtr(1)<0||projPtr(1)>800)
// return false;
// cout << "projPtr" << projPtr.transpose() << endl;
cout << "measure is " << measure.col(j).transpose() << endl;
cout << "feature in c is " << feature_in_c.transpose() << endl;
cout << "estimat is " << projPtr.transpose() << endl;
ri.segment(j * 2, 2) = measure.col(j) - projPtr;
// cout << "ri piece is" << ri.segment(j * 2, 2) <<endl;
if (ri.segment(j * 2, 2).norm() > 2.0)
{
return false;
}
Mij = cam.Jh(feature_in_c) * R_cb * R_gb.transpose();
tmp39 = MatrixXd::Zero(3, 9);
tmp39.block<3, 3>(0, 0) = skew_mtx(feature_pose - pose_mtx.block<3, 1>(4, j));
tmp39.block<3, 3>(0, 3) = -Matrix3d::Identity();
HxBj = Mij * tmp39; // 2x9
Hc = cam.Jh(feature_in_c); // 2x3
// cout << "Mij is " << endl << Mij << endl;
// cout << "tmp39 is " << endl << tmp39 << endl;
// cout << "HxBj is " << endl << HxBj << endl;
// cout << "Hc is " << endl << Hc << endl;
Hi.block<2, 9>(j * 2, ERROR_STATE_SIZE + 3 + ERROR_POSE_STATE_SIZE * (frame_offset + j)) = HxBj;
Hi.block<2, 3>(j * 2, ERROR_STATE_SIZE) = Hc;
Hf = cam.Jh(feature_in_c) * R_cb * R_gb.transpose();
Hfi.block<2, 3>(j * 2, 0) = Hf.cast<double>();
}
// now carry out feature error marginalization
JacobiSVD<MatrixXd> svd(Hfi.transpose(), ComputeFullV);
MatrixXd left_null = svd.matrixV().cast<double>().rightCols(2 * num_frame - 3).transpose();
// MatrixXd S = svd.singularValues().asDiagonal();
// MatrixXd U = svd.matrixU();
// MatrixXd V = svd.matrixV();
// MatrixXd D = Hfi.transpose()-U*S*V.transpose();
// std::cout << "\n" << D.norm() << " " << sqrt((D.adjoint()*D).trace()) << "\n";
// printf("left null size (%d, %d)\n", left_null.rows(), left_null.cols());
// printf("ri size (%d, %d)\n", ri.rows(), ri.cols());
// printf("Hi size (%d, %d)\n", Hi.rows(), Hi.cols());
// cout << "before left null" << endl;
// cout << "------------------" << endl;
// cout << ri << endl;
// cout << Hi << endl;
ri = left_null * ri;
Hi = left_null * Hi;
// cout << "one measure, one H" << endl;
// cout << "------------------" << endl;
// cout << ri << endl;
// cout << Hi << endl;
// printf("ri size (%d, %d)\n", ri.rows(), ri.cols());
// printf("Hi size (%d, %d)\n", Hi.rows(), Hi.cols());
double gamma;
MatrixXd tmpK = Hi * fullErrorCovariance * Hi.transpose();
MatrixXd tmpKinv = tmpK.colPivHouseholderQr().solve(MatrixXd::Identity(tmpK.rows(), tmpK.cols()));
int k = ri.size();
gamma = fabs(ri.dot( tmpKinv * ri));
cout << "gamma " << gamma<< endl;
// cout << "ha? " << fabs(ri.dot( Hi *Hi.transpose()* ri))<< endl;
if (k>30) k = 30;
if (gamma*gamma > chi_test[k])
return false;
else
return true;
}
// full reorthogonalization
double EigenTriangle::solve(int maxIter)
{
int n = graph -> VertexCount();
srand(time(0));
// check if rows == cols()
if (maxIter > n)
maxIter = n;
vector<VectorXd> v;
v.push_back(VectorXd::Random(n));
v[0] = v[0] / v[0].norm();
VectorXd alpha(n);
VectorXd beta(n + 1);
beta[0] = 0;
VectorXd w;
int last = 0;
printf("%d\n", maxIter);
for (int i = 0; i < maxIter; i ++)
{
if (i * 100 / maxIter > last + 10 || i == maxIter - 1)
{
last = (i + 1) * 100 / maxIter;
std::cout << "\r" << "[EigenTriangle] Processing " << last << "% ..." << std::flush;
}
w = adjMatrix * v[i];
alpha[i] = w.dot(v[i]);
if (i == maxIter - 1)
break;
w -= alpha[i] * v[i];
if (i > 0)
w -= beta[i] * v[i - 1];
beta[i + 1] = w.norm();
v.push_back(w / beta[i + 1]);
for (int j = 0; j <= i; j ++)
{
v[i + 1] = v[i + 1] - v[i + 1].dot(v[j]) * v[j];
}
}
printf("\n");
int k = maxIter;
MatrixXd T = MatrixXd::Zero(k, k);
for (int i = 0; i < k; i ++)
{
T(i, i) = alpha[i];
if (i < k - 1)
T(i, i + 1) = beta[i + 1];
if (i > 0)
T(i, i - 1) = beta[i];
}
//std::cout << T << std::endl;
Eigen::EigenSolver<MatrixXd> eigenSolver;
eigenSolver.compute(T, /* computeEigenvectors = */ false);
Eigen::VectorXcd eigens = eigenSolver.eigenvalues();
double res = 0;
for (int i = 0; i < eigens.size(); i ++)
{
std::complex<double> tmp = eigens[i];
res += pow(tmp.real(), 3);
printf("%.5lf\n", tmp.real());
}
res /= 6;
return res;
}
bool NewtonMinimizerGradHessian::findSaddlePoint(const VectorXd& start_point, VectorXd& min_point, double tol, unsigned int max_iter, double abs_tol)
{
try
{
if(!function){throw (char*)("minimize called but function has not been set");}
}
catch(char* str)
{
cout<<"Exception from NewtonMinimizerGradHessian: "<<str<<endl;
throw;
return false;
}
unsigned int npars = function->nPars();
try
{
if(!(npars>0)){throw (char*)("function to be minimized has zero dimensions");}
if(start_point.rows()!=(int)npars){throw (char*)("input to minimizer not of dimension required by function to be minimized");}
}
catch(char* str)
{
cout<<"Exception from NewtonMinimizerGradHessian: "<<str<<endl;
throw;
return false;
}
//parameters used for the Wolfe conditions
double c1 = 1.0e-6;
double c2 = 1.0e-1;
VectorXd working_points[2];
working_points[0] = VectorXd::Zero(npars);
working_points[1] = VectorXd::Zero(npars);
VectorXd* current_point = &(working_points[0]);
VectorXd* try_point = &(working_points[1]);
(*current_point) = start_point;
double value=0.;
double prev_value=0.;
double dir_der=0.;
double scale = 1.;
double scale_temp = 1.;
double grad0_dir = 0.;
bool good_value = true;
bool bounds = true;
unsigned int search_iter = 64;
VectorXd grad[2];
grad[0] = VectorXd::Zero(npars);
grad[1] = VectorXd::Zero(npars);
VectorXd* current_grad = &(grad[0]);
VectorXd* try_grad = &(grad[1]);
VectorXd newgrad = VectorXd::Zero(npars);
MatrixXd hessian = MatrixXd::Zero(npars, npars);
VectorXd move = VectorXd::Zero(npars);
VectorXd unit_move = VectorXd::Zero(npars);
FunctionGradHessian* orig_func = function;
SquareGradient gradsquared(orig_func);
function = &gradsquared;
//try a Newton iteration
orig_func->calcValGradHessian((*current_point), value, (*current_grad), hessian);
move = -hessian.fullPivLu().solve(*current_grad);
gradsquared.calcValGrad((*current_point), value, newgrad);
good_value=true;
for(unsigned int i=0;i<npars;++i){if(!(move(i) == move(i))){good_value=false;break;}}
if(good_value == false){move = -newgrad;}
dir_der = newgrad.dot(move);
if(dir_der>0.){move = -move;}
gradsquared.rescaleMove((*current_point), move);
grad0_dir = newgrad.dot(move);
scale_temp = 1.;
//find scale, such that move*scale satisfies the strong Wolfe conditions
bounds = lineSearch(scale_temp, c1, c2, (*try_grad), move, grad0_dir, value, (*current_point), (*try_point), tol, abs_tol, search_iter, scale);
if(bounds == false){min_point = start_point; function=orig_func;return false;}
move *= scale;
(*try_point) = ((*current_point) + move);
orig_func->calcValGradHessian((*try_point), prev_value, (*try_grad), hessian);
gradsquared.calcValGrad((*try_point), prev_value, newgrad);
swap(current_point, try_point);
swap(current_grad, try_grad);
swap(value, prev_value);
unsigned long int count = 1;
bool converged=false;
while(converged==false)
{
if((fabs((prev_value - value)/prev_value)<tol || fabs(prev_value - value)<abs_tol)){converged=true;break;}
prev_value = value;
//try a Newton iteration
move = -hessian.fullPivLu().solve(*current_grad);
gradsquared.calcValGrad((*current_point), value, newgrad);
good_value=true;
for(unsigned int i=0;i<npars;++i){if(!(move(i) == move(i))){good_value=false;break;}}
scale_temp = 1.;
scale_temp = fabs(value/newgrad.dot(move));
if(good_value == false){move = -newgrad;scale_temp = fabs(value/move.dot(move));}
dir_der = newgrad.dot(move);
if(dir_der>0.){move = -move;}
gradsquared.rescaleMove((*current_point), move);
grad0_dir = newgrad.dot(move);
//find scale, such that move*scale satisfies the strong Wolfe conditions
bounds = lineSearch(scale_temp, c1, c2, (*try_grad), move, grad0_dir, value, (*current_point), (*try_point), tol, abs_tol, search_iter, scale);
if(bounds == false){min_point = (*current_point); function=orig_func;return false;}
move *= scale;
(*try_point) = ((*current_point) + move);
orig_func->calcValGradHessian((*try_point), value, (*try_grad), hessian);
gradsquared.calcValGrad((*try_point), value, newgrad);
swap(current_point, try_point);
swap(current_grad, try_grad);
count++;
if(count > max_iter){break;}
}
orig_func->computeCovariance(value, hessian);
min_point = (*current_point);
function=orig_func;return converged;
}
bool NewtonMinimizerGradHessian::lineSearch(double& alpha, const double& wolfe1, const double& wolfe2, VectorXd& try_grad, VectorXd& direction, double& grad0_dir, double& val0, VectorXd& init_params, VectorXd& try_params, const double& precision, const double& accuracy, unsigned int max_iter, double& result)
{
double tryval = val0;
double prev_val = tryval;
double prev_alpha = tryval;
double lo = tryval;
double hi = tryval;
double temp1 = tryval;
double temp2 = tryval;
double temp3 = tryval;
unsigned int i = 1;
bool bounds = true;
prev_alpha = 0.;
while(true)
{
try_params = direction;
try_params *= alpha;
try_params += init_params;
bounds = function->calcValGrad(try_params, tryval, try_grad);
for(unsigned int j=0;j<fixparameter.size();++j)
{
if(fixparameter[j] != 0)
{
try_grad[j] = 0.;
}
}
if(bounds == false)
{
while(true)
{
alpha *= 0.5;
try_params = direction;
try_params *= alpha;
try_params += init_params;
bounds = function->calcValGrad(try_params, tryval, try_grad);
for(unsigned int j=0;j<fixparameter.size();++j)
{
if(fixparameter[j] != 0)
{
try_grad[j] = 0.;
}
}
if(bounds==true)
{
if(tryval < val0)
{
result=alpha;
return true;
}
}
if(i>max_iter){return false;}
i++;
}
alpha = 0.5*(prev_alpha + alpha);
i++;
if(i > max_iter){return false;}
continue;
}
temp1 = wolfe1*alpha;
temp1 *= grad0_dir;
temp1 += val0;
if( ( tryval > temp1 ) || ( ( tryval > prev_val ) && (i>1) ))
{
lo = prev_alpha;
hi = alpha;
return zoom(wolfe1, wolfe2, lo, hi, try_grad, direction, grad0_dir, val0, prev_val, init_params, try_params, max_iter, result);
}
temp1 = try_grad.dot(direction);
temp2 = -wolfe2*grad0_dir;
temp3 = fabs(temp1);
if( temp3 <= fabs(temp2) )
{
result = alpha;
return bounds;
}
if( temp1 >= 0. )
{
lo = alpha;
hi = prev_alpha;
return zoom(wolfe1, wolfe2, lo, hi, try_grad, direction, grad0_dir, val0, tryval, init_params, try_params, max_iter, result);
}
prev_val = tryval;
prev_alpha = alpha;
alpha *= 2.;
i++;
if(i > max_iter){return false;}
}
}
double drwnLPSolver::solve()
{
DRWN_FCN_TIC;
const int n = _A.cols();
const int m = _A.rows();
// find feasible starting point
if (_x.rows() == 0) {
_x = VectorXd::Zero(n);
for (int i = 0; i < n; i++) {
if (isUnbounded(i)) continue;
if (_ub[i] == DRWN_DBL_MAX) {
_x[i] = _lb[i] + 1.0;
} else if (_lb[i] == -DRWN_DBL_MAX) {
_x[i] = _ub[i] - 1.0;
} else {
_x[i] = 0.5 * (_lb[i] + _ub[i]);
}
}
}
// initialize kkt system variables
MatrixXd F = MatrixXd::Zero(n + m, n + m);
VectorXd g = VectorXd::Zero(n + m);
F.block(n, 0, m, n) = _A;
F.block(0, n, n, m) = _A.transpose();
// initialize dual variables (if needed)
VectorXd nu = VectorXd::Zero(m);
// iterate on interior point method
double t = t0;
while (1) {
// determine feasibility
const bool bFeasible = ((_b - _A * _x).squaredNorm() < eps);
if (!bFeasible) {
DRWN_LOG_VERBOSE("...finding feasible point");
}
// centering step and update
for (unsigned iter = 0; iter < maxiters; iter++) {
//! \todo solve with blockwise elimination
// construct KKT system, Fx = g
// | H A^T | | dx | = | - g |
// | A 0 | | w | | b - Ax |
for (int i = 0; i < n; i++) {
F(i, i) = 0.0;
if (_ub[i] != DRWN_DBL_MAX) {
F(i, i) += 1.0 / ((_ub[i] - _x[i]) * (_ub[i] - _x[i]));
}
if (_lb[i] != -DRWN_DBL_MAX) {
F(i, i) += 1.0 / ((_x[i] - _lb[i]) * (_x[i] - _lb[i]));
}
}
for (int i = 0; i < n; i++) {
g[i] = - t * _c[i];
if (_ub[i] != DRWN_DBL_MAX) {
g[i] += 1.0 / (_x[i] - _ub[i]);
}
if (_lb[i] != -DRWN_DBL_MAX) {
g[i] += 1.0 / (_x[i] - _lb[i]);
}
}
g.tail(m) = _b - _A * _x;
// check terminating condition
const double r_primal = g.tail(m).squaredNorm();
const double r_dual = (_A.transpose() * nu - g.head(n)).squaredNorm();
//if (!bFeasible && (r_primal + r_dual < drwnLPSolver::eps)) break;
if (!bFeasible && (r_primal < drwnLPSolver::eps)) break;
// solve KKT system
const VectorXd dxnu = F.fullPivLu().solve(g);
const double lambda_sqr = g.head(n).dot(dxnu.head(n));
if (bFeasible && (0.5 * lambda_sqr < 1.0e-6)) break;
if (bFeasible) {
// feasible line search
const double f_prev = t * _c.dot(_x) + barrierFunction(_x);
const double delta_f = alpha * g.dot(dxnu.head(n));
double step = 1.0;
while (1) {
const VectorXd nx = _x + step * dxnu.head(n);
if (isWithinBounds(nx)) {
const double f = t * _c.dot(nx) + barrierFunction(nx);
if (f - f_prev < step * delta_f) {
_x = nx;
break;
}
}
step *= beta;
}
} else {
// infeasible start line search
double step = 1.0;
while (1) {
const VectorXd nx = _x + step * dxnu.head(n);
if (isWithinBounds(nx)) {
const VectorXd nnu = (1.0 - step) * nu + step * dxnu.tail(m);
const double r = (_A.transpose() * nnu - g.head(n)).squaredNorm() +
(_b - _A * nx).squaredNorm();
if (r <= (1.0 - alpha * step) * (r_primal + r_dual)) {
_x = nx;
nu = nnu;
break;
}
}
step *= beta;
}
}
}
// check if feasible point was found
if (!bFeasible && ((_b - _A * _x).squaredNorm() > eps)) {
DRWN_LOG_WARNING("...could not find a feasible point (residual norm is " <<
(_b - _A * _x).norm() << ")");
DRWN_FCN_TOC;
return DRWN_DBL_MAX;
}
DRWN_LOG_VERBOSE("...objective is " << _c.dot(_x));
// check stopping criteria
if (m < eps * t) break;
// update barrier function multiplier
t *= mu;
}
// compute true objective and return
DRWN_FCN_TOC;
return _c.dot(_x);
}
double drwnSparseLPSolver::solve()
{
DRWN_FCN_TIC;
const int n = _A.cols();
const int m = _A.rows();
// find feasible starting point
if (_x.rows() == 0) {
_x = VectorXd::Zero(n);
for (int i = 0; i < n; i++) {
if (isUnbounded(i)) continue;
if (_ub[i] == DRWN_DBL_MAX) {
_x[i] = _lb[i] + 1.0;
} else if (_lb[i] == -DRWN_DBL_MAX) {
_x[i] = _ub[i] - 1.0;
} else {
_x[i] = 0.5 * (_lb[i] + _ub[i]);
}
}
}
// initialize kkt system variables
SparseMatrix<double> F(n + m, n + m);
VectorXd g = VectorXd::Zero(n + m);
vector<Eigen::Triplet<double> > entries;
entries.reserve(n + 2 * _A.nonZeros());
for (int i = 0; i < n; i++) {
entries.push_back(Eigen::Triplet<double>(i, i, 1.0));
}
for (int k = 0; k < _A.outerSize(); k++) {
for (SparseMatrix<double>::InnerIterator it(_A, k); it; ++it) {
entries.push_back(Eigen::Triplet<double>(n + it.row(), it.col(), it.value()));
entries.push_back(Eigen::Triplet<double>(it.col(), n + it.row(), it.value()));
}
}
F.setFromTriplets(entries.begin(), entries.end());
// initialize dual variables (if needed)
VectorXd nu = VectorXd::Zero(m);
// iterate on interior point method
double t = drwnLPSolver::t0;
while (1) {
// determine feasibility
const bool bFeasible = ((_b - _A * _x).squaredNorm() < drwnLPSolver::eps);
if (!bFeasible) {
DRWN_LOG_VERBOSE("...finding feasible point");
}
// centering step and update
for (unsigned iter = 0; iter < drwnLPSolver::maxiters; iter++) {
//! \todo solve with blockwise elimination
// construct KKT system, Fx = g
// | H A^T | | dx | = | - g |
// | A 0 | | w | | b - Ax |
for (int i = 0; i < n; i++) {
F.coeffRef(i, i) = 0.0;
if (_ub[i] != DRWN_DBL_MAX) {
F.coeffRef(i, i) += 1.0 / ((_ub[i] - _x[i]) * (_ub[i] - _x[i]));
}
if (_lb[i] != -DRWN_DBL_MAX) {
F.coeffRef(i, i) += 1.0 / ((_x[i] - _lb[i]) * (_x[i] - _lb[i]));
}
}
for (int i = 0; i < n; i++) {
g[i] = - t * _c[i];
if (_ub[i] != DRWN_DBL_MAX) {
g[i] += 1.0 / (_x[i] - _ub[i]);
}
if (_lb[i] != -DRWN_DBL_MAX) {
g[i] += 1.0 / (_x[i] - _lb[i]);
}
}
g.tail(m) = _b - _A * _x;
// check terminating condition
const double r_primal = g.tail(m).squaredNorm();
const double r_dual = (_A.transpose() * nu - g.head(n)).squaredNorm();
//if (!bFeasible && (r_primal + r_dual < drwnLPSolver::eps)) break;
if (!bFeasible && (r_primal < drwnLPSolver::eps)) break;
// solve KKT system
SimplicialLDLT<SparseMatrix<double> > chol(F);
const VectorXd dxnu = chol.solve(g);
const double lambda_sqr = g.head(n).dot(dxnu.head(n));
if (bFeasible && (0.5 * lambda_sqr < 1.0e-6)) break;
if (bFeasible) {
// feasible line search
const double f_prev = t * _c.dot(_x) + barrierFunction(_x);
const double delta_f = drwnLPSolver::alpha * g.dot(dxnu.head(n));
double step = 1.0;
while (1) {
const VectorXd nx = _x + step * dxnu.head(n);
if (isWithinBounds(nx)) {
const double f = t * _c.dot(nx) + barrierFunction(nx);
if (f - f_prev < step * delta_f) {
_x = nx;
break;
}
}
step *= drwnLPSolver::beta;
}
} else {
// infeasible start line search
DRWN_LOG_DEBUG("...iteration " << iter << ", primal residual " << r_primal
<< ", dual residual " << r_dual);
double step = 1.0;
while (1) {
const VectorXd nx = _x + step * dxnu.head(n);
if (isWithinBounds(nx)) {
const VectorXd nnu = (1.0 - step) * nu + step * dxnu.tail(m);
const double r = (_A.transpose() * nnu - g.head(n)).squaredNorm() +
(_b - _A * nx).squaredNorm();
if (r <= (1.0 - drwnLPSolver::alpha * step) * (r_primal + r_dual)) {
_x = nx;
nu = nnu;
break;
}
}
step *= drwnLPSolver::beta;
}
}
}
// check if feasible point was found
if (!bFeasible && ((_b - _A * _x).squaredNorm() > drwnLPSolver::eps)) {
DRWN_LOG_WARNING("...could not find a feasible point (residual norm is " <<
(_b - _A * _x).norm() << ")");
DRWN_FCN_TOC;
return DRWN_DBL_MAX;
}
DRWN_LOG_VERBOSE("...objective is " << _c.dot(_x));
// check stopping criteria
if (m < drwnLPSolver::eps * t) break;
// update barrier function multiplier
t *= drwnLPSolver::mu;
}
// compute true objective and return
DRWN_FCN_TOC;
return _c.dot(_x);
}
inline double Prob_Cijs(VectorXd& beta , VectorXd& aij){
double dotprod = beta.dot(aij);
double prob = 1/(1+exp(dotprod));
return(prob);
}
RcppExport SEXP nniv(SEXP arg1, SEXP arg2, SEXP arg3) {
// 3 arguments
// arg1 for parameters
// arg2 for data
// arg3 for Gibbs
// data
List list2(arg2);
const MatrixXd X=as< Map<MatrixXd> >(list2["X"]),
Z=as< Map<MatrixXd> >(list2["Z"]);
const VectorXd t=as< Map<VectorXd> >(list2["t"]),
y=as< Map<VectorXd> >(list2["y"]);
const int N=X.rows(), p=X.cols(), q=Z.cols(), r=p+q, s=p+r;
// parameters
List list1(arg1),
beta_info=list1["beta"],
Tprec_info=list1["Tprec"],
mu_info=list1["mu"],
theta_info=list1["theta"];
// prior parameters
List beta_prior=beta_info["prior"],
Tprec_prior=Tprec_info["prior"],
mu_prior=mu_info["prior"],
theta_prior=theta_info["prior"];
const double Tprec_prior_nu=as<double>(Tprec_prior["nu"]);
const Matrix2d Tprec_prior_Psi=as< Map<MatrixXd> >(Tprec_prior["Psi"]);
const double beta_prior_mean=as<double>(beta_prior["mean"]);
const double beta_prior_prec=as<double>(beta_prior["prec"]);
const Vector2d mu_prior_mean=as< Map<VectorXd> >(mu_prior["mean"]);
const Matrix2d mu_prior_prec=as< Map<MatrixXd> >(mu_prior["prec"]);
const VectorXd theta_prior_mean=as< Map<VectorXd> >(theta_prior["mean"]);
const MatrixXd theta_prior_prec=as< Map<MatrixXd> >(theta_prior["prec"]);
// initialize parameters
double beta=as<double>(beta_info["init"]);
Matrix2d Tprec=as< Map<MatrixXd> >(Tprec_info["init"]);
Vector2d mu =as< Map<VectorXd> >(mu_info["init"]);
VectorXd theta=as< Map<VectorXd> >(theta_info["init"]);
// Gibbs
List list3(arg3); //, save=list3["save"];
const int burnin=as<int>(list3["burnin"]), M=as<int>(list3["M"]),
thin=as<int>(list3["thin"]), m=7+s;
MatrixXd GS(M, m);
// prior parameter intermediate values
double beta_prior_prod=beta_prior_prec * beta_prior_mean;
VectorXd theta_prior_prod=theta_prior_prec * theta_prior_mean;
Vector2d mu_prior_prod=mu_prior_prec*mu_prior_mean;
// parameter intermediate values
Matrix2d Sigma, B_inverse;
Sigma=Tprec.inverse();
B_inverse.setIdentity();
VectorXd gamma=theta.segment(0, p),
delta=theta.segment(p, q),
eta =theta.segment(r, p);
/*
MatrixXd Theta(2, r);
Theta.row(0)=theta.segment(0, r);
Theta.bottomLeftCorner(1, q)=RowVectorXd::Zero(q);
Theta.bottomRightCorner(1, p)=eta.transpose();
*/
Vector2d eps, eps_sum;
MatrixXd D(N, 2), theta_cond_var_root(s, s), W(2, s);
W.setZero();
MatrixXd theta_cond_prec(s, s);
VectorXd theta_cond_prod(s), w(r);
Matrix2d mu_cond_prec, mu_cond_var_root, E;
Vector2d u, R, mu_cond_prod, mu_u;
double beta_scale, beta_prec, beta_prod, beta_cond_var, beta_cond_mean;
int h=0, i, l;
// Gibbs loop
//for(int l=-burnin; l<=(M-1)*thin; ++l) {
l=-burnin;
do{
// sample beta
D.col(0).setConstant(-mu[0]);
D.col(1).setConstant(-mu[1]);
D.col(0) += (t - X*gamma - Z*delta);
D.col(1) += (y - X*eta);
beta_scale=1./(Sigma(0, 0)*t.dot(t));
beta_prec=1./(beta_scale*Sigma.determinant());
beta_prod=beta_prec*beta_scale
*((Sigma(0, 0)*D.col(1)-Sigma(0, 1)*D.col(0)).array()*t.array()).sum();
beta_cond_var=1./(beta_prec+beta_prior_prec);
beta_cond_mean=beta_cond_var*(beta_prod+beta_prior_prod);
beta=rnorm1d(beta_cond_mean, sqrt(beta_cond_var));
B_inverse(1, 0)=-beta;
// sample theta
theta_cond_prec=theta_prior_prec;
theta_cond_prod=theta_prior_prod;
for(i=0; i<N; ++i) {
/*
W.topLeftCorner(1, p)=X.row(i);
W.block(0, p, 1, q)=Z.row(i);
W.bottomRightCorner(1, p)=X.row(i);
*/
W.block(0, 0, 1, p)=X.row(i);
W.block(0, p, 1, q)=Z.row(i);
W.block(1, r, 1, p)=X.row(i);
theta_cond_prec += (W.transpose() * Tprec * W);
u[0]=t[i];
u[1]=y[i];
R=B_inverse*u-mu;
theta_cond_prod += (W.transpose() * Tprec * R);
}
theta_cond_var_root=inv_root_chol(theta_cond_prec);
// theta_cond_var_root=inv_root_svd(theta_cond_prec); // for validation only
theta=theta_cond_var_root*(rnormXd(s)+theta_cond_var_root.transpose()*theta_cond_prod);
gamma=theta.segment(0, p);
delta=theta.segment(p, q);
eta =theta.segment(r, p);
/*
Theta.topRows(1)=theta.segment(0, r).transpose();
Theta.bottomRightCorner(1, p)=eta.transpose();
*/
// sample mu
eps_sum.setZero();
//W.setZero();
E.setZero();
for(i=0; i<N; ++i) {
/*
W.topLeftCorner(1, p)=X.row(i);
W.block(0, p, 1, q)=Z.row(i);
W.bottomRightCorner(1, p)=X.row(i);
*/
W.block(0, 0, 1, p)=X.row(i);
W.block(0, p, 1, q)=Z.row(i);
W.block(1, r, 1, p)=X.row(i);
/*
w.segment(0, q)=Z.row(i);
w.segment(q, p)=X.row(i);
*/
u[0]=t[i];
u[1]=y[i];
//eps += B_inverse*u - Theta*w;
eps = B_inverse*u - W*theta;
eps_sum += eps;
eps -= mu;
E += eps*eps.transpose();
}
mu_cond_prod=Tprec*eps_sum+mu_prior_prod;
mu_cond_prec=(N*Tprec+mu_prior_prec);
mu_cond_var_root=inv_root_chol(mu_cond_prec);
// mu_cond_var_root=inv_root_svd(mu_cond_prec); // for validation only
mu=mu_cond_var_root*(rnormXd(2)+mu_cond_var_root.transpose()*mu_cond_prod);
// sample Tprec
Tprec = rwishart((E+Tprec_prior_Psi).inverse(), N+Tprec_prior_nu);
Sigma = Tprec.inverse();
if(l>=0 && l%thin == 0) {
h = (l/thin);
GS.block(h, 0, 1, s)=theta.transpose();
GS(h, s)=beta;
GS(h, s+1)=mu[0];
GS(h, s+2)=mu[1];
GS(h, s+3)=Tprec(0, 0);
GS(h, s+4)=Tprec(0, 1);
GS(h, s+5)=Tprec(0, 1);
GS(h, s+6)=Tprec(1, 1);
}
l++;
} while (l<=(M-1)*thin && beta==beta);
if(beta != beta) GS.conservativeResize(h+1, m);
return wrap(GS);
}
