bool Solver_LP_abstract::readLpFromFile(const std::string& filename,
VectorX &c, VectorX &lb, VectorX &ub,
MatrixXX &A, VectorX &Alb, VectorX &Aub)
{
std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
typename MatrixXX::Index n=0, m=0;
in.read((char*) (&n),sizeof(typename MatrixXX::Index));
in.read((char*) (&m),sizeof(typename MatrixXX::Index));
c.resize(n);
lb.resize(n);
ub.resize(n);
A.resize(m,n);
Alb.resize(m);
Aub.resize(m);
in.read( (char *) c.data() , n*sizeof(typename MatrixXX::Scalar) );
in.read( (char *) lb.data() , n*sizeof(typename MatrixXX::Scalar) );
in.read( (char *) ub.data() , n*sizeof(typename MatrixXX::Scalar) );
in.read( (char *) A.data() , m*n*sizeof(typename MatrixXX::Scalar) );
in.read( (char *) Alb.data() , m*sizeof(typename MatrixXX::Scalar) );
in.read( (char *) Aub.data() , m*sizeof(typename MatrixXX::Scalar) );
in.close();
return true;
}
std::vector<double> Configurator::eigenToStdVector(const VectorX vec) {
return std::vector<double>(vec.data(), vec.data() + vec.size());
}
lbfgs::status lbfgs::cpu_lbfgs(float *h_x)
{
const size_t NX = m_costFunction.getNumberOfUnknowns();
floatdouble *d_x = new floatdouble[NX];
for (size_t idx = 0; idx < NX; ++idx)
d_x[idx] = h_x[idx];
VectorX xk = VectorX::Map(d_x, NX); // x_k, current solution
VectorX gk(NX); // g_k, gradient at x_k
VectorX gkm1(NX); // g_{k-1}, gradient at x_{k-1}
VectorX z(NX); // z, search direction
floatdouble fk; // f_k, value at x_k
floatdouble fkm1; // f_{k-1}, value at x_{k-1}
floatdouble H0 = 1.0f; // H_0, initial inverse Hessian (diagonal, same value for all elements)
// treat arrays as ring buffers!
VectorX s[HISTORY_SIZE]; // s, history of solution updates
VectorX y[HISTORY_SIZE]; // y, history of gradient updates
floatdouble alpha[HISTORY_SIZE]; // alpha, history of alphas (needed for z updates)
floatdouble rho [HISTORY_SIZE]; // rho, history of rhos (needed for z updates)
for (size_t i = 0; i < HISTORY_SIZE; ++i)
{
s[i] = VectorX(NX);
y[i] = VectorX(NX);
}
cpu_cost_function *cpucf = (cpu_cost_function*)&m_costFunction;
cpucf->cpu_f_gradf(xk.data(), &fk, gk.data());
size_t evals = 1;
status stat = LBFGS_REACHED_MAX_ITER;
#ifdef LBFGS_VERBOSE
std::cout << "lbfgs::cpu_lbfgs()" << std::endl;
#endif
size_t it;
for (it = 0; it < m_maxIter; ++it)
{
#ifdef LBFGS_VERBOSE
printf("f(x) = % 12e, ||grad||_2 = % 12e\n", fk, gk.norm());
#endif
// Check for convergence
// ---------------------
floatdouble xSquaredNorm = std::max<floatdouble>(1.0f, xk.squaredNorm());
if (gk.squaredNorm() < (m_gradientEps * m_gradientEps) * xSquaredNorm)
{
stat = LBFGS_BELOW_GRADIENT_EPS;
break;
}
// Find search direction
// ---------------------
z = -gk;
const size_t MAX_IDX = std::min<size_t>(it, HISTORY_SIZE);
for (size_t i = 1; i <= MAX_IDX; ++i)
{
const size_t idx = index(it - i);
alpha[idx] = s[idx].dot(z) * rho[idx];
z -= alpha[idx] * y[idx];
}
z *= H0;
for (size_t i = MAX_IDX; i > 0; --i)
{
const size_t idx = index(it - i);
const floatdouble beta = rho[idx] * y[idx].dot(z);
z += s[idx] * (alpha[idx] - beta);
}
// Perform backtracking line search
// --------------------------------
gkm1 = gk;
fkm1 = fk;
floatdouble step;
if (!cpu_linesearch(xk, z, cpucf, fk, gk, evals, gkm1, fkm1, stat, step, m_maxEvals))
{
break;
}
// Update s, y, rho and H_0
// ------------------------
s[index(it)] = z * step; // = x_k - x_{k-1}
y[index(it)] = gk - gkm1;
floatdouble yDotS = y[index(it)].dot(s[index(it)]);
rho[index(it)] = 1.0f / yDotS;
floatdouble yNorm2 = y[index(it)].squaredNorm();
if (yNorm2 > 1e-5f)
H0 = yDotS / yNorm2;
}
for (size_t i = 0; i < NX; ++i)
h_x[i] = float(xk[i]);
#ifdef LBFGS_VERBOSE
std::cout << "Number of iterations: " << it << std::endl;
std::cout << "Number of function/gradient evaluations: " << evals << std::endl;
std::cout << "Reason for termination: " << statusToString(stat) << std::endl;
#endif
return stat;
}
bool cpu_linesearch(VectorX &xk, VectorX &z,
cpu_cost_function *cpucf, floatdouble &fk, VectorX &gk,
size_t &evals, const VectorX &gkm1, const floatdouble &fkm1,
lbfgs::status &stat, floatdouble &step, size_t maxEvals)
{
const floatdouble c1 = 1e-4f;
const floatdouble c2 = 0.9f;
const floatdouble alpha_0 = 0.0;
const floatdouble phi_0 = fk;
const floatdouble phi_prime_0 = z.dot(gk);
if (phi_prime_0 >= 0.0)
{
stat = lbfgs::LBFGS_LINE_SEARCH_FAILED;
return false;
}
const floatdouble alpha_max = 1e8;
floatdouble alpha = 1.0;
floatdouble alpha_old = 0.0;
bool second_iter = false;
floatdouble alpha_low, alpha_high;
floatdouble phi_low, phi_high;
floatdouble phi_prime_low, phi_prime_high;
for (;;)
{
xk += (alpha - alpha_old) * z;
cpucf->cpu_f_gradf(xk.data(), &fk, gk.data());
++evals;
const floatdouble phi_alpha = fk;
const floatdouble phi_prime_alpha = z.dot(gk);
const bool armijo_violated = (phi_alpha > phi_0 + c1 * alpha * phi_prime_0 || (second_iter && phi_alpha >= phi_0));
const bool strong_wolfe = (std::abs(phi_prime_alpha) <= -c2 * phi_prime_0);
// If both Armijo and Strong Wolfe hold, we're done
if (!armijo_violated && strong_wolfe)
{
step = alpha;
return true;
}
if (evals >= maxEvals)
{
stat = lbfgs::LBFGS_REACHED_MAX_EVALS;
return false;
}
// If Armijio condition is violated, we've bracketed a viable minimum
// Interval is [alpha_0, alpha]
if (armijo_violated)
{
alpha_low = alpha_0;
alpha_high = alpha;
phi_low = phi_0;
phi_high = phi_alpha;
phi_prime_low = phi_prime_0;
phi_prime_high = phi_prime_alpha;
break;
}
// If the directional derivative at alpha is positive, we've bracketed a viable minimum
// Interval is [alpha, alpha_0]
if (phi_prime_alpha >= 0)
{
alpha_low = alpha;
alpha_high = alpha_0;
phi_low = phi_alpha;
phi_high = phi_0;
phi_prime_low = phi_prime_alpha;
phi_prime_high = phi_prime_0;
break;
}
// Else look to the "right" of alpha for a viable minimum
floatdouble alpha_new = alpha + 4 * (alpha - alpha_old);
alpha_old = alpha;
alpha = alpha_new;
if (alpha > alpha_max)
{
stat = lbfgs::LBFGS_LINE_SEARCH_FAILED;
return false;
}
second_iter = true;
}
// The minimum is now bracketed in [alpha_low, alpha_high]
// Find it...
size_t tries = 0;
const size_t minTries = 10;
while (true)
{
tries++;
alpha_old = alpha;
// Quadratic interpolation:
// Least-squares fit a parabola to (alpha_low, phi_low),
// (alpha_high, phi_high) with gradients phi_prime_low and
// phi_prime_high and select the minimum of that parabola as
// the new alpha
alpha = 0.5f * (alpha_low + alpha_high);
alpha += (phi_high - phi_low) / (phi_prime_low - phi_prime_high);
if (alpha < alpha_low || alpha > alpha_high)
alpha = 0.5f * (alpha_low + alpha_high);
xk += (alpha - alpha_old) * z;
cpucf->cpu_f_gradf(xk.data(), &fk, gk.data());
++evals;
const floatdouble phi_j = fk;
const floatdouble phi_prime_j = z.dot(gk);
const bool armijo_violated = (phi_j > phi_0 + c1 * alpha * phi_prime_0 || phi_j >= phi_low);
const bool strong_wolfe = (std::abs(phi_prime_j) <= -c2 * phi_prime_0);
if (!armijo_violated && strong_wolfe)
{
// The Armijo and Strong Wolfe conditions hold
step = alpha;
break;
}
else if (std::abs(alpha_high - alpha_low) < 1e-5 && tries > minTries)
{
// The search interval has become too small
stat = lbfgs::LBFGS_LINE_SEARCH_FAILED;
return false;
}
else if (armijo_violated)
{
alpha_high = alpha;
phi_high = phi_j;
phi_prime_high = phi_prime_j;
}
else
{
if (phi_prime_j * (alpha_high - alpha_low) >= 0)
{
alpha_high = alpha_low;
phi_high = phi_low;
phi_prime_high = phi_prime_low;
}
alpha_low = alpha;
phi_low = phi_j;
phi_prime_low = phi_prime_j;
}
if (evals >= maxEvals)
{
stat = lbfgs::LBFGS_REACHED_MAX_EVALS;
return false;
}
}
return true;
}