Reworked nonlinear optimizer so that it only uses a solver member variable if you are using spcg. SPCG may be broken at this point, and its member variable will be removed soon.
Alex Cunningham
parent
06b08c6f85
commit
7407843214
|
|
@ -31,8 +31,6 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class G, class C, class L, class S, class W>
|
||||
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
|
||||
|
|
@ -47,50 +45,37 @@ namespace gtsam {
|
|||
"NonlinearOptimizer constructor: ordering = NULL");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class G, class C, class L, class S, class W>
|
||||
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(
|
||||
shared_graph graph,
|
||||
shared_values values,
|
||||
shared_ordering ordering,
|
||||
shared_solver solver,
|
||||
shared_parameters parameters):
|
||||
graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering), solver_(solver),
|
||||
parameters_(parameters), iterations_(0), dimensions_(new vector<size_t>(values->dims(*ordering))) {
|
||||
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
|
||||
shared_values values, shared_ordering ordering,
|
||||
shared_solver spcg_solver, shared_parameters parameters) :
|
||||
graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering),
|
||||
parameters_(parameters), iterations_(0),
|
||||
dimensions_(new vector<size_t>(values->dims(*ordering))),
|
||||
spcg_solver_(spcg_solver) {
|
||||
if (!graph) throw std::invalid_argument(
|
||||
"NonlinearOptimizer constructor: graph = NULL");
|
||||
if (!values) throw std::invalid_argument(
|
||||
"NonlinearOptimizer constructor: values = NULL");
|
||||
if (!ordering) throw std::invalid_argument(
|
||||
"NonlinearOptimizer constructor: ordering = NULL");
|
||||
if (!solver) throw std::invalid_argument(
|
||||
"NonlinearOptimizer constructor: solver = NULL");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// linearize and optimize
|
||||
/* ************************************************************************* */
|
||||
template<class G, class C, class L, class S, class W>
|
||||
VectorValues NonlinearOptimizer<G, C, L, S, W>::linearizeAndOptimizeForDelta() const {
|
||||
boost::shared_ptr<L> linearized = graph_->linearize(*values_, *ordering_)->template dynamicCastFactors<L>();
|
||||
// NonlinearOptimizer prepared(graph_, values_, ordering_, error_, lambda_);
|
||||
return *S(*linearized).optimize();
|
||||
if (!spcg_solver) throw std::invalid_argument(
|
||||
"NonlinearOptimizer constructor: spcg_solver = NULL");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// One iteration of Gauss Newton
|
||||
/* ************************************************************************* */
|
||||
|
||||
template<class G, class C, class L, class S, class W>
|
||||
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterate() const {
|
||||
|
||||
Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
|
||||
boost::shared_ptr<L> linearized = graph_->linearize(*values_, *ordering_)->template dynamicCastFactors<L>();
|
||||
shared_solver newSolver = solver_;
|
||||
|
||||
if(newSolver) newSolver->replaceFactors(linearized);
|
||||
else newSolver.reset(new S(*linearized));
|
||||
// FIXME: allow for passing variable index through createSolver()
|
||||
// FIXME: get rid of spcg solver
|
||||
if (spcg_solver_) spcg_solver_->replaceFactors(linearize());
|
||||
shared_solver solver = (spcg_solver_) ? spcg_solver_ : createSolver();
|
||||
|
||||
VectorValues delta = *newSolver->optimize();
|
||||
VectorValues delta = *solver->optimize();
|
||||
|
||||
// maybe show output
|
||||
if (verbosity >= Parameters::DELTA) delta.print("delta");
|
||||
|
|
@ -101,14 +86,13 @@ namespace gtsam {
|
|||
// maybe show output
|
||||
if (verbosity >= Parameters::VALUES) newValues->print("newValues");
|
||||
|
||||
NonlinearOptimizer newOptimizer = newValuesSolver_(newValues, newSolver);
|
||||
NonlinearOptimizer newOptimizer = newValues_(newValues);
|
||||
|
||||
if (verbosity >= Parameters::ERROR) cout << "error: " << newOptimizer.error_ << endl;
|
||||
return newOptimizer;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
template<class G, class C, class L, class S, class W>
|
||||
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::gaussNewton() const {
|
||||
static W writer(error_);
|
||||
|
|
@ -157,6 +141,7 @@ namespace gtsam {
|
|||
if (verbosity >= Parameters::TRYLAMBDA) cout << "trying lambda = " << lambda << endl;
|
||||
|
||||
// add prior-factors
|
||||
// TODO: replace this dampening with a backsubstitution approach
|
||||
typename L::shared_ptr damped(new L(linear));
|
||||
{
|
||||
double sigma = 1.0 / sqrt(lambda);
|
||||
|
|
@ -174,14 +159,15 @@ namespace gtsam {
|
|||
if (verbosity >= Parameters::DAMPED) damped->print("damped");
|
||||
|
||||
// solve
|
||||
if(solver_) solver_->replaceFactors(damped);
|
||||
else solver_.reset(new S(*damped));
|
||||
|
||||
VectorValues delta = *solver_->optimize();
|
||||
// FIXME: incorporate variable index
|
||||
// FIXME: remove spcg specific code
|
||||
if (spcg_solver_) spcg_solver_->replaceFactors(damped);
|
||||
shared_solver solver = (spcg_solver_) ? spcg_solver_ : shared_solver(new S(*damped));
|
||||
VectorValues delta = *solver->optimize();
|
||||
if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
|
||||
|
||||
// update values
|
||||
shared_values newValues(new C(values_->expmap(delta, *ordering_))); // TODO: updateValues
|
||||
shared_values newValues(new C(values_->expmap(delta, *ordering_)));
|
||||
|
||||
// create new optimization state with more adventurous lambda
|
||||
double error = graph_->error(*newValues);
|
||||
|
|
@ -212,7 +198,6 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
// One iteration of Levenberg Marquardt
|
||||
/* ************************************************************************* */
|
||||
|
||||
template<class G, class C, class L, class S, class W>
|
||||
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterateLM() {
|
||||
|
||||
|
|
@ -273,133 +258,4 @@ namespace gtsam {
|
|||
iterations_++;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// New version of functional approach
|
||||
/* ************************************************************************* */
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class G, class C, class L, class S, class W>
|
||||
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::try_lambdaRecursive(const L& linear) const {
|
||||
|
||||
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
|
||||
double lambda = parameters_->lambda_ ;
|
||||
const Parameters::LambdaMode lambdaMode = parameters_->lambdaMode_ ;
|
||||
const double factor = parameters_->lambdaFactor_ ;
|
||||
|
||||
if( lambdaMode >= Parameters::CAUTIOUS) throw runtime_error("CAUTIOUS mode not working yet, please use BOUNDED.");
|
||||
|
||||
double next_error = error_;
|
||||
|
||||
shared_values next_values = values_;
|
||||
shared_solver solver = solver_;
|
||||
|
||||
while(true) {
|
||||
if (verbosity >= Parameters::TRYLAMBDA) cout << "trying lambda = " << lambda << endl;
|
||||
|
||||
// add prior-factors
|
||||
typename L::shared_ptr damped(new L(linear));
|
||||
{
|
||||
double sigma = 1.0 / sqrt(lambda);
|
||||
damped->reserve(damped->size() + dimensions_->size());
|
||||
// for each of the variables, add a prior
|
||||
for(Index j=0; j<dimensions_->size(); ++j) {
|
||||
size_t dim = (*dimensions_)[j];
|
||||
Matrix A = eye(dim);
|
||||
Vector b = zero(dim);
|
||||
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma);
|
||||
GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model));
|
||||
damped->push_back(prior);
|
||||
}
|
||||
}
|
||||
if (verbosity >= Parameters::DAMPED) damped->print("damped");
|
||||
|
||||
// solve
|
||||
solver.reset(new S(*damped));
|
||||
|
||||
VectorValues delta = *solver->optimize();
|
||||
if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
|
||||
|
||||
// update values
|
||||
shared_values newValues(new C(values_->expmap(delta, *ordering_))); // TODO: updateValues
|
||||
|
||||
// create new optimization state with more adventurous lambda
|
||||
double error = graph_->error(*newValues);
|
||||
|
||||
if (verbosity >= Parameters::TRYLAMBDA) cout << "next error = " << error << endl;
|
||||
|
||||
if( error <= error_ ) {
|
||||
next_values = newValues;
|
||||
next_error = error;
|
||||
lambda /= factor;
|
||||
break;
|
||||
} else {
|
||||
// Either we're not cautious, or the same lambda was worse than the current error.
|
||||
// The more adventurous lambda was worse too, so make lambda more conservative
|
||||
// and keep the same values.
|
||||
if(lambdaMode >= Parameters::BOUNDED && lambda >= 1.0e5) {
|
||||
break;
|
||||
} else {
|
||||
lambda *= factor;
|
||||
}
|
||||
}
|
||||
} // end while
|
||||
|
||||
return NonlinearOptimizer(graph_, next_values, next_error, ordering_, solver,
|
||||
parameters_->newLambda_(lambda), dimensions_, iterations_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class G, class C, class L, class S, class W>
|
||||
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterateLMRecursive() const {
|
||||
|
||||
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
|
||||
const double lambda = parameters_->lambda_ ;
|
||||
|
||||
// show output
|
||||
if (verbosity >= Parameters::VALUES) values_->print("values");
|
||||
if (verbosity >= Parameters::ERROR) cout << "error: " << error_ << endl;
|
||||
if (verbosity >= Parameters::LAMBDA) cout << "lambda = " << lambda << endl;
|
||||
|
||||
// linearize all factors once
|
||||
boost::shared_ptr<L> linear = graph_->linearize(*values_, *ordering_)->template dynamicCastFactors<L>();
|
||||
|
||||
if (verbosity >= Parameters::LINEAR) linear->print("linear");
|
||||
|
||||
// try lambda steps with successively larger lambda until we achieve descent
|
||||
if (verbosity >= Parameters::LAMBDA) cout << "Trying Lambda for the first time" << endl;
|
||||
|
||||
return try_lambdaRecursive(*linear);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class G, class C, class L, class S, class W>
|
||||
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::levenbergMarquardtRecursive() const {
|
||||
|
||||
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
|
||||
|
||||
// check if we're already close enough
|
||||
if (error_ < parameters_->sumError_) {
|
||||
if ( verbosity >= Parameters::ERROR )
|
||||
cout << "Exiting, as sumError = " << error_ << " < " << parameters_->sumError_ << endl;
|
||||
return *this;
|
||||
}
|
||||
|
||||
// do one iteration of LM
|
||||
NonlinearOptimizer next = iterateLMRecursive();
|
||||
|
||||
// check convergence
|
||||
// TODO: move convergence checks here and incorporate in verbosity levels
|
||||
bool converged = gtsam::check_convergence(*parameters_, error_, next.error_);
|
||||
|
||||
if(iterations_ >= parameters_->maxIterations_ || converged) {
|
||||
if (verbosity >= Parameters::VALUES) values_->print("final values");
|
||||
if (verbosity >= Parameters::ERROR) cout << "final error: " << error_ << endl;
|
||||
if (verbosity >= Parameters::LAMBDA) cout << "final lambda = " << lambda() << endl;
|
||||
return *this;
|
||||
} else {
|
||||
return next.newIterations_(iterations_ + 1).levenbergMarquardtRecursive();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -29,359 +29,326 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
class NullOptimizerWriter {
|
||||
public:
|
||||
NullOptimizerWriter(double error) {}
|
||||
virtual void write(double error) {}
|
||||
};
|
||||
class NullOptimizerWriter {
|
||||
public:
|
||||
NullOptimizerWriter(double error) {}
|
||||
virtual void write(double error) {}
|
||||
};
|
||||
|
||||
/**
|
||||
* The class NonlinearOptimizer encapsulates an optimization state.
|
||||
* Typically it is instantiated with a NonlinearFactorGraph and initial values
|
||||
* and then one of the optimization routines is called. These iterate
|
||||
* until convergence. All methods are functional and return a new state.
|
||||
*
|
||||
* The class is parameterized by the Graph type $G$, Values class type $T$,
|
||||
* linear system class $L$, the non linear solver type $S$, and the writer type $W$
|
||||
*
|
||||
* The values class type $T$ is in order to be able to optimize over non-vector values structures.
|
||||
*
|
||||
* A nonlinear system solver $S$
|
||||
* Concept NonLinearSolver<G,T,L> implements
|
||||
* linearize: G * T -> L
|
||||
* solve : L -> T
|
||||
*
|
||||
* The writer $W$ generates output to disk or the screen.
|
||||
*
|
||||
* For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Values,
|
||||
* $L$ can be GaussianFactorGraph and $S$ can be Factorization<Pose2Graph, Pose2Values>.
|
||||
* The solver class has two main functions: linearize and optimize. The first one
|
||||
* linearizes the nonlinear cost function around the current estimate, and the second
|
||||
* one optimizes the linearized system using various methods.
|
||||
*
|
||||
* To use the optimizer in code, include <gtsam/NonlinearOptimizer-inl.h> in your cpp file
|
||||
*
|
||||
*
|
||||
*/
|
||||
template<class G, class T, class L = GaussianFactorGraph, class GS = GaussianMultifrontalSolver, class W = NullOptimizerWriter>
|
||||
class NonlinearOptimizer {
|
||||
public:
|
||||
|
||||
// For performance reasons in recursion, we store values in a shared_ptr
|
||||
typedef boost::shared_ptr<const T> shared_values;
|
||||
typedef boost::shared_ptr<const G> shared_graph;
|
||||
typedef boost::shared_ptr<L> shared_linear;
|
||||
typedef boost::shared_ptr<const Ordering> shared_ordering;
|
||||
typedef boost::shared_ptr<GS> shared_solver;
|
||||
typedef NonlinearOptimizationParameters Parameters;
|
||||
typedef boost::shared_ptr<const Parameters> shared_parameters ;
|
||||
|
||||
private:
|
||||
|
||||
typedef NonlinearOptimizer<G, T, L, GS> This;
|
||||
typedef boost::shared_ptr<const std::vector<size_t> > shared_dimensions;
|
||||
|
||||
// keep a reference to const version of the graph
|
||||
// These normally do not change
|
||||
const shared_graph graph_;
|
||||
|
||||
// keep a values structure and its error
|
||||
// These typically change once per iteration (in a functional way)
|
||||
shared_values values_;
|
||||
|
||||
// current error for this state
|
||||
double error_;
|
||||
|
||||
// the variable ordering
|
||||
const shared_ordering ordering_;
|
||||
|
||||
// storage for parameters, lambda, thresholds, etc.
|
||||
shared_parameters parameters_;
|
||||
|
||||
// for performance track
|
||||
size_t iterations_;
|
||||
|
||||
// The dimensions of each linearized variable
|
||||
const shared_dimensions dimensions_;
|
||||
|
||||
// solver used only for SPCG
|
||||
// FIXME: remove this!
|
||||
shared_solver spcg_solver_;
|
||||
|
||||
/**
|
||||
* The class NonlinearOptimizer encapsulates an optimization state.
|
||||
* Typically it is instantiated with a NonlinearFactorGraph and an initial values
|
||||
* and then one of the optimization routines is called. These recursively iterate
|
||||
* until convergence. All methods are functional and return a new state.
|
||||
*
|
||||
* The class is parameterized by the Graph type $G$, Values class type $T$,
|
||||
* linear system class $L$, the non linear solver type $S$, and the writer type $W$
|
||||
*
|
||||
* The values class type $T$ is in order to be able to optimize over non-vector values structures.
|
||||
*
|
||||
* A nonlinear system solver $S$
|
||||
* Concept NonLinearSolver<G,T,L> implements
|
||||
* linearize: G * T -> L
|
||||
* solve : L -> T
|
||||
*
|
||||
* The writer $W$ generates output to disk or the screen.
|
||||
*
|
||||
* For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Values,
|
||||
* $L$ can be GaussianFactorGraph and $S$ can be Factorization<Pose2Graph, Pose2Values>.
|
||||
* The solver class has two main functions: linearize and optimize. The first one
|
||||
* linearizes the nonlinear cost function around the current estimate, and the second
|
||||
* one optimizes the linearized system using various methods.
|
||||
*
|
||||
* To use the optimizer in code, include <gtsam/NonlinearOptimizer-inl.h> in your cpp file
|
||||
*
|
||||
*
|
||||
* Constructor that does not do any computation
|
||||
*/
|
||||
template<class G, class T, class L = GaussianFactorGraph, class GS = GaussianMultifrontalSolver, class W = NullOptimizerWriter>
|
||||
class NonlinearOptimizer {
|
||||
public:
|
||||
|
||||
// For performance reasons in recursion, we store values in a shared_ptr
|
||||
typedef boost::shared_ptr<const T> shared_values;
|
||||
typedef boost::shared_ptr<const G> shared_graph;
|
||||
typedef boost::shared_ptr<const Ordering> shared_ordering;
|
||||
typedef boost::shared_ptr<GS> shared_solver;
|
||||
typedef NonlinearOptimizationParameters Parameters;
|
||||
typedef boost::shared_ptr<const Parameters> shared_parameters ;
|
||||
|
||||
private:
|
||||
|
||||
typedef NonlinearOptimizer<G, T, L, GS> This;
|
||||
typedef boost::shared_ptr<const std::vector<size_t> > shared_dimensions;
|
||||
|
||||
// keep a reference to const version of the graph
|
||||
// These normally do not change
|
||||
const shared_graph graph_;
|
||||
|
||||
// keep a values structure and its error
|
||||
// These typically change once per iteration (in a functional way)
|
||||
shared_values values_;
|
||||
double error_;
|
||||
|
||||
// the variable ordering
|
||||
const shared_ordering ordering_;
|
||||
|
||||
// the linear system solver
|
||||
shared_solver solver_;
|
||||
|
||||
shared_parameters parameters_;
|
||||
|
||||
// for performance track
|
||||
size_t iterations_;
|
||||
|
||||
// // keep current lambda for use within LM only
|
||||
// // TODO: red flag, should we have an LM class ?
|
||||
// const double lambda_;
|
||||
|
||||
// The dimensions of each linearized variable
|
||||
const shared_dimensions dimensions_;
|
||||
|
||||
/**
|
||||
* Constructor that does not do any computation
|
||||
*/
|
||||
// NonlinearOptimizer(shared_graph graph, shared_values values, const double error, shared_ordering ordering,
|
||||
// shared_solver solver, const double lambda, shared_dimensions dimensions): graph_(graph), values_(values),
|
||||
// error_(error), ordering_(ordering), solver_(solver), lambda_(lambda), dimensions_(dimensions) {}
|
||||
|
||||
NonlinearOptimizer(shared_graph graph, shared_values values, const double error, shared_ordering ordering,
|
||||
shared_solver solver, shared_parameters parameters, shared_dimensions dimensions, size_t iterations): graph_(graph), values_(values),
|
||||
error_(error), ordering_(ordering), solver_(solver), parameters_(parameters), iterations_(iterations), dimensions_(dimensions) {}
|
||||
shared_parameters parameters, shared_dimensions dimensions, size_t iterations): graph_(graph), values_(values),
|
||||
error_(error), ordering_(ordering), parameters_(parameters), iterations_(iterations), dimensions_(dimensions) {}
|
||||
|
||||
/** Create a new NonlinearOptimizer with a different lambda */
|
||||
This newValuesSolver_(shared_values newValues, shared_solver newSolver) const {
|
||||
return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, newSolver, parameters_, dimensions_, iterations_); }
|
||||
/** constructors to replace specific components */
|
||||
|
||||
This newValuesErrorLambda_(shared_values newValues, double newError, double newLambda) const {
|
||||
return NonlinearOptimizer(graph_, newValues, newError, ordering_, solver_, parameters_->newLambda_(newLambda), dimensions_, iterations_); }
|
||||
This newValues_(shared_values newValues) const {
|
||||
return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, parameters_, dimensions_, iterations_); }
|
||||
|
||||
This newIterations_(int iterations) const {
|
||||
return NonlinearOptimizer(graph_, values_, error_, ordering_, solver_, parameters_, dimensions_, iterations); }
|
||||
This newValuesErrorLambda_(shared_values newValues, double newError, double newLambda) const {
|
||||
return NonlinearOptimizer(graph_, newValues, newError, ordering_, parameters_->newLambda_(newLambda), dimensions_, iterations_); }
|
||||
|
||||
/*
|
||||
This newLambda_(double newLambda) const {
|
||||
return NonlinearOptimizer(graph_, values_, error_, ordering_, solver_, parameters_->newLambda_(newLambda), dimensions_); }
|
||||
This newIterations_(int iterations) const {
|
||||
return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters_, dimensions_, iterations); }
|
||||
|
||||
This newValuesSolver_(shared_values newValues, shared_solver newSolver) const {
|
||||
return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, newSolver, parameters_, dimensions_); }
|
||||
This newLambda_(double newLambda) const {
|
||||
return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters_->newLambda_(newLambda), dimensions_, iterations_); }
|
||||
|
||||
This newValuesSolverLambda_(shared_values newValues, shared_solver newSolver, double newLambda) const {
|
||||
return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, newSolver, parameters_->newLambda_(newLambda), dimensions_); }
|
||||
This newValuesLambda_(shared_values newValues, double newLambda) const {
|
||||
return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, parameters_->newLambda_(newLambda), dimensions_, iterations_); }
|
||||
|
||||
This newValuesErrorLambda_(shared_values newValues, double newError, double newLambda) const {
|
||||
return NonlinearOptimizer(graph_, newValues, newError, ordering_, solver_, parameters_->newLambda_(newLambda), dimensions_); }
|
||||
This newParameters_(shared_parameters parameters) const {
|
||||
return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters, dimensions_, iterations_); }
|
||||
|
||||
|
||||
This newVerbosity_(Parameters::verbosityLevel verbosity) const {
|
||||
return NonlinearOptimizer(graph_, values_, error_, ordering_, solver_, parameters_->newVerbosity_(verbosity), dimensions_); }
|
||||
|
||||
This newParameters_(shared_parameters parameters) const {
|
||||
return NonlinearOptimizer(graph_, values_, error_, ordering_, solver_, parameters, dimensions_); }
|
||||
|
||||
This newMaxIterations_(int maxIterations) const {
|
||||
return NonlinearOptimizer(graph_, values_, error_, ordering_, solver_, parameters_->newMaxIterations_(maxIterations), dimensions_); }
|
||||
*/
|
||||
|
||||
public:
|
||||
/**
|
||||
* Constructor that evaluates new error
|
||||
*/
|
||||
|
||||
// suggested constructors
|
||||
NonlinearOptimizer(shared_graph graph,
|
||||
shared_values values,
|
||||
shared_ordering ordering,
|
||||
shared_parameters parameters = boost::make_shared<Parameters>());
|
||||
|
||||
// suggested constructors
|
||||
NonlinearOptimizer(shared_graph graph,
|
||||
shared_values values,
|
||||
shared_ordering ordering,
|
||||
shared_solver solver,
|
||||
shared_parameters parameters = boost::make_shared<Parameters>());
|
||||
|
||||
/**
|
||||
* Copy constructor
|
||||
*/
|
||||
// NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) :
|
||||
// graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_),
|
||||
// ordering_(optimizer.ordering_), solver_(optimizer.solver_), lambda_(optimizer.lambda_), dimensions_(optimizer.dimensions_) {}
|
||||
|
||||
NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) :
|
||||
graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_),
|
||||
ordering_(optimizer.ordering_), solver_(optimizer.solver_),
|
||||
parameters_(optimizer.parameters_), iterations_(0), dimensions_(optimizer.dimensions_) {}
|
||||
|
||||
/**
|
||||
* Return current error
|
||||
*/
|
||||
double error() const { return error_; }
|
||||
|
||||
/**
|
||||
* Return current lambda
|
||||
*/
|
||||
double lambda() const { return parameters_->lambda_; }
|
||||
|
||||
/**
|
||||
* Return the values
|
||||
*/
|
||||
shared_values values() const { return values_; }
|
||||
|
||||
/**
|
||||
* Return the graph
|
||||
*/
|
||||
shared_graph graph() const { return graph_; }
|
||||
|
||||
/**
|
||||
* Return the itertions
|
||||
*/
|
||||
size_t iterations() const { return iterations_; }
|
||||
|
||||
/**
|
||||
* Return the solver
|
||||
*/
|
||||
shared_solver solver() const { return solver_; }
|
||||
|
||||
/**
|
||||
* Return the ordering
|
||||
*/
|
||||
shared_ordering ordering() const { return ordering_; }
|
||||
|
||||
/**
|
||||
* Return the parameters
|
||||
*/
|
||||
shared_parameters parameters() const { return parameters_; }
|
||||
|
||||
/**
|
||||
* Return mean and covariance on a single variable
|
||||
*/
|
||||
std::pair<Vector,Matrix> marginalCovariance(Symbol j) const {
|
||||
return solver_->marginalCovariance((*ordering_)[j]);
|
||||
}
|
||||
|
||||
/**
|
||||
* linearize and optimize
|
||||
* This returns an VectorValues, i.e., vectors in tangent space of T
|
||||
*/
|
||||
VectorValues linearizeAndOptimizeForDelta() const;
|
||||
|
||||
/**
|
||||
* Do one Gauss-Newton iteration and return next state
|
||||
*/
|
||||
|
||||
// suggested interface
|
||||
NonlinearOptimizer iterate() const;
|
||||
|
||||
/**
|
||||
* Optimize a solution for a non linear factor graph
|
||||
* @param relativeTreshold
|
||||
* @param absoluteTreshold
|
||||
* @param verbosity Integer specifying how much output to provide
|
||||
*/
|
||||
|
||||
// suggested interface
|
||||
NonlinearOptimizer gaussNewton() const;
|
||||
|
||||
/** Recursively try to do tempered Gauss-Newton steps until we succeed */
|
||||
NonlinearOptimizer try_lambda(const L& linear);
|
||||
|
||||
/**
|
||||
* One iteration of Levenberg Marquardt
|
||||
*/
|
||||
|
||||
// suggested interface
|
||||
NonlinearOptimizer iterateLM();
|
||||
|
||||
/**
|
||||
* Optimize using Levenberg-Marquardt. Really Levenberg's
|
||||
* algorithm at this moment, as we just add I*\lambda to Hessian
|
||||
* H'H. The probabilistic explanation is very simple: every
|
||||
* variable gets an extra Gaussian prior that biases staying at
|
||||
* current value, with variance 1/lambda. This is done very easily
|
||||
* (but perhaps wastefully) by adding a prior factor for each of
|
||||
* the variables, after linearization.
|
||||
*
|
||||
* @param relativeThreshold
|
||||
* @param absoluteThreshold
|
||||
* @param verbosity Integer specifying how much output to provide
|
||||
* @param lambdaFactor Factor by which to decrease/increase lambda
|
||||
*/
|
||||
|
||||
// suggested interface
|
||||
NonlinearOptimizer levenbergMarquardt();
|
||||
|
||||
/**
|
||||
* The following are const versions of the LM algorithm for comparison and
|
||||
* testing - functions are largely identical, but maintain constness
|
||||
*/
|
||||
NonlinearOptimizer try_lambdaRecursive(const L& linear) const;
|
||||
NonlinearOptimizer iterateLMRecursive() const;
|
||||
NonlinearOptimizer levenbergMarquardtRecursive() const;
|
||||
|
||||
/**
|
||||
* Static interface to LM optimization using default ordering and thresholds
|
||||
* @param graph Nonlinear factor graph to optimize
|
||||
* @param values Initial values
|
||||
* @param verbosity Integer specifying how much output to provide
|
||||
* @return an optimized values structure
|
||||
*/
|
||||
|
||||
static shared_values optimizeLM(shared_graph graph,
|
||||
shared_values values,
|
||||
shared_parameters parameters = boost::make_shared<Parameters>()) {
|
||||
|
||||
// Use a variable ordering from COLAMD
|
||||
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
|
||||
// initial optimization state is the same in both cases tested
|
||||
//GS solver(*graph->linearize(*values, *ordering));
|
||||
|
||||
NonlinearOptimizer optimizer(graph, values, ordering, parameters);
|
||||
NonlinearOptimizer result = optimizer.levenbergMarquardt();
|
||||
return result.values();
|
||||
}
|
||||
|
||||
static shared_values optimizeLM(shared_graph graph,
|
||||
shared_values values,
|
||||
Parameters::verbosityLevel verbosity)
|
||||
{
|
||||
return optimizeLM(graph, values, Parameters::newVerbosity(verbosity));
|
||||
}
|
||||
/**
|
||||
* Static interface to LM optimization (no shared_ptr arguments) - see above
|
||||
*/
|
||||
|
||||
static shared_values optimizeLM(const G& graph,
|
||||
const T& values,
|
||||
const Parameters parameters = Parameters()) {
|
||||
return optimizeLM(boost::make_shared<const G>(graph),
|
||||
boost::make_shared<const T>(values),
|
||||
boost::make_shared<Parameters>(parameters));
|
||||
}
|
||||
|
||||
static shared_values optimizeLM(const G& graph,
|
||||
const T& values,
|
||||
Parameters::verbosityLevel verbosity) {
|
||||
return optimizeLM(boost::make_shared<const G>(graph),
|
||||
boost::make_shared<const T>(values),
|
||||
verbosity);
|
||||
}
|
||||
|
||||
/**
|
||||
* Static interface to GN optimization using default ordering and thresholds
|
||||
* @param graph Nonlinear factor graph to optimize
|
||||
* @param values Initial values
|
||||
* @param verbosity Integer specifying how much output to provide
|
||||
* @return an optimized values structure
|
||||
*/
|
||||
|
||||
// suggested interface
|
||||
static shared_values optimizeGN(shared_graph graph,
|
||||
shared_values values,
|
||||
shared_parameters parameters = boost::make_shared<Parameters>()) {
|
||||
|
||||
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
|
||||
// initial optimization state is the same in both cases tested
|
||||
GS solver(*graph->linearize(*values, *ordering));
|
||||
|
||||
NonlinearOptimizer optimizer(graph, values, ordering, parameters);
|
||||
NonlinearOptimizer result = optimizer.gaussNewton();
|
||||
return result.values();
|
||||
}
|
||||
|
||||
/**
|
||||
* Static interface to GN optimization (no shared_ptr arguments) - see above
|
||||
*/
|
||||
|
||||
// suggested interface
|
||||
static shared_values optimizeGN(const G& graph, const T& values, const Parameters parameters = Parameters()) {
|
||||
return optimizeGN(boost::make_shared<const G>(graph),
|
||||
boost::make_shared<const T>(values),
|
||||
boost::make_shared<Parameters>(parameters));
|
||||
}
|
||||
};
|
||||
public:
|
||||
|
||||
/**
|
||||
* Check convergence
|
||||
* Constructor that evaluates new error
|
||||
*/
|
||||
bool check_convergence (
|
||||
double relativeErrorTreshold,
|
||||
double absoluteErrorTreshold,
|
||||
double errorThreshold,
|
||||
double currentError, double newError, int verbosity);
|
||||
NonlinearOptimizer(shared_graph graph,
|
||||
shared_values values,
|
||||
shared_ordering ordering,
|
||||
shared_parameters parameters = boost::make_shared<Parameters>());
|
||||
|
||||
bool check_convergence (
|
||||
const NonlinearOptimizationParameters ¶meters,
|
||||
double currentError, double newError);
|
||||
/**
|
||||
* Constructor that takes a solver directly - only useful for SPCG
|
||||
* FIXME: REMOVE THIS FUNCTION!
|
||||
*/
|
||||
NonlinearOptimizer(shared_graph graph,
|
||||
shared_values values,
|
||||
shared_ordering ordering,
|
||||
shared_solver spcg_solver,
|
||||
shared_parameters parameters = boost::make_shared<Parameters>());
|
||||
|
||||
/**
|
||||
* Copy constructor
|
||||
*/
|
||||
NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) :
|
||||
graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_),
|
||||
ordering_(optimizer.ordering_), parameters_(optimizer.parameters_),
|
||||
iterations_(0), dimensions_(optimizer.dimensions_) {}
|
||||
|
||||
// access to member variables
|
||||
|
||||
/** Return current error */
|
||||
double error() const { return error_; }
|
||||
|
||||
/** Return current lambda */
|
||||
double lambda() const { return parameters_->lambda_; }
|
||||
|
||||
/** Return the values */
|
||||
shared_values values() const { return values_; }
|
||||
|
||||
/** Return the graph */
|
||||
shared_graph graph() const { return graph_; }
|
||||
|
||||
/** Return the itertions */
|
||||
size_t iterations() const { return iterations_; }
|
||||
|
||||
/** Return the ordering */
|
||||
shared_ordering ordering() const { return ordering_; }
|
||||
|
||||
/** Return the parameters */
|
||||
shared_parameters parameters() const { return parameters_; }
|
||||
|
||||
/**
|
||||
* Return a linearized graph at the current graph/values/ordering
|
||||
*/
|
||||
shared_linear linearize() const {
|
||||
return graph_->linearize(*values_, *ordering_)->template dynamicCastFactors<L>();
|
||||
}
|
||||
|
||||
/**
|
||||
* create a solver around the current graph/values
|
||||
* NOTE: this will actually solve a linear system
|
||||
*/
|
||||
shared_solver createSolver() const {
|
||||
shared_linear linearGraph = linearize();
|
||||
shared_solver solver(new GS(*linearGraph));
|
||||
return solver;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return mean and covariance on a single variable
|
||||
*/
|
||||
std::pair<Vector,Matrix> marginalCovariance(Symbol j) const {
|
||||
return createSolver()->marginalCovariance((*ordering_)[j]);
|
||||
}
|
||||
|
||||
/**
|
||||
* linearize and optimize
|
||||
* This returns an VectorValues, i.e., vectors in tangent space of T
|
||||
*/
|
||||
VectorValues linearizeAndOptimizeForDelta() const {
|
||||
return *createSolver()->optimize();
|
||||
}
|
||||
|
||||
/**
|
||||
* Do one Gauss-Newton iteration and return next state
|
||||
* suggested interface
|
||||
*/
|
||||
NonlinearOptimizer iterate() const;
|
||||
|
||||
/**
|
||||
* Optimize a solution for a non linear factor graph
|
||||
* @param relativeTreshold
|
||||
* @param absoluteTreshold
|
||||
* @param verbosity Integer specifying how much output to provide
|
||||
*/
|
||||
|
||||
// suggested interface
|
||||
NonlinearOptimizer gaussNewton() const;
|
||||
|
||||
/** Recursively try to do tempered Gauss-Newton steps until we succeed */
|
||||
NonlinearOptimizer try_lambda(const L& linear);
|
||||
|
||||
/**
|
||||
* One iteration of Levenberg Marquardt
|
||||
*/
|
||||
NonlinearOptimizer iterateLM();
|
||||
|
||||
/**
|
||||
* Optimize using Levenberg-Marquardt. Really Levenberg's
|
||||
* algorithm at this moment, as we just add I*\lambda to Hessian
|
||||
* H'H. The probabilistic explanation is very simple: every
|
||||
* variable gets an extra Gaussian prior that biases staying at
|
||||
* current value, with variance 1/lambda. This is done very easily
|
||||
* (but perhaps wastefully) by adding a prior factor for each of
|
||||
* the variables, after linearization.
|
||||
*
|
||||
* @param relativeThreshold
|
||||
* @param absoluteThreshold
|
||||
* @param verbosity Integer specifying how much output to provide
|
||||
* @param lambdaFactor Factor by which to decrease/increase lambda
|
||||
*/
|
||||
NonlinearOptimizer levenbergMarquardt();
|
||||
|
||||
// static interfaces to LM and GN optimization techniques
|
||||
|
||||
/**
|
||||
* Static interface to LM optimization using default ordering and thresholds
|
||||
* @param graph Nonlinear factor graph to optimize
|
||||
* @param values Initial values
|
||||
* @param verbosity Integer specifying how much output to provide
|
||||
* @return an optimized values structure
|
||||
*/
|
||||
static shared_values optimizeLM(shared_graph graph,
|
||||
shared_values values,
|
||||
shared_parameters parameters = boost::make_shared<Parameters>()) {
|
||||
|
||||
// Use a variable ordering from COLAMD
|
||||
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
|
||||
// initial optimization state is the same in both cases tested
|
||||
//GS solver(*graph->linearize(*values, *ordering));
|
||||
|
||||
NonlinearOptimizer optimizer(graph, values, ordering, parameters);
|
||||
NonlinearOptimizer result = optimizer.levenbergMarquardt();
|
||||
return result.values();
|
||||
}
|
||||
|
||||
static shared_values optimizeLM(shared_graph graph,
|
||||
shared_values values,
|
||||
Parameters::verbosityLevel verbosity)
|
||||
{
|
||||
return optimizeLM(graph, values, Parameters::newVerbosity(verbosity));
|
||||
}
|
||||
|
||||
/**
|
||||
* Static interface to LM optimization (no shared_ptr arguments) - see above
|
||||
*/
|
||||
static shared_values optimizeLM(const G& graph,
|
||||
const T& values,
|
||||
const Parameters parameters = Parameters()) {
|
||||
return optimizeLM(boost::make_shared<const G>(graph),
|
||||
boost::make_shared<const T>(values),
|
||||
boost::make_shared<Parameters>(parameters));
|
||||
}
|
||||
|
||||
static shared_values optimizeLM(const G& graph,
|
||||
const T& values,
|
||||
Parameters::verbosityLevel verbosity) {
|
||||
return optimizeLM(boost::make_shared<const G>(graph),
|
||||
boost::make_shared<const T>(values),
|
||||
verbosity);
|
||||
}
|
||||
|
||||
/**
|
||||
* Static interface to GN optimization using default ordering and thresholds
|
||||
* @param graph Nonlinear factor graph to optimize
|
||||
* @param values Initial values
|
||||
* @param verbosity Integer specifying how much output to provide
|
||||
* @return an optimized values structure
|
||||
*/
|
||||
static shared_values optimizeGN(shared_graph graph,
|
||||
shared_values values,
|
||||
shared_parameters parameters = boost::make_shared<Parameters>()) {
|
||||
|
||||
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
|
||||
// initial optimization state is the same in both cases tested
|
||||
GS solver(*graph->linearize(*values, *ordering));
|
||||
|
||||
NonlinearOptimizer optimizer(graph, values, ordering, parameters);
|
||||
NonlinearOptimizer result = optimizer.gaussNewton();
|
||||
return result.values();
|
||||
}
|
||||
|
||||
/**
|
||||
* Static interface to GN optimization (no shared_ptr arguments) - see above
|
||||
*/
|
||||
static shared_values optimizeGN(const G& graph, const T& values, const Parameters parameters = Parameters()) {
|
||||
return optimizeGN(boost::make_shared<const G>(graph),
|
||||
boost::make_shared<const T>(values),
|
||||
boost::make_shared<Parameters>(parameters));
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Check convergence
|
||||
*/
|
||||
bool check_convergence (
|
||||
double relativeErrorTreshold,
|
||||
double absoluteErrorTreshold,
|
||||
double errorThreshold,
|
||||
double currentError, double newError, int verbosity);
|
||||
|
||||
bool check_convergence (
|
||||
const NonlinearOptimizationParameters ¶meters,
|
||||
double currentError, double newError);
|
||||
|
||||
} // gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -177,45 +177,6 @@ TEST( NonlinearOptimizer, optimize )
|
|||
DOUBLES_EQUAL(0,fg->error(*(actual2.values())),tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearOptimizer, optimize_LM_recursive )
|
||||
{
|
||||
shared_ptr<example::Graph> fg(new example::Graph(
|
||||
example::createReallyNonlinearFactorGraph()));
|
||||
|
||||
// test error at minimum
|
||||
Point2 xstar(0,0);
|
||||
example::Values cstar;
|
||||
cstar.insert(simulated2D::PoseKey(1), xstar);
|
||||
EXPECT_DOUBLES_EQUAL(0.0,fg->error(cstar),0.0);
|
||||
|
||||
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
|
||||
Point2 x0(3,3);
|
||||
boost::shared_ptr<example::Values> c0(new example::Values);
|
||||
c0->insert(simulated2D::PoseKey(1), x0);
|
||||
EXPECT_DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);
|
||||
|
||||
// optimize parameters
|
||||
shared_ptr<Ordering> ord(new Ordering());
|
||||
ord->push_back("x1");
|
||||
|
||||
// initial optimization state is the same in both cases tested
|
||||
boost::shared_ptr<NonlinearOptimizationParameters> params = boost::make_shared<NonlinearOptimizationParameters>();
|
||||
params->relDecrease_ = 1e-5;
|
||||
params->absDecrease_ = 1e-5;
|
||||
Optimizer optimizer(fg, c0, ord, params);
|
||||
|
||||
// Levenberg-Marquardt
|
||||
Optimizer actual2 = optimizer.levenbergMarquardtRecursive();
|
||||
EXPECT_DOUBLES_EQUAL(0,fg->error(*(actual2.values())),tol);
|
||||
|
||||
// calculate the marginal
|
||||
Matrix actualCovariance; Vector mean;
|
||||
boost::tie(mean, actualCovariance) = actual2.marginalCovariance("x1");
|
||||
Matrix expectedCovariance = Matrix_(2,2, 8.60817108, 0.0, 0.0, 0.01);
|
||||
EXPECT(assert_equal(expectedCovariance, actualCovariance, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( NonlinearOptimizer, SimpleLMOptimizer )
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue