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

@@ -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_);
 }

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> {