Cleanup with NonlinearConstraints to make the active() function do thresholding for inequality constraints, rather than constraint function itself.

testSQP now has all tests active and passing.
Added greaterThanOrEqual() for vector comparison.

cpp/NonlinearConstraint-inl.h

@@ -14,10 +14,7 @@ namespace gtsam {
 /* ************************************************************************* */
 template <class Config>
 bool NonlinearConstraint<Config>::active(const Config& config) const {
-	if (!isEquality_ && zero(error_vector(config)))
-		return false;
-	else
-		return true;
+	return !(!isEquality_ && greaterThanOrEqual(error_vector(config), zero(p_)));
 }
 
 /* ************************************************************************* */

cpp/NonlinearOptimizer-inl.h

@@ -134,7 +134,7 @@ namespace gtsam {
 			delta.print("delta");
 
 		// update config
-		shared_config newConfig(new C(config_->exmap(delta)));
+		shared_config newConfig(new C(config_->exmap(delta))); // TODO: updateConfig
 		if (verbosity >= TRYCONFIG)
 			newConfig->print("config");
 

cpp/NonlinearOptimizer.h

@@ -38,10 +38,10 @@ namespace gtsam {
 			LAMBDA,
 			CONFIG,
 			DELTA,
-			LINEAR,
 			TRYLAMBDA,
 			TRYCONFIG,
 			TRYDELTA,
+			LINEAR,
 			DAMPED
 		};
 

cpp/SQPOptimizer-inl.h

@@ -95,7 +95,7 @@ SQPOptimizer<G, C> SQPOptimizer<G, C>::iterate(Verbosity v) const {
 	GaussianFactorGraph fg;
 
 	// prepare an ordering of lagrange multipliers to remove
-	Ordering rem;
+	Ordering keysToRemove;
 
 	// iterate over all factors and linearize
 	for (const_iterator factor = graph_->begin(); factor < graph_->end(); factor++) {
@@ -115,22 +115,22 @@ SQPOptimizer<G, C> SQPOptimizer<G, C>::iterate(Verbosity v) const {
 			fg.push_back(c);
 		} else {
 			if (verbose) constraint->print("Skipping...");
-			rem += constraint->lagrangeKey();
+			keysToRemove += constraint->lagrangeKey();
 		}
 	}
 	if (verbose) fg.print("Before Optimization");
 
 	// optimize linear graph to get full delta config
-	VectorConfig delta = fg.optimize(full_ordering_.subtract(rem));
+	VectorConfig delta = fg.optimize(full_ordering_.subtract(keysToRemove));
 
 	if (verbose) delta.print("Delta Config");
 
 	// update both state variables
 	shared_config newConfig(new C(config_->exmap(delta)));
-	shared_vconfig newLamConfig(new VectorConfig(lagrange_config_->exmap(delta)));
+	shared_vconfig newLambdas(new VectorConfig(lagrange_config_->exmap(delta)));
 
 	// construct a new optimizer
-	return SQPOptimizer<G, C>(*graph_, full_ordering_, newConfig, newLamConfig);
+	return SQPOptimizer<G, C>(*graph_, full_ordering_, newConfig, newLambdas);
 }
 
 /* **************************************************************** */

cpp/Vector.cpp

@@ -111,6 +111,17 @@ namespace gtsam {
     return true;
   }
   
+  /* ************************************************************************* */
+  bool greaterThanOrEqual(const Vector& vec1, const Vector& vec2) {
+	  Vector::const_iterator it1 = vec1.begin();
+	  Vector::const_iterator it2 = vec2.begin();
+	  size_t m = vec1.size();
+	  for(size_t i=0; i<m; i++)
+		  if(!(it1[i] >= it2[i]))
+			  return false;
+	  return true;
+  }
+
   /* ************************************************************************* */
   bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) {
     Vector::const_iterator it1 = vec1.begin();

cpp/Vector.h

@@ -87,6 +87,13 @@ void print(const Vector& v, const std::string& s = "");
  */
 bool operator==(const Vector& vec1,const Vector& vec2);
 
+/**
+ * Greater than or equal to operation
+ * returns true if all elements in v1
+ * are greater than corresponding elements in v2
+ */
+bool greaterThanOrEqual(const Vector& v1, const Vector& v2);
+
 /**
  * VecA == VecB up to tolerance
  */

cpp/testSQP.cpp

@@ -528,25 +528,16 @@ TEST (SQP, stereo_truth ) {
 }
 
 
-// TEST DISABLED: optimization becomes unstable with one coordinate
-// growing indefinitely and by the 4th iteration the system is
-// underconstrained
 /**
  * Ground truth for a visual slam example with stereo vision
  * with some noise injected into the initial config
  */
 TEST (SQP, stereo_truth_noisy ) {
 	bool verbose = false;
-	int maxIt = 5;
 
 	// setting to determine how far away the noisy landmark is,
 	// given that the ground truth is 5m in front of the cameras
-	double noisyDist = 7.4;
-	/** RESULTS of optimization based on size of error:
-	 * With a ground truth distance away of 5m, results based on initial input:
-	 * >= 7.5m: the system diverges
-	 *  < 7.5m: system finds a solution with near zero error
-	 */
+	double noisyDist = 7.6;
 
 	// create initial estimates
 	Rot3 faceDownY(Matrix_(3,3,
@@ -604,17 +595,9 @@ TEST (SQP, stereo_truth_noisy ) {
 	if (verbose)
 		cout << "Initial Error: " << optimizer.error() << endl;
 
-	// use Gauss-Newton optimization
+	// use Levenberg-Marquardt optimization
 	double relThresh = 1e-5, absThresh = 1e-5;
-	optimizer = optimizer.gaussNewton(relThresh, absThresh);
-
-//	// optimize
-//	for (int i = 0; i<maxIt; ++i) {
-//		cout << "Iteration: " << i << endl;
-//		optimizer = optimizer.iterate();
-//		optimizer.config()->print("Updated Config");
-//		cout << "Error after iteration: " << optimizer.error() << endl;
-//	}
+	optimizer = optimizer.levenbergMarquardt(relThresh, absThresh, VOptimizer::SILENT);
 
 	// verify
 	DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
@@ -742,10 +725,6 @@ TEST (SQP, stereo_sqp ) {
 	CHECK(assert_equal(*truthConfig,*(afterOneIteration.config())));
 }
 
-
-// TEST DISABLED: There is a stability problem that sends the solution
-// further away with every iteration, and eventually will result in the linearized
-// system being underconstrained
 /**
  * SQP version of the above stereo example,
  * with noise in the initial estimate
@@ -807,12 +786,9 @@ TEST (SQP, stereo_sqp_noisy ) {
 	}
 
 	// check if correct
-	//CHECK(assert_equal(*truthConfig,*(optimizer.config())));
+	CHECK(assert_equal(*truthConfig,*(optimizer.config())));
 }
 
-// TEST DISABLED: There is a stability problem that sends the solution
-// further away with every iteration, and eventually will result in the linearized
-// system being underconstrained
 /**
  * SQP version of the above stereo example,
  * with noise in the initial estimate and manually specified
@@ -855,17 +831,17 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) {
 	// optimize
 	double start_error = optimizer.error();
 	int maxIt = 5;
-//	for (int i=0; i<maxIt; ++i) {
-//		if (verbose) {
-//			cout << "\n ************************** \n"
-//				 << " Iteration: " << i << endl;
-//			optimizer.config()->print("Config Before Iteration");
-//			optimizer.configLagrange()->print("Lagrange Before Iteration");
-//			optimizer = optimizer.iterate(SOptimizer::FULL);
-//		}
-//		else
-//			optimizer = optimizer.iterate(SOptimizer::SILENT);
-//	}
+	for (int i=0; i<maxIt; ++i) {
+		if (verbose) {
+			cout << "\n ************************** \n"
+				 << " Iteration: " << i << endl;
+			optimizer.config()->print("Config Before Iteration");
+			optimizer.configLagrange()->print("Lagrange Before Iteration");
+			optimizer = optimizer.iterate(SOptimizer::FULL);
+		}
+		else
+			optimizer = optimizer.iterate(SOptimizer::SILENT);
+	}
 
 	if (verbose) cout << "Initial Error: " << start_error << "\n"
 					  << "Final Error:   " << optimizer.error() << endl;
@@ -880,7 +856,7 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) {
 	}
 
 	// check if correct
-	//CHECK(assert_equal(*truthConfig,*(optimizer.config())));
+	CHECK(assert_equal(*truthConfig,*(optimizer.config())));
 }
 
 /* ************************************************************************* */

cpp/testSQPOptimizer.cpp

@@ -224,15 +224,12 @@ namespace sqp_avoid1 {
 double radius = 1.0;
 
 // binary avoidance constraint
-/** g(x) = ||x2-obs||^2 > radius^2 */
+/** g(x) = ||x2-obs||^2 - radius^2 > 0 */
 Vector g_func(const VectorConfig& config, const list<string>& keys) {
 	Vector delta = config[keys.front()]-config[keys.back()];
 	double dist2 = sum(emul(delta, delta));
 	double thresh = radius*radius;
-	if (dist2 < thresh)
-		return Vector_(1, dist2-thresh);
-	else
-		return zero(1);
+	return Vector_(1, dist2-thresh);
 }
 
 /** gradient at pose */

cpp/testVector.cpp

@@ -209,6 +209,15 @@ TEST( TestVector, equals )
 	CHECK(!equal_with_abs_tol(v1, v2, tol));
 }
 
+/* ************************************************************************* */
+TEST( TestVector, greater_than )
+{
+	Vector v1 = Vector_(3, 1.0, 2.0, 3.0),
+		   v2 = zero(3);
+	CHECK(greaterThanOrEqual(v1, v1)); // test basic greater than
+	CHECK(greaterThanOrEqual(v1, v2)); // test equals
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */