NonlinearFactors handle constrained noise models properly now, as before they would try to whiten and fail

cpp/NoiseModel.h

@@ -164,6 +164,11 @@ virtual SharedDiagonal QR(Matrix& Ab) const;
 	 */
 virtual Matrix R() const { return thisR();}
 
+/**
+ * Simple check for constrained-ness
+ */
+virtual bool isConstrained() const {return false;}
+
 }; // Gaussian
 
 
@@ -232,12 +237,6 @@ public:
 		virtual Matrix R() const {
 			return diag(invsigmas_);
 		}
-
-		/**
-		 * Simple check for constrained-ness
-		 */
-		virtual bool isConstrained() {return false;}
-
 }; // Diagonal
 
 /**
@@ -308,9 +307,9 @@ public:
 	virtual SharedDiagonal QR(Matrix& Ab) const;
 
 	/**
-	 * Not constrained
+	 * Check constrained is always true
 	 */
-	virtual bool isConstrained() {return true;}
+	virtual bool isConstrained() const {return true;}
 
 }; // Constrained
 

cpp/NonlinearFactor.h

@@ -190,6 +190,11 @@ namespace gtsam {
 			Matrix A;
 			Vector b = - evaluateError(xj, A);
 			// TODO pass unwhitened + noise model to Gaussian factor
+			SharedDiagonal constrained =
+					boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
+			if (constrained.get() != NULL) {
+				return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b, constrained));
+			}
 			this->noiseModel_->WhitenInPlace(A);
 			this->noiseModel_->whitenInPlace(b);
 			return GaussianFactor::shared_ptr(new GaussianFactor(key_, A, b,
@@ -288,6 +293,12 @@ namespace gtsam {
 			Matrix A1, A2;
 			Vector b = -evaluateError(x1, x2, A1, A2);
 			// TODO pass unwhitened + noise model to Gaussian factor
+			SharedDiagonal constrained =
+					boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
+			if (constrained.get() != NULL) {
+				return GaussianFactor::shared_ptr(new GaussianFactor(key1_, A1, key2_,
+						A2, b, constrained));
+			}
 			this->noiseModel_->WhitenInPlace(A1);
 			this->noiseModel_->WhitenInPlace(A2);
 			this->noiseModel_->whitenInPlace(b);

cpp/testNonlinearFactor.cpp

@@ -175,6 +175,45 @@ TEST( NonlinearFactor, size )
 	CHECK(factor3->size() == 2);
 }
 
+/* ************************************************************************* */
+TEST( NonlinearFactor, linearize_constraint1 )
+{
+	Vector sigmas = Vector_(2, 0.2, 0.0);
+	SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas);
+
+	Point2 mu(1., -1.);
+	Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, 1));
+
+	Config config;
+	config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
+	GaussianFactor::shared_ptr actual = f0->linearize(config);
+
+	// create expected
+	Vector b = Vector_(2, 0., -3.);
+	GaussianFactor expected("x1", eye(2), b, constraint);
+	CHECK(assert_equal(expected, *actual));
+}
+
+/* ************************************************************************* */
+TEST( NonlinearFactor, linearize_constraint2 )
+{
+	Vector sigmas = Vector_(2, 0.2, 0.0);
+	SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas);
+
+	Point2 z3(1.,-1.);
+	simulated2D::Measurement f0(z3, constraint, 1,1);
+
+	Config config;
+	config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
+	config.insert(simulated2D::PointKey(1), Point2(5.0, 4.0));
+	GaussianFactor::shared_ptr actual = f0.linearize(config);
+
+	// create expected
+	Vector b = Vector_(2, -3., -3.);
+	GaussianFactor expected("x1", -1*eye(2), "l1", eye(2), b, constraint);
+	CHECK(assert_equal(expected, *actual));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */