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());
}
Beispiel #3
0
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;
}
Beispiel #4
0
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;
}