12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.

release/4.3a0
Alex Cunningham 2011-02-10 16:01:29 +00:00
parent 06b08c6f85
commit 7407843214
3 changed files with 327 additions and 543 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

190
gtsam/nonlinear/NonlinearOptimizer-inl.h
View File

@ -31,8 +31,6 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class C, class L, class S, class W> template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph, NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
@ -47,50 +45,37 @@ namespace gtsam {
"NonlinearOptimizer constructor: ordering = NULL"); "NonlinearOptimizer constructor: ordering = NULL");
} }
/* ************************************************************************* */
template<class G, class C, class L, class S, class W> template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer( NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
shared_graph graph, shared_values values, shared_ordering ordering,
shared_values values, shared_solver spcg_solver, shared_parameters parameters) :
shared_ordering ordering, graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering),
shared_solver solver, parameters_(parameters), iterations_(0),
shared_parameters parameters): dimensions_(new vector<size_t>(values->dims(*ordering))),
graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering), solver_(solver), spcg_solver_(spcg_solver) {
parameters_(parameters), iterations_(0), dimensions_(new vector<size_t>(values->dims(*ordering))) {
if (!graph) throw std::invalid_argument( if (!graph) throw std::invalid_argument(
"NonlinearOptimizer constructor: graph = NULL"); "NonlinearOptimizer constructor: graph = NULL");
if (!values) throw std::invalid_argument( if (!values) throw std::invalid_argument(
"NonlinearOptimizer constructor: values = NULL"); "NonlinearOptimizer constructor: values = NULL");
if (!ordering) throw std::invalid_argument( if (!spcg_solver) throw std::invalid_argument(
"NonlinearOptimizer constructor: ordering = NULL"); "NonlinearOptimizer constructor: spcg_solver = 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();
} }
/* ************************************************************************* */ /* ************************************************************************* */
// One iteration of Gauss Newton // One iteration of Gauss Newton
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class C, class L, class S, class W> 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 { NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterate() const {
Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; 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); // FIXME: allow for passing variable index through createSolver()
else newSolver.reset(new S(*linearized)); // 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 // maybe show output
if (verbosity >= Parameters::DELTA) delta.print("delta"); if (verbosity >= Parameters::DELTA) delta.print("delta");
@ -101,14 +86,13 @@ namespace gtsam {
// maybe show output // maybe show output
if (verbosity >= Parameters::VALUES) newValues->print("newValues"); 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; if (verbosity >= Parameters::ERROR) cout << "error: " << newOptimizer.error_ << endl;
return newOptimizer; return newOptimizer;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class C, class L, class S, class W> 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 { NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::gaussNewton() const {
static W writer(error_); static W writer(error_);
@ -157,6 +141,7 @@ namespace gtsam {
if (verbosity >= Parameters::TRYLAMBDA) cout << "trying lambda = " << lambda << endl; if (verbosity >= Parameters::TRYLAMBDA) cout << "trying lambda = " << lambda << endl;
// add prior-factors // add prior-factors
// TODO: replace this dampening with a backsubstitution approach
typename L::shared_ptr damped(new L(linear)); typename L::shared_ptr damped(new L(linear));
{ {
double sigma = 1.0 / sqrt(lambda); double sigma = 1.0 / sqrt(lambda);
@ -174,14 +159,15 @@ namespace gtsam {
if (verbosity >= Parameters::DAMPED) damped->print("damped"); if (verbosity >= Parameters::DAMPED) damped->print("damped");
// solve // solve
if(solver_) solver_->replaceFactors(damped); // FIXME: incorporate variable index
else solver_.reset(new S(*damped)); // FIXME: remove spcg specific code
if (spcg_solver_) spcg_solver_->replaceFactors(damped);
VectorValues delta = *solver_->optimize(); shared_solver solver = (spcg_solver_) ? spcg_solver_ : shared_solver(new S(*damped));
VectorValues delta = *solver->optimize();
if (verbosity >= Parameters::TRYDELTA) delta.print("delta"); if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
// update values // 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 // create new optimization state with more adventurous lambda
double error = graph_->error(*newValues); double error = graph_->error(*newValues);
@ -212,7 +198,6 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// One iteration of Levenberg Marquardt // One iteration of Levenberg Marquardt
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class C, class L, class S, class W> template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterateLM() { NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterateLM() {
@ -273,133 +258,4 @@ namespace gtsam {
iterations_++; 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();
}
}
} }

641
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -29,359 +29,326 @@
namespace gtsam { namespace gtsam {
class NullOptimizerWriter { class NullOptimizerWriter {
public: public:
NullOptimizerWriter(double error) {} NullOptimizerWriter(double error) {}
virtual void write(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. * Constructor that does not do any computation
* 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
*
*
*/ */
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, 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), 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) {} error_(error), ordering_(ordering), parameters_(parameters), iterations_(iterations), dimensions_(dimensions) {}
/** Create a new NonlinearOptimizer with a different lambda */ /** constructors to replace specific components */
This newValuesSolver_(shared_values newValues, shared_solver newSolver) const {
return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, newSolver, parameters_, dimensions_, iterations_); }
This newValuesErrorLambda_(shared_values newValues, double newError, double newLambda) const { This newValues_(shared_values newValues) const {
return NonlinearOptimizer(graph_, newValues, newError, ordering_, solver_, parameters_->newLambda_(newLambda), dimensions_, iterations_); } return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, parameters_, dimensions_, iterations_); }
This newIterations_(int iterations) const { This newValuesErrorLambda_(shared_values newValues, double newError, double newLambda) const {
return NonlinearOptimizer(graph_, values_, error_, ordering_, solver_, parameters_, dimensions_, iterations); } return NonlinearOptimizer(graph_, newValues, newError, ordering_, parameters_->newLambda_(newLambda), dimensions_, iterations_); }
/* This newIterations_(int iterations) const {
This newLambda_(double newLambda) const { return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters_, dimensions_, iterations); }
return NonlinearOptimizer(graph_, values_, error_, ordering_, solver_, parameters_->newLambda_(newLambda), dimensions_); }
This newValuesSolver_(shared_values newValues, shared_solver newSolver) const { This newLambda_(double newLambda) const {
return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, newSolver, parameters_, dimensions_); } return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters_->newLambda_(newLambda), dimensions_, iterations_); }
This newValuesSolverLambda_(shared_values newValues, shared_solver newSolver, double newLambda) const { This newValuesLambda_(shared_values newValues, double newLambda) const {
return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, newSolver, parameters_->newLambda_(newLambda), dimensions_); } return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues), ordering_, parameters_->newLambda_(newLambda), dimensions_, iterations_); }
This newValuesErrorLambda_(shared_values newValues, double newError, double newLambda) const { This newParameters_(shared_parameters parameters) const {
return NonlinearOptimizer(graph_, newValues, newError, ordering_, solver_, parameters_->newLambda_(newLambda), dimensions_); } return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters, dimensions_, iterations_); }
public:
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));
}
};
/** /**
* Check convergence * Constructor that evaluates new error
*/ */
bool check_convergence ( NonlinearOptimizer(shared_graph graph,
double relativeErrorTreshold, shared_values values,
double absoluteErrorTreshold, shared_ordering ordering,
double errorThreshold, shared_parameters parameters = boost::make_shared<Parameters>());
double currentError, double newError, int verbosity);
bool check_convergence ( /**
const NonlinearOptimizationParameters &parameters, * Constructor that takes a solver directly - only useful for SPCG
double currentError, double newError); * 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 &parameters,
double currentError, double newError);
} // gtsam } // gtsam

39
tests/testNonlinearOptimizer.cpp
View File

@ -177,45 +177,6 @@ TEST( NonlinearOptimizer, optimize )
DOUBLES_EQUAL(0,fg->error(*(actual2.values())),tol); 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 ) TEST( NonlinearOptimizer, SimpleLMOptimizer )
{ {