add new interface to levenbergMarquardt, now can take a threshold for sum of error

nonlinear/NonlinearOptimizer-inl.h

@@ -21,9 +21,21 @@ using namespace std;
 namespace gtsam {
 
 	/* ************************************************************************* */
-	inline bool check_convergence(double relativeErrorTreshold,
+	inline bool check_convergence(
-			double absoluteErrorTreshold, double currentError, double newError,
+			double relativeErrorTreshold,
-			int verbosity) {
+			double absoluteErrorTreshold,
+			double errorThreshold,
+			double currentError, double newError, int verbosity) {
+
+		if ( verbosity >= 2 ) {
+			if ( newError <= errorThreshold )
+				cout << "errorThreshold: " << newError << " < " << errorThreshold << endl;
+			else
+				cout << "errorThreshold: " << newError << " > " << errorThreshold << endl;
+		}
+
+		if ( newError <= errorThreshold ) return true ;
+
 		// check if diverges
 		double absoluteDecrease = currentError - newError;
 		if (verbosity >= 2) {
@@ -120,8 +132,13 @@ namespace gtsam {
 		writer.write(next.error_);
 
 		// check convergence
-		bool converged = gtsam::check_convergence(relativeThreshold,
+		bool converged = gtsam::check_convergence(
-				absoluteThreshold, error_, next.error_, verbosity);
+				relativeThreshold,
+				absoluteThreshold,
+				0.0,
+				error_,
+				next.error_,
+				verbosity);
 
 				// return converged state or iterate
 		if (converged)
@@ -236,37 +253,53 @@ namespace gtsam {
 			double relativeThreshold, double absoluteThreshold,
 			verbosityLevel verbosity, int maxIterations, double lambdaFactor, LambdaMode lambdaMode) const {
 
-		if (maxIterations <= 0) return *this;
+		return levenbergMarquardt(NonLinearOptimizerPara (absoluteThreshold, relativeThreshold, absoluteThreshold,
+				maxIterations, lambdaFactor, verbosity, lambdaMode)) ;
+	}
+
+
+	template<class G, class C, class L, class S, class W>
+	NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::
+	levenbergMarquardt(const NonLinearOptimizerPara &para) const {
+
+		if (para.maxIterations_ <= 0) return *this;
 
 		// check if we're already close enough
-		if (error_ < absoluteThreshold) {
+		if (error_ < para.sumError_) {
-			if (verbosity >= ERROR) cout << "Exiting, as error = " << error_
+			if (para.verbosity_ >= ERROR)
-					<< " < absoluteThreshold (" << absoluteThreshold << ")" << endl;
+				cout << "Exiting, as error = " << error_ << " < " << para.sumError_ << endl;
 			return *this;
 		}
 
 		// do one iteration of LM
-		NonlinearOptimizer next = iterateLM(verbosity, lambdaFactor, lambdaMode);
+		NonlinearOptimizer next = iterateLM(para.verbosity_, para.lambdaFactor_, para.lambdaMode_);
 
 		// check convergence
 		// TODO: move convergence checks here and incorporate in verbosity levels
 		// TODO: build into iterations somehow as an instance variable
-		bool converged = gtsam::check_convergence(relativeThreshold,
+		bool converged = gtsam::check_convergence(
-				absoluteThreshold, error_, next.error_, verbosity);
+				para.relDecrease_,
+				para.absDecrease_,
+				para.sumError_,
+				error_,
+				next.error_,
+				para.verbosity_);
 
 		// return converged state or iterate
-		if (converged || maxIterations <= 1) {
+		if (converged || para.maxIterations_ <= 1) {
 			// maybe show output
-			if (verbosity >= CONFIG)
+			if (para.verbosity_ >= CONFIG)
 				next.config_->print("final config");
-			if (verbosity >= ERROR)
+			if (para.verbosity_ >= ERROR)
 				cout << "final error: " << next.error_ << endl;
-			if (verbosity >= LAMBDA)
+			if (para.verbosity_ >= LAMBDA)
 				cout << "final lambda = " << next.lambda_ << endl;
 			return next;
-		} else
+		} else {
-			return next.levenbergMarquardt(relativeThreshold, absoluteThreshold,
+			NonLinearOptimizerPara newPara = para ;
-					verbosity, maxIterations-1, lambdaFactor, lambdaMode);
+			newPara.maxIterations_ = newPara.maxIterations_ - 1;
+			return next.levenbergMarquardt(newPara) ;
+		}
 	}
 
 	/* ************************************************************************* */

nonlinear/NonlinearOptimizer.h

@@ -68,6 +68,30 @@ namespace gtsam {
 			CAUTIOUS
 		} LambdaMode;
 
+		// a container for all related parameters
+		struct NonLinearOptimizerPara {
+		public:
+			double absDecrease_; /* threshold for the absolute decrease per iteration */
+			double relDecrease_; /* threshold for the relative decrease per iteration */
+			double sumError_; /* threshold for the sum of error */
+			int maxIterations_ ;
+			double lambdaFactor_ ;
+			verbosityLevel verbosity_;
+			LambdaMode lambdaMode_;
+
+		public:
+
+			NonLinearOptimizerPara(): absDecrease_(1), relDecrease_(1e-3), sumError_(0.0),
+				maxIterations_(100), lambdaFactor_(10.0), verbosity_(ERROR), lambdaMode_(BOUNDED){}
+
+			NonLinearOptimizerPara(double absDecrease, double relDecrease, double sumError,
+					int iIters = 100, double lambdaFactor = 10, verbosityLevel v = ERROR, LambdaMode lambdaMode = BOUNDED)
+			:absDecrease_(absDecrease), relDecrease_(relDecrease), sumError_(sumError),
+			 maxIterations_(iIters), lambdaFactor_(lambdaFactor), verbosity_(v), lambdaMode_(lambdaMode){}
+
+		};
+
+
 	private:
 
 		// keep a reference to const version of the graph
@@ -180,6 +204,9 @@ namespace gtsam {
 				double lambdaFactor = 10, LambdaMode lambdaMode = BOUNDED) const;
 
 
+		NonlinearOptimizer
+		levenbergMarquardt(const NonLinearOptimizerPara &para) const;
+
 		/**
 		 * Static interface to LM optimization using default ordering and thresholds
 		 * @param graph 	   Nonlinear factor graph to optimize
@@ -247,10 +274,12 @@ namespace gtsam {
 	/**
 	 * Check convergence
 	 */
-	bool check_convergence (double relativeErrorTreshold,
+	bool check_convergence (
+			double relativeErrorTreshold,
 			double absoluteErrorTreshold,
-			double currentError, double newError,
+			double errorThreshold,
-			int verbosity);
+			double currentError, double newError, int verbosity);
+
 
 } // gtsam