Made unsafe constructor private, but made ExpressionFactor<T> a friend.

gtsam/linear/JacobianFactor.h

@@ -82,20 +82,22 @@ namespace gtsam {
   class GTSAM_EXPORT JacobianFactor : public GaussianFactor
   {
   public:
+
     typedef JacobianFactor This; ///< Typedef to this class
     typedef GaussianFactor Base; ///< Typedef to base class
     typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
 
-  protected:
-    VerticalBlockMatrix Ab_;      // the block view of the full matrix
-    noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
-
-  public:
     typedef VerticalBlockMatrix::Block ABlock;
     typedef VerticalBlockMatrix::constBlock constABlock;
     typedef ABlock::ColXpr BVector;
     typedef constABlock::ConstColXpr constBVector;
 
+  protected:
+
+    VerticalBlockMatrix Ab_;      // the block view of the full matrix
+    noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
+
+  public:
 
     /** Convert from other GaussianFactor */
     explicit JacobianFactor(const GaussianFactor& gf);
@@ -140,25 +142,6 @@ namespace gtsam {
     JacobianFactor(
       const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal());
 
-    /**
-     *  Constructor
-     *  @param keys in some order
-     *  @param diemnsions of the variables in same order
-     *  @param m output dimension
-     *  @param model noise model (default NULL)
-     */
-    template<class KEYS, class DIMENSIONS>
-    JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
-        const SharedDiagonal& model = SharedDiagonal()) :
-        Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
-      Ab_.matrix().setZero();
-    }
-
-    /// Direct access to VerticalBlockMatrix
-    VerticalBlockMatrix& Ab() {
-      return Ab_;
-    }
-
     /**
      * Build a dense joint factor from all the factors in a factor graph.  If a VariableSlots
      * structure computed for \c graph is already available, providing it will reduce the amount of
@@ -347,6 +330,21 @@ namespace gtsam {
     
   private:
 
+    /** Unsafe Constructor that creates an uninitialized Jacobian of right size
+     *  @param keys in some order
+     *  @param diemnsions of the variables in same order
+     *  @param m output dimension
+     *  @param model noise model (default NULL)
+     */
+    template<class KEYS, class DIMENSIONS>
+    JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
+        const SharedDiagonal& model = SharedDiagonal()) :
+        Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
+    }
+
+    // be very selective on who can access these private methods:
+    template<typename T> friend class ExpressionFactor;
+
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -107,24 +107,26 @@ public:
 
   virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
 
-    // Create noise model
-    SharedDiagonal model;
+    // Check whether noise model is constrained or not
     noiseModel::Constrained::shared_ptr constrained = //
         boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
-    if (constrained)
-      model = constrained->unit();
 
     // Create a writeable JacobianFactor in advance
     boost::shared_ptr<JacobianFactor> factor(
-        new JacobianFactor(keys_, dimensions_, traits::dimension<T>::value,
-            model));
+        constrained ? new JacobianFactor(keys_, dimensions_, Dim,
+            constrained->unit()) :
+            new JacobianFactor(keys_, dimensions_, Dim));
 
     // Wrap keys and VerticalBlockMatrix into structure passed to expression_
-    JacobianMap map(keys_, factor->Ab());
+    VerticalBlockMatrix& Ab = factor->matrixObject();
+    JacobianMap map(keys_, Ab);
+
+    // Zero out Jacobian so we can simply add to it
+    Ab.matrix().setZero();
 
     // Evaluate error to get Jacobians and RHS vector b
     T value = expression_.value(x, map); // <<< Reverse AD happens here !
-    factor->Ab()(size()).col(0) = -measurement_.localCoordinates(value);
+    Ab(size()).col(0) = -measurement_.localCoordinates(value);
 
     // Whiten the corresponding system now
     // TODO ! this->noiseModel_->WhitenSystem(Ab);

gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp

@@ -423,19 +423,6 @@ TEST(ExpressionFactor, composeTernary) {
   EXPECT( assert_equal(expected, *jf,1e-9));
 }
 
-/* ************************************************************************* */
-// Test Writeable JacobianFactor
-TEST(ExpressionFactor, WriteableJacobianFactor) {
-  std::list<size_t> keys = list_of(1)(2);
-  vector<size_t> dimensions(2);
-  dimensions[0] = 6;
-  dimensions[1] = 3;
-  SharedDiagonal model;
-  JacobianFactor actual(keys, dimensions, 2, model);
-  JacobianFactor expected(1, zeros(2, 6), 2, zeros(2, 3), zero(2));
-  EXPECT( assert_equal(expected, actual,1e-9));
-}
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;