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

Documentation and variable renaming for clarity

release/4.3a0
Frank Dellaert 2011-09-05 21:28:26 +00:00
parent 0e3b96db60
commit 638d4f1978
2 changed files with 30 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/nonlinear/NonlinearOptimization-inl.h
View File

@ -39,13 +39,13 @@ namespace gtsam {
// Use a variable ordering from COLAMD // Use a variable ordering from COLAMD
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate); Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
// initial optimization state is the same in both cases tested // Create an nonlinear Optimizer that uses a Sequential Solver
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianSequentialSolver> Optimizer; typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
Optimizer optimizer(boost::make_shared<const G>(graph), Optimizer optimizer(boost::make_shared<const G>(graph),
boost::make_shared<const T>(initialEstimate), ordering, boost::make_shared<const T>(initialEstimate), ordering,
boost::make_shared<NonlinearOptimizationParameters>(parameters)); boost::make_shared<NonlinearOptimizationParameters>(parameters));
// choose nonlinear optimization method // Now optimize using either LM or GN methods.
if (useLM) if (useLM)
return *optimizer.levenbergMarquardt().values(); return *optimizer.levenbergMarquardt().values();
else else
@ -62,13 +62,13 @@ namespace gtsam {
// Use a variable ordering from COLAMD // Use a variable ordering from COLAMD
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate); Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
// initial optimization state is the same in both cases tested // Create an nonlinear Optimizer that uses a Multifrontal Solver
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer; typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
Optimizer optimizer(boost::make_shared<const G>(graph), Optimizer optimizer(boost::make_shared<const G>(graph),
boost::make_shared<const T>(initialEstimate), ordering, boost::make_shared<const T>(initialEstimate), ordering,
boost::make_shared<NonlinearOptimizationParameters>(parameters)); boost::make_shared<NonlinearOptimizationParameters>(parameters));
// choose nonlinear optimization method // now optimize using either LM or GN methods
if (useLM) if (useLM)
return *optimizer.levenbergMarquardt().values(); return *optimizer.levenbergMarquardt().values();
else else

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

@ -130,30 +130,31 @@ namespace gtsam {
// Form damped system with given lambda, and return a new, more optimistic // Form damped system with given lambda, and return a new, more optimistic
// optimizer if error decreased or iterate with a larger lambda if not. // optimizer if error decreased or iterate with a larger lambda if not.
// TODO: in theory we can't infinitely recurse, but maybe we should put a max. // TODO: in theory we can't infinitely recurse, but maybe we should put a max.
/* ************************************************************************* */ // Reminder: the parameters are Graph type $G$, Values class type $T$,
template<class G, class C, class L, class S, class W> // linear system class $L$, the non linear solver type $S$, and the writer type $W$
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::try_lambda(const L& linear) { template<class G, class T, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::try_lambda(const L& linearSystem) {
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
double lambda = parameters_->lambda_ ;
const Parameters::LambdaMode lambdaMode = parameters_->lambdaMode_ ; const Parameters::LambdaMode lambdaMode = parameters_->lambdaMode_ ;
const double factor = parameters_->lambdaFactor_ ; const double factor = parameters_->lambdaFactor_ ;
double lambda = parameters_->lambda_ ;
if( lambdaMode >= Parameters::CAUTIOUS) throw runtime_error("CAUTIOUS mode not working yet, please use BOUNDED."); if( lambdaMode >= Parameters::CAUTIOUS) throw runtime_error("CAUTIOUS mode not working yet, please use BOUNDED.");
double next_error = error_; double next_error = error_;
shared_values next_values = values_; shared_values next_values = values_;
// Keep increasing lambda until we make make progress
while(true) { while(true) {
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 // TODO: replace this dampening with a backsubstitution approach
typename L::shared_ptr damped(new L(linear)); typename L::shared_ptr dampedSystem(new L(linearSystem));
{ {
double sigma = 1.0 / sqrt(lambda); double sigma = 1.0 / sqrt(lambda);
damped->reserve(damped->size() + dimensions_->size()); dampedSystem->reserve(dampedSystem->size() + dimensions_->size());
// for each of the variables, add a prior // for each of the variables, add a prior
for(Index j=0; j<dimensions_->size(); ++j) { for(Index j=0; j<dimensions_->size(); ++j) {
size_t dim = (*dimensions_)[j]; size_t dim = (*dimensions_)[j];
@ -161,22 +162,24 @@ namespace gtsam {
Vector b = zero(dim); Vector b = zero(dim);
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma); SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma);
typename L::sharedFactor prior(new JacobianFactor(j, A, b, model)); typename L::sharedFactor prior(new JacobianFactor(j, A, b, model));
damped->push_back(prior); dampedSystem->push_back(prior);
} }
} }
if (verbosity >= Parameters::DAMPED) damped->print("damped"); if (verbosity >= Parameters::DAMPED) dampedSystem->print("damped");
// solve // Create a new solver using the damped linear system
// FIXME: remove spcg specific code // FIXME: remove spcg specific code
if (spcg_solver_) spcg_solver_->replaceFactors(damped); if (spcg_solver_) spcg_solver_->replaceFactors(dampedSystem);
shared_solver solver = (spcg_solver_) ? spcg_solver_ : shared_solver( shared_solver solver = (spcg_solver_) ? spcg_solver_ : shared_solver(
new S(damped, structure_, parameters_->useQR_)); new S(dampedSystem, structure_, parameters_->useQR_));
// Try solving
try { try {
VectorValues delta = *solver->optimize(); 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_))); shared_values newValues(new T(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);
@ -219,9 +222,10 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// One iteration of Levenberg Marquardt // One iteration of Levenberg Marquardt
/* ************************************************************************* */ // Reminder: the parameters are Graph type $G$, Values class type $T$,
template<class G, class C, class L, class S, class W> // linear system class $L$, the non linear solver type $S$, and the writer type $W$
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterateLM() { template<class G, class T, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateLM() {
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
const double lambda = parameters_->lambda_ ; const double lambda = parameters_->lambda_ ;
@ -243,10 +247,12 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class G, class C, class L, class S, class W> // Reminder: the parameters are Graph type $G$, Values class type $T$,
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::levenbergMarquardt() { // linear system class $L$, the non linear solver type $S$, and the writer type $W$
template<class G, class T, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::levenbergMarquardt() {
iterations_ = 0; // Initialize
bool converged = false; bool converged = false;
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
@ -262,6 +268,7 @@ namespace gtsam {
if (iterations_ >= parameters_->maxIterations_) if (iterations_ >= parameters_->maxIterations_)
return *this; return *this;
// Iterative loop that implements Levenberg-Marquardt
while (true) { while (true) {
double previous_error = error_; double previous_error = error_;
// do one iteration of LM // do one iteration of LM