Added tests for NonlinearEquality for poses near the origin, removed extraneous print statement.

cpp/testConstraintOptimizer.cpp

@@ -377,7 +377,7 @@ TEST( matrix, line_search ) {
 		   final_error = penalty(x0 + actual);
 
 	double actual_stepsize = dot(actual, delta)/dot(delta, delta);
-	cout << "actual_stepsize: " << actual_stepsize << endl;
+//	cout << "actual_stepsize: " << actual_stepsize << endl;
 
 	CHECK(final_error <= init_error);
 }

cpp/testNonlinearEquality.cpp

@@ -7,14 +7,22 @@
 
 #define GTSAM_MAGIC_KEY
 
+#include "Key.h"
+#include "Pose2.h"
 #include "VectorConfig.h"
 #include "NonlinearEquality.h"
 
+#include "LieConfig-inl.h"
+
 using namespace std;
 using namespace gtsam;
 
 typedef NonlinearEquality<VectorConfig,string,Vector> NLE;
 typedef boost::shared_ptr<NLE> shared_nle;
+typedef TypedSymbol<Pose2, 'x'> PoseKey;
+typedef LieConfig<PoseKey, Pose2> PoseConfig;
+typedef NonlinearEquality<PoseConfig, PoseKey, Pose2> PoseNLE;
+typedef boost::shared_ptr<PoseNLE> shared_poseNLE;
 
 bool vector_compare(const Vector& a, const Vector& b) {
 	return equal_with_abs_tol(a, b, 1e-5);
@@ -37,6 +45,21 @@ TEST ( NonlinearEquality, linearization ) {
 	CHECK(assert_equal(*actualLF, expLF));
 }
 
+/* ********************************************************************** */
+TEST ( NonlinearEquality, linearization_pose ) {
+
+	PoseKey key(1);
+	Pose2 value;
+	PoseConfig config;
+	config.insert(key, value);
+
+	// create a nonlinear equality constraint
+	shared_poseNLE nle(new PoseNLE(key, value));
+
+	GaussianFactor::shared_ptr actualLF = nle->linearize(config);
+	CHECK(true);
+}
+
 /* ********************************************************************** */
 TEST ( NonlinearEquality, linearization_fail ) {
   Symbol key = "x";
@@ -57,6 +80,48 @@ TEST ( NonlinearEquality, linearization_fail ) {
 	}
 }
 
+/* ********************************************************************** */
+TEST ( NonlinearEquality, linearization_fail_pose ) {
+
+	PoseKey key(1);
+	Pose2 value(2.0, 1.0, 2.0),
+		  wrong(2.0, 3.0, 4.0);
+	PoseConfig bad_linearize;
+	bad_linearize.insert(key, wrong);
+
+	// create a nonlinear equality constraint
+	shared_poseNLE nle(new PoseNLE(key, value));
+
+	// check linearize to ensure that it fails for bad linearization points
+	try {
+		GaussianFactor::shared_ptr actualLF = nle->linearize(bad_linearize);
+		CHECK(false);
+	} catch (std::invalid_argument) {
+		CHECK(true);
+	}
+}
+
+/* ********************************************************************** */
+TEST ( NonlinearEquality, linearization_fail_pose_origin ) {
+
+	PoseKey key(1);
+	Pose2 value,
+		  wrong(2.0, 3.0, 4.0);
+	PoseConfig bad_linearize;
+	bad_linearize.insert(key, wrong);
+
+	// create a nonlinear equality constraint
+	shared_poseNLE nle(new PoseNLE(key, value));
+
+	// check linearize to ensure that it fails for bad linearization points
+	try {
+		GaussianFactor::shared_ptr actualLF = nle->linearize(bad_linearize);
+		CHECK(false);
+	} catch (std::invalid_argument) {
+		CHECK(true);
+	}
+}
+
 /* ************************************************************************* */
 TEST ( NonlinearEquality, error ) {
   Symbol key = "x";