From 649478f18608d220f2e3b86095ee44506dfbca16 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 15 Oct 2014 01:19:14 +0200
Subject: [PATCH] Should work but seg-faults

---
 gtsam/linear/NoiseModel.h                   | 10 ++++++++
 gtsam_unstable/nonlinear/ExpressionFactor.h | 27 ++++++++++++---------
 2 files changed, 25 insertions(+), 12 deletions(-)

diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index 4b0e4848d..597ebe1dd 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_; }
 
diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h
index b6bfba27f..a0e843935 100644
--- a/gtsam_unstable/nonlinear/ExpressionFactor.h
+++ b/gtsam_unstable/nonlinear/ExpressionFactor.h
@@ -63,11 +63,11 @@ public:
 
       // Create and zero out blocks to be passed to expression_
       JacobianMap blocks;
-      for(DenseIndex i=0;i<size();i++) {
+      for (DenseIndex i = 0; i < size(); i++) {
         Matrix& Hi = H->at(i);
         Hi.resize(T::dimension, dims[i]);
         Hi.setZero(); // zero out
-        Eigen::Block<Matrix> block = Hi.block(0,0,T::dimension, dims[i]);
+        Eigen::Block<Matrix> block = Hi.block(0, 0, T::dimension, dims[i]);
         blocks.insert(std::make_pair(keys_[i], block));
       }
 
@@ -87,17 +87,18 @@ public:
     std::vector<size_t> dims = expression_.dimensions();
 
     // Allocate memory on stack and create a view on it (saves a malloc)
-    size_t m1 = std::accumulate(dims.begin(),dims.end(),1);
-    double memory[T::dimension*m1];
-    Eigen::Map<Eigen::Matrix<double,T::dimension,Eigen::Dynamic> > matrix(memory,T::dimension,m1);
+    size_t m1 = std::accumulate(dims.begin(), dims.end(), 1);
+    double memory[T::dimension * m1];
+    Eigen::Map<Eigen::Matrix<double, T::dimension, Eigen::Dynamic> > matrix(
+        memory, T::dimension, m1);
     matrix.setZero(); // zero out
 
-    // Construct block matrix, is of right size but un-initialized
+    // Construct block matrix, is then of right and initialized to zero
     VerticalBlockMatrix Ab(dims, matrix, true);
 
     // Create blocks to be passed to expression_
     JacobianMap blocks;
-    for(DenseIndex i=0;i<size();i++)
+    for (DenseIndex i = 0; i < size(); i++)
       blocks.insert(std::make_pair(keys_[i], Ab(i)));
     // Evaluate error to get Jacobians and RHS vector b
     T value = expression_.value(x, blocks);
@@ -108,11 +109,13 @@ 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 boost::make_shared<JacobianFactor>(this->keys(), Ab,
-          constrained->unit());
+    if (noiseModel_->is_constrained()) {
+      noiseModel::Constrained::shared_ptr p = //
+          boost::dynamic_pointer_cast<noiseModel::Constrained>(noiseModel_);
+      if (!p)
+        throw std::invalid_argument(
+            "ExpressionFactor: constrained NoiseModel cast failed.");
+      return boost::make_shared<JacobianFactor>(this->keys(), Ab, p->unit());
     } else
       return boost::make_shared<JacobianFactor>(this->keys(), Ab);
   }