From a2310b4eab84e09fbb91b5aca179b1389a212f1f Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Tue, 23 Feb 2010 18:37:17 +0000
Subject: [PATCH] Fixed small error in NonlinearConstraint and added note
 explaining why nonlinear constraints do not use a constrained noisemodel in
 the nonlinear sense.

---
 cpp/NonlinearConstraint-inl.h | 2 +-
 cpp/NonlinearConstraint.h     | 6 ++++++
 2 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h
index b237081b9..5f55b48f7 100644
--- a/cpp/NonlinearConstraint-inl.h
+++ b/cpp/NonlinearConstraint-inl.h
@@ -25,7 +25,7 @@ template <class Config>
 NonlinearConstraint<Config>::NonlinearConstraint(const LagrangeKey& lagrange_key,
 					size_t dim_lagrange,
 					bool isEquality) :
-	NonlinearFactor<Config>(noiseModel::Unit::Create(dim_lagrange)),
+	NonlinearFactor<Config>(noiseModel::Unit::Create(2*dim_lagrange)),
 	lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality) {
 	this->keys_.push_back(lagrange_key_);
 }
diff --git a/cpp/NonlinearConstraint.h b/cpp/NonlinearConstraint.h
index 4787b1e10..b2e807c73 100644
--- a/cpp/NonlinearConstraint.h
+++ b/cpp/NonlinearConstraint.h
@@ -25,6 +25,12 @@ typedef TypedSymbol<Vector, 'L'> LagrangeKey;
  *
  * The measurement z in the underlying NonlinearFactor is the
  * set of Lagrange multipliers.
+ *
+ * Note on NoiseModel:
+ * The nonlinear constraint actually uses a Unit noisemodel so that
+ * it is possible to have a finite error value when the constraint is
+ * not fulfilled.  Using a constrained noisemodel will immediately cause
+ * infinite error and break optimization.
  */
 template <class Config>
 class NonlinearConstraint : public NonlinearFactor<Config> {