diff --git a/cpp/SQPOptimizer-inl.h b/cpp/SQPOptimizer-inl.h
index f6d0c98a9..6111ca310 100644
--- a/cpp/SQPOptimizer-inl.h
+++ b/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) {
diff --git a/cpp/SQPOptimizer.h b/cpp/SQPOptimizer.h
index d94adfa7f..6f1f65b8e 100644
--- a/cpp/SQPOptimizer.h
+++ b/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);
 };
diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp
index a69d46991..102348210 100644
--- a/cpp/testSQPOptimizer.cpp
+++ b/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); }