Added a function for the SQPOptimizer that will iterate until convergence. At the moment, the convergence conditions are quite simple (error below threshold or too many iterations). The system does, however, strictly limit the number of iterations.

cpp/SQPOptimizer-inl.h

@@ -22,12 +22,35 @@ using namespace boost::assign;
 
 namespace gtsam {
 
+/* **************************************************************** */
+template <class G, class C>
+double constraintError(const G& graph, const C& config) {
+	// local typedefs
+	typedef typename G::const_iterator const_iterator;
+	typedef NonlinearConstraint<C> NLConstraint;
+	typedef boost::shared_ptr<NLConstraint > shared_c;
+
+	// accumulate error
+	double error = 0;
+
+	// find the constraints
+	for (const_iterator factor = graph.begin(); factor < graph.end(); factor++) {
+		const shared_c constraint = boost::shared_dynamic_cast<NLConstraint >(*factor);
+		if (constraint != NULL) {
+			Vector e = constraint->error_vector(config);
+			error += inner_prod(trans(e),e);
+		}
+	}
+	return error;
+}
+
 /* **************************************************************** */
 template <class G, class C>
 SQPOptimizer<G,C>::SQPOptimizer(const G& graph, const Ordering& ordering,
 		shared_config config)
 : graph_(&graph), ordering_(&ordering), full_ordering_(ordering),
-  config_(config), lagrange_config_(new VectorConfig), error_(graph.error(*config))
+  config_(config), lagrange_config_(new VectorConfig), error_(graph.error(*config)),
+  constraint_error_(constraintError(graph, *config))
 {
 	// local typedefs
 	typedef typename G::const_iterator const_iterator;
@@ -53,7 +76,8 @@ template <class G, class C>
 SQPOptimizer<G,C>::SQPOptimizer(const G& graph, const Ordering& ordering,
 		shared_config config, shared_vconfig lagrange)
 : graph_(&graph), ordering_(&ordering), full_ordering_(ordering),
-  config_(config), lagrange_config_(lagrange), error_(graph.error(*config))
+  config_(config), lagrange_config_(lagrange), error_(graph.error(*config)),
+  constraint_error_(constraintError(graph, *config))
 {
 }
 
@@ -109,6 +133,37 @@ SQPOptimizer<G, C> SQPOptimizer<G, C>::iterate(Verbosity v) const {
 	return SQPOptimizer<G, C>(*graph_, full_ordering_, newConfig, newLamConfig);
 }
 
+/* **************************************************************** */
+template<class G, class C>
+SQPOptimizer<G, C> SQPOptimizer<G, C>::iterateSolve(double relThresh, double absThresh,
+		double constraintThresh, size_t maxIterations, Verbosity v) const {
+	bool verbose = v == SQPOptimizer<G, C>::FULL;
+
+	// do an iteration
+	SQPOptimizer<G, C> next = iterate(v);
+
+	// if converged or out of iterations, return result
+	if (maxIterations == 1 ||
+			next.checkConvergence(relThresh, absThresh, constraintThresh,
+								  error_, constraint_error_))
+		return next;
+	else // otherwise, recurse with a lower maxIterations
+		return next.iterateSolve(relThresh, absThresh, constraintThresh,
+				maxIterations-1, v);
+}
+
+/* **************************************************************** */
+template<class G, class C>
+bool SQPOptimizer<G, C>::checkConvergence(double relThresh, double absThresh,
+		double constraintThresh, double full_error, double constraint_error) const {
+	// if error sufficiently low, then the system has converged
+	if (error_ < absThresh && constraint_error_ < constraintThresh)
+		return true;
+
+	// TODO: determine other cases
+	return false;
+}
+
 /* **************************************************************** */
 template<class G, class C>
 void SQPOptimizer<G, C>::print(const std::string& s) {

cpp/SQPOptimizer.h

@@ -83,6 +83,30 @@ public:
 	 */
 	SQPOptimizer<FactorGraph, Config> iterate(Verbosity verbosity=SILENT) const;
 
+	/**
+	 * Iterates recursively until converence occurs
+	 * @param relThresh minimum change in error between iterations
+	 * @param absThresh  minimum error necessary to converge
+	 * @param constraintThresh  minimum constraint error to be feasible
+	 * @param maxIterations is the maximum number of iterations
+	 * @param verbosity controls output print statements
+	 * @return a new optimization object with final values
+	 */
+	SQPOptimizer<FactorGraph, Config>
+	iterateSolve(double relThresh, double absThresh, double constraintThresh,
+			size_t maxIterations = 10, Verbosity verbosity=SILENT) const;
+
+	/**
+	 * Checks whether convergence has occurred, and returns true if
+	 * the solution will not get better, based on the previous error conditions.
+	 * @param full_error is the error all the factors and constraints
+	 * @param constraint_error is the error of just the constraints
+	 * @param relThresh is the relative threshold between
+	 * @return true if the problem has converged
+	 */
+	bool checkConvergence(double relThresh, double absThresh,
+			double constraintThresh, double full_error, double constraint_error) const;
+
 	/** Standard print function with optional name */
 	void print(const std::string& s);
 };

cpp/testSQPOptimizer.cpp

@@ -323,6 +323,35 @@ TEST ( SQPOptimizer, inequality_avoid ) {
 	CHECK(assert_equal(exp2, *(after2ndIteration.config())));
 }
 
+/* ********************************************************************* */
+TEST ( SQPOptimizer, inequality_avoid_iterative ) {
+	// create the graph
+	NLGraph graph; VectorConfig feasible;
+	boost::tie(graph, feasible) = obstacleAvoidGraph();
+
+	// create the rest of the config
+	shared_config init(new VectorConfig(feasible));
+	init->insert("x2", Vector_(2, 5.0, 100.0));
+
+	// create an ordering
+	Ordering ord;
+	ord += "x1", "x2", "x3", "obs";
+
+	// create an optimizer
+	Optimizer optimizer(graph, ord, init);
+
+	double relThresh = 1e-5; // minimum change in error between iterations
+	double absThresh = 1e-5; // minimum error necessary to converge
+	double constraintThresh = 1e-9; // minimum constraint error to be feasible
+	Optimizer final = optimizer.iterateSolve(relThresh, absThresh, constraintThresh);
+
+	// verify
+	VectorConfig exp2(feasible);
+	exp2.insert("x2", Vector_(2, 5.0, 0.5));
+	CHECK(assert_equal(exp2, *(final.config())));
+}
+
+
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }