diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h
index 010341804..3df049268 100644
--- a/gtsam/nonlinear/NonlinearFactor.h
+++ b/gtsam/nonlinear/NonlinearFactor.h
@@ -315,10 +315,15 @@ public:
     }
 
     // TODO pass unwhitened + noise model to Gaussian factor
+    // For now, only linearized constrained factors have noise model at linear level!!!
     noiseModel::Constrained::shared_ptr constrained =
         boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
-    if(constrained)
-      return GaussianFactor::shared_ptr(new JacobianFactor(terms, b, constrained->unit()));
+    if(constrained) {
+      size_t augmentedDim = terms[0].second.rows() - constrained->dim();
+      Vector augmentedZero = zero(augmentedDim);
+      Vector augmentedb = concatVectors(2, &b, &augmentedZero);
+      return GaussianFactor::shared_ptr(new JacobianFactor(terms, augmentedb, constrained->unit(augmentedDim)));
+    }
     else
       return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
   }