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>
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
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))) {
if (!graph) throw std::invalid_argument(
"NonlinearOptimizer constructor: graph = NULL");
@ -54,7 +54,7 @@ namespace gtsam {
shared_ordering ordering,
shared_solver solver,
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))) {
if (!graph) throw std::invalid_argument(
"NonlinearOptimizer constructor: graph = NULL");
@ -245,7 +245,7 @@ namespace gtsam {
template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::levenbergMarquardt() {
int maxIterations = parameters_->maxIterations_ ;
iterations_ = 0;
bool converged = false;
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
@ -256,6 +256,7 @@ namespace gtsam {
return *this;
}
iterations_ = 1;
while (true) {
double previous_error = error_;
// do one iteration of LM
@ -269,13 +270,13 @@ namespace gtsam {
// TODO: build into iterations somehow as an instance variable
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::ERROR) cout << "final error: " << error_ << endl;
if (verbosity >= Parameters::LAMBDA) cout << "final lambda = " << lambda() << endl;
return *this;
}
maxIterations--;
iterations_++;
}
}
/* ************************************************************************* */

10
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -97,6 +97,9 @@ namespace gtsam {
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_;
@ -178,7 +181,7 @@ namespace gtsam {
// 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_),
graph_(optimizer.graph_), values_(optimizer.values_), iterations_(0), error_(optimizer.error_),
ordering_(optimizer.ordering_), solver_(optimizer.solver_), parameters_(optimizer.parameters_), dimensions_(optimizer.dimensions_) {}
/**
@ -196,6 +199,11 @@ namespace gtsam {
*/
shared_values values() const{ return values_; }
/**
* Return the itertions
*/
size_t iterations() const { return iterations_; }
/**
* Return mean and covariance on a single variable
*/