From b55b9de27f4198e363d7dd8ed2a7cfe8b1eb731f Mon Sep 17 00:00:00 2001
From: Kai Ni <nikai97@gmail.com>
Date: Fri, 3 Dec 2010 03:23:35 +0000
Subject: [PATCH] record iteration numbers

---
 gtsam/nonlinear/NonlinearOptimizer-inl.h | 11 ++++++-----
 gtsam/nonlinear/NonlinearOptimizer.h     | 10 +++++++++-
 2 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h
index 8dc6033db..5ac5431a0 100644
--- a/gtsam/nonlinear/NonlinearOptimizer-inl.h
+++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h
@@ -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_++;
 		}
 	}
 	/* ************************************************************************* */
diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h
index d1fc33196..31aa34cb0 100644
--- a/gtsam/nonlinear/NonlinearOptimizer.h
+++ b/gtsam/nonlinear/NonlinearOptimizer.h
@@ -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
 		 */