Merged twio changes in: the is_constrained() flag that does away with the dynamic cast, and the changes duy made in commit a61b49d to remove the constrained noise model hack. At the same time, both linearize methods (in expressionFactor and NonlinearFactor) now make use of is_constrained().

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<m; ++i) {
-    for (DenseIndex i=0; i<v.size(); ++i) {
       const double& vi = v(i);
       if (std::isfinite(vi))
         A.row(i) *= vi;

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

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
+  size_t n = a.size();
-  // from the Lagrange multiplier. So a.size() >= b.size()
+  assert (b.size()==a.size());
-  Vector c = a;
+  Vector c(n);
-  for( DenseIndex i = 0; i < b.size(); i++ ) {
+  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
+  return vector_scale(invsigmas(), H, true);
-  // from the Lagrange multiplier.
-  Matrix M(H.block(0, 0, dim(), H.cols()));
-  Constrained::WhitenInPlace(M);
-  return M;
 }
 
 /* ************************************************************************* */
@@ -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 {
+Constrained::shared_ptr Constrained::unit() const {
-  Vector sigmas = ones(dim()+augmentedDim);
+  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);
+  return MixedSigmas(mu_, sigmas);
-  subInsert(augmentedMu, mu_, 0);
-  return MixedSigmas(augmentedMu, sigmas);
 }
 
 /* ************************************************************************* */

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<EFBFBD>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!

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!!!
+  if (noiseModel_->is_constrained())
-  noiseModel::Constrained::shared_ptr constrained = //
+    return GaussianFactor::shared_ptr(
-      boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
+        new JacobianFactor(terms, b,
-  if (constrained) {
+            boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()));
-    // Create a factor of reduced row dimension d_
+  else
-    size_t d_ = b.size() - constrained->dim();
+    return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
-    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);
 }
 
 /* ************************************************************************* */

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_