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

record iteration numbers

release/4.3a0
Kai Ni 2010-12-03 03:23:35 +00:00
parent d9935519f9
commit b55b9de27f
2 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -37,7 +37,7 @@ 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,
shared_values values, shared_ordering ordering, shared_parameters parameters) : shared_values values, shared_ordering ordering, shared_parameters parameters) :
graph_(graph), values_(values), error_(graph->error(*values)), graph_(graph), values_(values), iterations_(0), error_(graph->error(*values)),
ordering_(ordering), parameters_(parameters), dimensions_(new vector<size_t>(values->dims(*ordering))) { ordering_(ordering), parameters_(parameters), 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");
@ -54,7 +54,7 @@ namespace gtsam {
shared_ordering ordering, shared_ordering ordering,
shared_solver solver, shared_solver solver,
shared_parameters parameters): shared_parameters parameters):
graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering), solver_(solver), graph_(graph), values_(values), iterations_(0), error_(graph->error(*values)), ordering_(ordering), solver_(solver),
parameters_(parameters), dimensions_(new vector<size_t>(values->dims(*ordering))) { parameters_(parameters), 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");
@ -245,7 +245,7 @@ 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<G, C, L, S, W>::levenbergMarquardt() { NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::levenbergMarquardt() {
int maxIterations = parameters_->maxIterations_ ; iterations_ = 0;
bool converged = false; bool converged = false;
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
@ -256,6 +256,7 @@ namespace gtsam {
return *this; return *this;
} }
iterations_ = 1;
while (true) { while (true) {
double previous_error = error_; double previous_error = error_;
// do one iteration of LM // do one iteration of LM
@ -269,13 +270,13 @@ namespace gtsam {
// TODO: build into iterations somehow as an instance variable // TODO: build into iterations somehow as an instance variable
converged = gtsam::check_convergence(*parameters_, previous_error, error_); converged = gtsam::check_convergence(*parameters_, previous_error, error_);
if(maxIterations <= 0 || converged == true) { if(iterations_ >= parameters_->maxIterations_ || converged == true) {
if (verbosity >= Parameters::VALUES) values_->print("final values"); if (verbosity >= Parameters::VALUES) values_->print("final values");
if (verbosity >= Parameters::ERROR) cout << "final error: " << error_ << endl; if (verbosity >= Parameters::ERROR) cout << "final error: " << error_ << endl;
if (verbosity >= Parameters::LAMBDA) cout << "final lambda = " << lambda() << endl; if (verbosity >= Parameters::LAMBDA) cout << "final lambda = " << lambda() << endl;
return *this; return *this;
} }
maxIterations--; iterations_++;
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */

10
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -97,6 +97,9 @@ namespace gtsam {
shared_parameters parameters_; shared_parameters parameters_;
// for performance track
size_t iterations_;
// // keep current lambda for use within LM only // // keep current lambda for use within LM only
// // TODO: red flag, should we have an LM class ? // // TODO: red flag, should we have an LM class ?
// const double lambda_; // const double lambda_;
@ -178,7 +181,7 @@ namespace gtsam {
// ordering_(optimizer.ordering_), solver_(optimizer.solver_), lambda_(optimizer.lambda_), dimensions_(optimizer.dimensions_) {} // ordering_(optimizer.ordering_), solver_(optimizer.solver_), lambda_(optimizer.lambda_), dimensions_(optimizer.dimensions_) {}
NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) : NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) :
graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_), graph_(optimizer.graph_), values_(optimizer.values_), iterations_(0), error_(optimizer.error_),
ordering_(optimizer.ordering_), solver_(optimizer.solver_), parameters_(optimizer.parameters_), dimensions_(optimizer.dimensions_) {} ordering_(optimizer.ordering_), solver_(optimizer.solver_), parameters_(optimizer.parameters_), dimensions_(optimizer.dimensions_) {}
/** /**
@ -196,6 +199,11 @@ namespace gtsam {
*/ */
shared_values values() const{ return values_; } shared_values values() const{ return values_; }
/**
* Return the itertions
*/
size_t iterations() const { return iterations_; }
/** /**
* Return mean and covariance on a single variable * Return mean and covariance on a single variable
*/ */