diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp
index f51197cf2..dd17c00ef 100644
--- a/gtsam/base/Matrix.cpp
+++ b/gtsam/base/Matrix.cpp
@@ -543,8 +543,7 @@ Matrix collect(size_t nrMatrices, ...)
 void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) {
   const DenseIndex m = A.rows();
   if (inf_mask) {
-    // only scale the first v.size() rows of A to support augmented Matrix
-    for (DenseIndex i=0; i<v.size(); ++i) {
+    for (DenseIndex i=0; i<m; ++i) {
       const double& vi = v(i);
       if (std::isfinite(vi))
         A.row(i) *= vi;
diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index f3d5be9e8..1094306a9 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -416,7 +416,6 @@ GTSAM_EXPORT Matrix collect(size_t nrMatrices, ...);
  * Arguments (Matrix, Vector) scales the columns,
  * (Vector, Matrix) scales the rows
  * @param inf_mask when true, will not scale with a NaN or inf value.
- * The inplace version also allows v.size()<A.rows() and only scales the first v.size() rows of A.
  */
 GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
 GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row
diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index fcaec3afa..3b9c5d79c 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -339,12 +339,12 @@ Vector Constrained::whiten(const Vector& v) const {
   // a hard constraint, we don't do anything.
   const Vector& a = v;
   const Vector& b = sigmas_;
-  // Now allows for whiten augmented vector with a new additional part coming
-  // from the Lagrange multiplier. So a.size() >= b.size()
-  Vector c = a;
-  for( DenseIndex i = 0; i < b.size(); i++ ) {
+  size_t n = a.size();
+  assert (b.size()==a.size());
+  Vector c(n);
+  for( size_t i = 0; i < n; i++ ) {
     const double& ai = a(i), &bi = b(i);
-    if (bi!=0) c(i) = ai/bi;
+    c(i) = (bi==0.0) ? ai : ai/bi; // NOTE: not ediv_()
   }
   return c;
 }
@@ -362,11 +362,7 @@ double Constrained::distance(const Vector& v) const {
 /* ************************************************************************* */
 Matrix Constrained::Whiten(const Matrix& H) const {
   // selective scaling
-  // Now allow augmented Matrix with a new additional part coming
-  // from the Lagrange multiplier.
-  Matrix M(H.block(0, 0, dim(), H.cols()));
-  Constrained::WhitenInPlace(M);
-  return M;
+  return vector_scale(invsigmas(), H, true);
 }
 
 /* ************************************************************************* */
@@ -375,8 +371,6 @@ void Constrained::WhitenInPlace(Matrix& H) const {
   // Scale row i of H by sigmas[i], basically multiplying H with diag(sigmas)
   // Set inf_mask flag is true so that if invsigmas[i] is inf, i.e. sigmas[i] = 0,
   // indicating a hard constraint, we leave H's row i in place.
-  // Now allow augmented Matrix with a new additional part coming
-  // from the Lagrange multiplier.
 //  Inlined: vector_scale_inplace(invsigmas(), H, true);
   // vector_scale_inplace(v, A, true);
   for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) {
@@ -399,14 +393,12 @@ void Constrained::WhitenInPlace(Eigen::Block<Matrix> H) const {
 }
 
 /* ************************************************************************* */
-Constrained::shared_ptr Constrained::unit(size_t augmentedDim) const {
-  Vector sigmas = ones(dim()+augmentedDim);
+Constrained::shared_ptr Constrained::unit() const {
+  Vector sigmas = ones(dim());
   for (size_t i=0; i<dim(); ++i)
     if (this->sigmas_(i) == 0.0)
       sigmas(i) = 0.0;
-  Vector augmentedMu = zero(dim()+augmentedDim);
-  subInsert(augmentedMu, mu_, 0);
-  return MixedSigmas(augmentedMu, sigmas);
+  return MixedSigmas(mu_, sigmas);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index 4b0e4848d..caad6b01a 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -61,6 +61,11 @@ namespace gtsam {
       Base(size_t dim = 1):dim_(dim) {}
       virtual ~Base() {}
 
+      /** true if a constrained noise mode, saves slow/clumsy dynamic casting */
+      virtual bool is_constrained() const {
+        return false;
+      }
+
       /// Dimensionality
       inline size_t dim() const { return dim_;}
 
@@ -385,6 +390,11 @@ namespace gtsam {
 
       virtual ~Constrained() {}
 
+      /** true if a constrained noise mode, saves slow/clumsy dynamic casting */
+      virtual bool is_constrained() const {
+        return true;
+      }
+
       /// Access mu as a vector
       const Vector& mu() const { return mu_; }
 
@@ -481,9 +491,8 @@ namespace gtsam {
       /**
        * Returns a Unit version of a constrained noisemodel in which
        * constrained sigmas remain constrained and the rest are unit scaled
-       * Now support augmented part from the Lagrange multiplier.
        */
-      shared_ptr unit(size_t augmentedDim = 0) const;
+      shared_ptr unit() const;
 
     private:
       /** Serialization function */
@@ -732,7 +741,7 @@ namespace gtsam {
       };
 
       /// Cauchy implements the "Cauchy" robust error model (Lee2013IROS).  Contributed by:
-      ///   Dipl.-Inform. Jan Oberl�nder (M.Sc.), FZI Research Center for
+      ///   Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
       ///   Information Technology, Karlsruhe, Germany.
       ///   oberlaender@fzi.de
       /// Thanks Jan!
diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp
index 08b131152..19e8e2b00 100644
--- a/gtsam/nonlinear/NonlinearFactor.cpp
+++ b/gtsam/nonlinear/NonlinearFactor.cpp
@@ -22,7 +22,6 @@
 namespace gtsam {
 
 /* ************************************************************************* */
-
 void NonlinearFactor::print(const std::string& s,
     const KeyFormatter& keyFormatter) const {
   std::cout << s << "  keys = { ";
@@ -32,10 +31,12 @@ void NonlinearFactor::print(const std::string& s,
   std::cout << "}" << std::endl;
 }
 
+/* ************************************************************************* */
 bool NonlinearFactor::equals(const NonlinearFactor& f, double tol) const {
   return Base::equals(f);
 }
 
+/* ************************************************************************* */
 NonlinearFactor::shared_ptr NonlinearFactor::rekey(
     const std::map<Key, Key>& rekey_mapping) const {
   shared_ptr new_factor = clone();
@@ -48,6 +49,7 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey(
   return new_factor;
 }
 
+/* ************************************************************************* */
 NonlinearFactor::shared_ptr NonlinearFactor::rekey(
     const std::vector<Key>& new_keys) const {
   assert(new_keys.size() == this->keys().size());
@@ -57,13 +59,13 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey(
 }
 
 /* ************************************************************************* */
-
 void NoiseModelFactor::print(const std::string& s,
     const KeyFormatter& keyFormatter) const {
   Base::print(s, keyFormatter);
   this->noiseModel_->print("  noise model: ");
 }
 
+/* ************************************************************************* */
 bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
   const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
   return e && Base::equals(f, tol)
@@ -72,6 +74,7 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
               && noiseModel_->equals(*e->noiseModel_, tol)));
 }
 
+/* ************************************************************************* */
 static void check(const SharedNoiseModel& noiseModel, size_t m) {
   if (!noiseModel)
     throw std::invalid_argument("NoiseModelFactor: no NoiseModel.");
@@ -80,12 +83,14 @@ static void check(const SharedNoiseModel& noiseModel, size_t m) {
         "NoiseModelFactor was created with a NoiseModel of incorrect dimension.");
 }
 
+/* ************************************************************************* */
 Vector NoiseModelFactor::whitenedError(const Values& c) const {
   const Vector b = unwhitenedError(c);
   check(noiseModel_, b.size());
   return noiseModel_->whiten(b);
 }
 
+/* ************************************************************************* */
 double NoiseModelFactor::error(const Values& c) const {
   if (this->active(c)) {
     const Vector b = unwhitenedError(c);
@@ -96,6 +101,7 @@ double NoiseModelFactor::error(const Values& c) const {
   }
 }
 
+/* ************************************************************************* */
 boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
     const Values& x) const {
 
@@ -119,18 +125,12 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
   }
 
   // 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) {
-    // Create a factor of reduced row dimension d_
-    size_t d_ = b.size() - constrained->dim();
-    Vector zero_ = zero(d_);
-    Vector b_ = concatVectors(2, &b, &zero_);
-    noiseModel::Constrained::shared_ptr model = constrained->unit(d_);
-    return boost::make_shared<JacobianFactor>(terms, b_, model);
-  } else
-    return boost::make_shared<JacobianFactor>(terms, b);
+  if (noiseModel_->is_constrained())
+    return GaussianFactor::shared_ptr(
+        new JacobianFactor(terms, b,
+            boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()));
+  else
+    return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h
index 1d5d0cb12..68abedd02 100644
--- a/gtsam_unstable/nonlinear/ExpressionFactor.h
+++ b/gtsam_unstable/nonlinear/ExpressionFactor.h
@@ -107,14 +107,12 @@ public:
 
   virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
 
-    // Check whether noise model is constrained or not
-    noiseModel::Constrained::shared_ptr constrained = //
-        boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
-
     // Create a writeable JacobianFactor in advance
+    // In case noise model is constrained, we need to provide a noise model
+    bool constrained = noiseModel_->is_constrained();
     boost::shared_ptr<JacobianFactor> factor(
         constrained ? new JacobianFactor(keys_, dimensions_, Dim,
-            constrained->unit()) :
+            boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()) :
             new JacobianFactor(keys_, dimensions_, Dim));
 
     // Wrap keys and VerticalBlockMatrix into structure passed to expression_