Commit 0901b40c authored by Max Lyon's avatar Max Lyon
Browse files

Merge branch 'VCI/master'

parents bd13932e 4232c92c
......@@ -48,8 +48,16 @@ public:
/// Destructor
~AcceleratedQuadraticProxy() {}
// solve
int solve(NProblemInterface* _quadratic_problem, NProblemInterface* _nonlinear_problem, const SMatrixD& _A, const VectorD& _b)
// solve without linear constraints
int solve(NProblemInterface* _quadratic_problem, NProblemInterface* _nonlinear_problem, bool _update_factorization = true)
{
SMatrixD A(0,_quadratic_problem->n_unknowns());
VectorD b(VectorD::Index(0));
return solve(_quadratic_problem, _nonlinear_problem, A, b, _update_factorization);
}
// solve with linear constraints
int solve(NProblemInterface* _quadratic_problem, NProblemInterface* _nonlinear_problem, const SMatrixD& _A, const VectorD& _b, bool _update_factorization = true)
{
// time solution procedure
COMISO::StopWatch sw; sw.start();
......@@ -78,7 +86,8 @@ public:
const double theta = (1.0-std::sqrt(1.0/accelerate_))/(1.0+std::sqrt(1.0/accelerate_));
// pre-factorize linear system
pre_factorize(_quadratic_problem, _A, _b);
if(_update_factorization)
pre_factorize(_quadratic_problem, _A, _b);
int iter=0;
while( iter < max_iters_)
......@@ -206,7 +215,10 @@ protected:
if( fx_ls <= fx + alpha_ls_*t*gtdx )
{
_rel_df = 1.0-fx_ls/fx;
_rel_df = std::abs(1.0-fx_ls/fx);
std::cerr << "LS improved objective function " << fx << " -> " << fx_ls << std::endl;
return t;
}
else
......
//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>
#if COMISO_EIGEN3_AVAILABLE
//== INCLUDES =================================================================
#include "CombinedProblem.hh"
namespace COMISO {
CombinedProblem::CombinedProblem (NProblemInterface* _p1, NProblemInterface* _p2, const double _c1, const double _c2)
: p1_(_p1) , p2_(_p2), c1_(_c1), c2_(_c2)
{
if(p1_->n_unknowns() != p2_->n_unknowns())
std::cerr << "Warning: CombinedProblem received two problems with different #unknowns!!!" << std::endl;
g_temp_.resize(p1_->n_unknowns());
}
CombinedProblem::~CombinedProblem()
{
}
int CombinedProblem::n_unknowns()
{
return p1_->n_unknowns();
}
void CombinedProblem::initial_x(double* _x)
{
// use initial_x of first problem
p1_->initial_x(_x);
}
double CombinedProblem::eval_f( const double* _x)
{
return c1_*p1_->eval_f(_x) + c2_*p2_->eval_f(_x);
}
void CombinedProblem::eval_gradient( const double* _x, double* _g)
{
p1_->eval_gradient(_x, _g);
p2_->eval_gradient(_x, g_temp_.data());
for( int i=0; i<this->n_unknowns(); ++i)
_g[i] = c1_*_g[i]+c2_*g_temp_[i];
}
void CombinedProblem::eval_hessian ( const double* _x, SMatrixNP& _H)
{
SMatrixNP H1, H2;
p1_->eval_hessian(_x, H1);
p2_->eval_hessian(_x, H2);
_H.resize(n_unknowns(), n_unknowns());
_H = c1_*H1 + c2_*H2;
}
void CombinedProblem::store_result ( const double* _x )
{
p1_->store_result(_x);
p2_->store_result(_x);
}
bool CombinedProblem::constant_gradient() const
{
return (p1_->constant_gradient() && p2_->constant_gradient());
}
bool CombinedProblem::constant_hessian() const
{
return (p1_->constant_hessian() && p2_->constant_hessian());
}
double CombinedProblem::max_feasible_step ( const double* _x, const double* _v)
{
return std::min(p1_->max_feasible_step(_x,_v), p2_->max_feasible_step(_x,_v));
}
//=============================================================================
} // namespace COMISO
//=============================================================================
#endif // COMISO_EIGEN3_AVAILABLE
//=============================================================================
//=============================================================================
//
// CLASS CombinedProblem
//
//=============================================================================
#ifndef COMISO_COMBINEDPROBLEM_HH
#define COMISO_COMBINEDPROBLEM_HH
//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>
#if COMISO_EIGEN3_AVAILABLE
//== INCLUDES =================================================================
#include <stdio.h>
#include <iostream>
#include <vector>
#include <CoMISo/Config/CoMISoDefines.hh>
#include <CoMISo/NSolver/NProblemInterface.hh>
//== FORWARDDECLARATIONS ======================================================
//== NAMESPACES ===============================================================
namespace COMISO {
//== CLASS DEFINITION =========================================================
/** \class CombinedProblem CombinedProblem.hh <CoMISo/NSolver/CombinedProblem.hh>
Create Problem of the type f(x) = _c1*_p1(x) + _c2*_p2(x), which is the linear combination
of two other problems (specified in the constructor).
Note: the initial_x is taken from _p1! And the result is stored for both _p1 and _p2
A more elaborate description follows.
*/
class COMISODLLEXPORT CombinedProblem : public NProblemInterface
{
public:
/// Default constructor
CombinedProblem (NProblemInterface* _p1, NProblemInterface* _p2, const double _c1 = 1.0, const double _c2 = 1.0);
/// Destructor
virtual ~CombinedProblem();
// problem definition
virtual int n_unknowns();
virtual void initial_x(double* _x);
virtual double eval_f( const double* _x);
virtual void eval_gradient( const double* _x, double* _g);
virtual void eval_hessian ( const double* _x, SMatrixNP& _H);
virtual void store_result ( const double* _x );
// advanced properties
virtual bool constant_gradient() const;
virtual bool constant_hessian() const;
virtual double max_feasible_step ( const double* _x, const double* _v);
private:
// pointer to two original problems
NProblemInterface* p1_;
NProblemInterface* p2_;
// weighting coefficients
double c1_;
double c2_;
// temporary variables
std::vector<double> g_temp_;
};
//=============================================================================
} // namespace COMISO
//=============================================================================
#endif // COMISO_EIGEN3_AVAILABLE
//=============================================================================
#endif // COMISO_NPROBLEMGMMINTERFACE_HH defined
//=============================================================================
//=============================================================================
//
// CLASS LinearConstraintConverter
//
//=============================================================================
#ifndef COMISO_LINEARCONSTRAINTCONVERTER_HH
#define COMISO_LINEARCONSTRAINTCONVERTER_HH
//== INCLUDES =================================================================
#include <CoMISo/Config/CoMISoDefines.hh>
#include "NConstraintInterface.hh"
#include "LinearConstraint.hh"
#include <Eigen/StdVector>
#include <Eigen/Dense>
#include <Eigen/Sparse>
//== FORWARDDECLARATIONS ======================================================
//== NAMESPACES ===============================================================
namespace COMISO {
//== CLASS DEFINITION =========================================================
/** \class NProblemGmmInterface NProblemGmmInterface.hh <ACG/.../NPRoblemGmmInterface.hh>
Brief Description.
A more elaborate description follows.
*/
class COMISODLLEXPORT LinearConstraintConverter
{
public:
// sparse vector type
typedef NConstraintInterface::SVectorNC SVectorNC;
typedef Eigen::SparseMatrix<double,Eigen::ColMajor> SparseMatrixC;
typedef Eigen::SparseMatrix<double,Eigen::RowMajor> SparseMatrixR;
typedef Eigen::VectorXd VectorXd;
LinearConstraintConverter(SparseMatrixR& _A, VectorXd& _b)
: A_(_A), b_(_b), linear_constraints_initialized_(false)
{}
LinearConstraintConverter(SparseMatrixC& _A, VectorXd& _b)
: A_(_A), b_(_b), linear_constraints_initialized_(false)
{}
std::vector<NConstraintInterface*>& constraints_nsolver()
{
initialize_linear_constraints();
return constraint_pointers_;
}
private:
void initialize_linear_constraints()
{
if(!linear_constraints_initialized_)
{
// tag as done
linear_constraints_initialized_ = true;
int m = A_.rows();
int n = A_.cols();
linear_constraints_.clear();
linear_constraints_.resize(m);
constraint_pointers_.clear();
constraint_pointers_.resize(m);
for( int i=0; i<m; ++i)
{
// convert i-th constraint
linear_constraints_.resize(n);
linear_constraints_[i].coeffs() = A_.row(i);
linear_constraints_[i].b() = -b_[i];
// store pointer
constraint_pointers_[i] = &(linear_constraints_[i]);
}
}
}
private:
SparseMatrixR A_;
VectorXd b_;
bool linear_constraints_initialized_;
std::vector<LinearConstraint> linear_constraints_;
std::vector<NConstraintInterface*> constraint_pointers_;
};
//=============================================================================
} // namespace COMISO
//=============================================================================
//=============================================================================
#endif // ACG_LINEARCONSTRAINTCONVERTER_HH defined
//=============================================================================
......@@ -38,11 +38,11 @@ solve(NProblemGmmInterface* _problem)
double reg = 1e-3;
COMISO::CholmodSolver chol;
for(int i=0; i<max_iter_; ++i)
for(int i=0; i<max_iters_; ++i)
{
_problem->eval_gradient(P(x), P(g));
// check for convergence
if( gmm::vect_norm2(g) < convergence_eps_)
if( gmm::vect_norm2(g) < eps_)
{
std::cerr << "Newton Solver converged after "
<< i << " iterations" << std::endl;
......@@ -103,7 +103,7 @@ solve(NProblemGmmInterface* _problem)
}
_problem->store_result(P(x));
std::cerr << "Newton Solver did not converge!!! after "
<< max_iter_ << " iterations." << std::endl;
<< max_iters_ << " iterations." << std::endl;
return false;
#else
......
......@@ -14,9 +14,14 @@
//== INCLUDES =================================================================
#include <CoMISo/Config/CoMISoDefines.hh>
#include <gmm/gmm.h>
#include <CoMISo/Utils/StopWatch.hh>
#include "NProblemInterface.hh"
#include "NProblemGmmInterface.hh"
#if COMISO_SUITESPARSE_AVAILABLE
#include <Eigen/UmfPackSupport>
#endif
//== FORWARDDECLARATIONS ======================================================
//== NAMESPACES ===============================================================
......@@ -36,24 +41,227 @@ namespace COMISO {
class COMISODLLEXPORT NewtonSolver
{
public:
typedef Eigen::VectorXd VectorD;
typedef Eigen::SparseMatrix<double> SMatrixD;
typedef Eigen::Triplet<double> Triplet;
/// Default constructor
NewtonSolver() : max_iter_(20), convergence_eps_(1e-6), constant_hessian_structure_(false) {}
NewtonSolver(const double _eps = 1e-6, const int _max_iters = 200, const double _alpha_ls=0.2, const double _beta_ls = 0.6)
: eps_(_eps), max_iters_(_max_iters), alpha_ls_(_alpha_ls), beta_ls_(_beta_ls), constant_hessian_structure_(false) {}
/// Destructor
~NewtonSolver() {}
// solve without linear constraints
int solve(NProblemInterface* _problem)
{
SMatrixD A(0,_problem->n_unknowns());
VectorD b(VectorD::Index(0));
return solve(_problem, A, b);
}
// solve with linear constraints
// Warning: so far only feasible starting points with (_A*_problem->initial_x() == b) are supported!
// Extension to infeasible starting points is planned
int solve(NProblemInterface* _problem, const SMatrixD& _A, const VectorD& _b)
{
// time solution procedure
COMISO::StopWatch sw; sw.start();
// number of unknowns
int n = _problem->n_unknowns();
// number of constraints
int m = _b.size();
std::cerr << "optmize via Newton with " << n << " unknowns and " << m << " linear constraints" << std::endl;
// initialize vectors of unknowns
VectorD x(n);
_problem->initial_x(x.data());
// storage of update vector dx and rhs of KKT system
VectorD dx(n+m), rhs(n+m), g(n);
rhs.setZero();
// resize temp vector for line search (and set to x1 to approx Hessian correctly if problem is non-quadratic!)
x_ls_ = x;
// start with no regularization
double regularize(0.0);
int iter=0;
while( iter < max_iters_)
{
// get Newton search direction by solving LSE
factorize(_problem, _A, _b, x, regularize);
// get rhs
_problem->eval_gradient(x.data(), g.data());
rhs.head(n) = -g;
rhs.tail(m) = _b - _A*x;
// solve KKT system
dx = lu_solver_.solve(rhs);
// get maximal reasonable step
double t_max = std::min(1.0, 0.5*_problem->max_feasible_step(x.data(), dx.data()));
// perform line-search
double newton_decrement(0.0);
double fx(0.0);
double t = backtracking_line_search(_problem, x, g, dx, newton_decrement, fx, t_max);
x += dx.head(n)*t;
std::cerr << "iter: " << iter
<< ", f(x) = " << fx
<< ", t = " << t << " (tmax=" << t_max << ")"
<< ", eps = [Newton decrement] = " << newton_decrement
<< ", constraint violation = " << rhs.tail(m).norm()
<< std::endl;
// converged?
if(newton_decrement < eps_ || t == 0.0)
break;
++iter;
}
// store result
_problem->store_result(x.data());
double solution_time = sw.stop();
std::cerr << "Newton Method finished in " << solution_time/1000.0 << "s" << std::endl;
// return success
return 1;
}
protected:
void factorize(NProblemInterface* _problem, const SMatrixD& _A, const VectorD& _b, const VectorD& _x, double& _regularize)
{
const int n = _problem->n_unknowns();
const int m = _A.rows();
const int nf = n+m;
// get hessian of quadratic problem
SMatrixD H(n,n);
_problem->eval_hessian(_x.data(), H);
// set up KKT matrix
// create sparse matrix
std::vector< Triplet > trips;
trips.reserve(H.nonZeros() + 2*_A.nonZeros());
// add elements of H
for (int k=0; k<H.outerSize(); ++k)
for (SMatrixD::InnerIterator it(H,k); it; ++it)
trips.push_back(Triplet(it.row(),it.col(),it.value()));
// add elements of _A
for (int k=0; k<_A.outerSize(); ++k)
for (SMatrixD::InnerIterator it(_A,k); it; ++it)
{
// insert _A block below
trips.push_back(Triplet(it.row()+n,it.col(),it.value()));
// insert _A^T block right
trips.push_back(Triplet(it.col(),it.row()+n,it.value()));
}
if(_regularize != 0.0)
for( int i=0; i<m; ++i)
trips.push_back(Triplet(n+i,n+i,_regularize));
// create KKT matrix
SMatrixD KKT(nf,nf);
KKT.setFromTriplets(trips.begin(), trips.end());
// compute LU factorization
lu_solver_.compute(KKT);
if(lu_solver_.info() != Eigen::Success)
{
// DEB_line(2, "Eigen::SparseLU reported problem: " << lu_solver_.lastErrorMessage());
// DEB_line(2, "-> re-try with regularized constraints...");
std::cerr << "Eigen::SparseLU reported problem while factoring KKT system: " << lu_solver_.lastErrorMessage() << std::endl;
//#if COMISO_SUITESPARSE_AVAILABLE
// std::cerr << "Eigen::SparseLU reported problem while factoring KKT system. " << std::endl;
//#else
// std::cerr << "Eigen::SparseLU reported problem while factoring KKT system: " << lu_solver_.lastErrorMessage() << std::endl;
//#endif
// add more regularization
if(_regularize == 0.0)
_regularize = 1e-8;
else
_regularize *= 2.0;
for( int i=0; i<m; ++i)
trips.push_back(Triplet(n+i,n+i,_regularize));
// create KKT matrix
KKT.setFromTriplets(trips.begin(), trips.end());
// compute LU factorization
lu_solver_.compute(KKT);
// IGM_THROW_if(lu_solver_.info() != Eigen::Success, QGP_BOUNDED_DISTORTION_FAILURE);
}
}
double backtracking_line_search(NProblemInterface* _problem, VectorD& _x, VectorD& _g, VectorD& _dx, double& _newton_decrement, double& _fx, const double _t_start = 1.0)
{
int n = _x.size();
// pre-compute objective
double fx = _problem->eval_f(_x.data());
// pre-compute dot product
double gtdx = _g.transpose()*_dx.head(n);
_newton_decrement = std::abs(gtdx);
// current step size
double t = _t_start;
// backtracking (stable in case of NAN and with max 100 iterations)
for(int i=0; i<100; ++i)
{
// current update
x_ls_ = _x + _dx.head(n)*t;
double fx_ls = _problem->eval_f(x_ls_.data());
if( fx_ls <= fx + alpha_ls_*t*gtdx )
{
_fx = fx_ls;
return t;
}
else
t *= beta_ls_;
}
std::cerr << "Warning: line search could not find a valid step within 100 iterations..." << std::endl;
_fx = fx;
return 0.0;
}
// deprecated function!
// solve
int solve(NProblemGmmInterface* _problem);
// deprecated function!
// solve specifying parameters
int solve(NProblemGmmInterface* _problem, int _max_iter, double _eps)
{
max_iter_ = _max_iter;
convergence_eps_ = _eps;
max_iters_ = _max_iter;
eps_ = _eps;
return solve(_problem);
}
// deprecated function!
bool& constant_hessian_structure() { return constant_hessian_structure_; }
protected:
......@@ -66,8 +274,25 @@ protected:
}
private:
int max_iter_;
double convergence_eps_;
double eps_;
int max_iters_;
// double accelerate_;
double alpha_ls_;
double beta_ls_;
VectorD x_ls_;
// Sparse LU decomposition
Eigen::SparseLU<SMatrixD>