To simplify FactorGraph, removed convertCastFactors and dynamicCastFactors from FactorGraph - replaced their calls with in-place code to do the needed conversions

gtsam/inference/FactorGraph.h

@@ -200,39 +200,6 @@ template<class CONDITIONAL, class CLIQUE> class BayesTree;
 		/** return the number valid factors */
 		size_t nrFactors() const;
 
-		/** dynamic_cast the factor pointers down or up the class hierarchy */
-		template<class RELATED>
-		typename RELATED::shared_ptr dynamicCastFactors() const {
-		  typename RELATED::shared_ptr ret(new RELATED);
-		  ret->reserve(this->size());
-		  BOOST_FOREACH(const sharedFactor& factor, *this) {
-		    typename RELATED::FactorType::shared_ptr castedFactor(boost::dynamic_pointer_cast<typename RELATED::FactorType>(factor));
-		    if(castedFactor)
-		      ret->push_back(castedFactor);
-		    else
-		      throw std::invalid_argument("In FactorGraph<FACTOR>::dynamic_factor_cast(), dynamic_cast failed, meaning an invalid cast was requested.");
-		  }
-		  return ret;
-		}
-
-		/**
-		 * dynamic_cast factor pointers if possible, otherwise convert with a
-		 * constructor of the target type.
-		 */
-		template<class TARGET>
-		typename TARGET::shared_ptr convertCastFactors() const {
-      typename TARGET::shared_ptr ret(new TARGET);
-      ret->reserve(this->size());
-      BOOST_FOREACH(const sharedFactor& factor, *this) {
-        typename TARGET::FactorType::shared_ptr castedFactor(boost::dynamic_pointer_cast<typename TARGET::FactorType>(factor));
-        if(castedFactor)
-          ret->push_back(castedFactor);
-        else
-          ret->push_back(typename TARGET::FactorType::shared_ptr(new typename TARGET::FactorType(*factor)));
-      }
-      return ret;
-    }
-
 	private:
 
 		/** Serialization function */

gtsam/inference/tests/testFactorGraph.cpp

@@ -91,12 +91,6 @@ typedef boost::shared_ptr<SymbolicFactorGraph> shared;
 //	CHECK(singletonGraph_excepted.equals(singletonGraph));
 //}
 
-/* ************************************************************************* */
-TEST(FactorGraph, dynamic_factor_cast) {
-  FactorGraph<IndexFactor> fg;
-  fg.dynamicCastFactors<FactorGraph<IndexFactor> >();
-}
-
 /* ************************************************************************* */
 int main() {	TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/linear/GaussianConditional.h

@@ -80,7 +80,7 @@ public:
 	GaussianConditional();
 
 	/** constructor */
-	GaussianConditional(Index key);
+	explicit GaussianConditional(Index key);
 
 	/** constructor with no parents
 	 * |Rx-d|

gtsam/linear/GaussianFactorGraph.cpp

@@ -160,9 +160,18 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	Matrix GaussianFactorGraph::denseJacobian() const {
+    // Convert to Jacobians
+    FactorGraph<JacobianFactor> jfg;
+    jfg.reserve(this->size());
+    BOOST_FOREACH(const sharedFactor& factor, *this) {
+      if(boost::shared_ptr<JacobianFactor> jf =
+        boost::dynamic_pointer_cast<JacobianFactor>(factor))
+        jfg.push_back(jf);
+      else
+        jfg.push_back(boost::make_shared<JacobianFactor>(*factor));
+    }
 		// combine all factors
-		JacobianFactor combined(*CombineJacobians(*convertCastFactors<FactorGraph<
+		JacobianFactor combined(*CombineJacobians(jfg, VariableSlots(*this)));
-				JacobianFactor> > (), VariableSlots(*this)));
 		return combined.matrix_augmented();
 	}
 

gtsam/linear/HessianFactor.h

@@ -197,10 +197,10 @@ namespace gtsam {
         const std::vector<Vector>& gs, double f);
 
     /** Construct from Conditional Gaussian */
-    HessianFactor(const GaussianConditional& cg);
+    explicit HessianFactor(const GaussianConditional& cg);
 
     /** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */
-    HessianFactor(const GaussianFactor& factor);
+    explicit HessianFactor(const GaussianFactor& factor);
 
     /** Special constructor used in EliminateCholesky which combines the given factors */
     HessianFactor(const FactorGraph<GaussianFactor>& factors,

gtsam/linear/JacobianFactor.cpp

@@ -66,6 +66,18 @@ namespace gtsam {
     assertInvariants();
   }
 
+  /* ************************************************************************* */
+  JacobianFactor::JacobianFactor(const GaussianFactor& gf) : Ab_(matrix_) {
+    // Copy the matrix data depending on what type of factor we're copying from
+    if(const JacobianFactor* rhs = dynamic_cast<const JacobianFactor*>(&gf))
+      *this = JacobianFactor(*rhs);
+    else if(const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&gf))
+      *this = JacobianFactor(*rhs);
+    else
+      throw std::invalid_argument("In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
+    assertInvariants();
+  }
+
   /* ************************************************************************* */
   JacobianFactor::JacobianFactor() : Ab_(matrix_) { assertInvariants(); }
 

gtsam/linear/JacobianFactor.h

@@ -102,6 +102,9 @@ namespace gtsam {
     /** Copy constructor */
     JacobianFactor(const JacobianFactor& gf);
 
+    /** Convert from other GaussianFactor */
+    JacobianFactor(const GaussianFactor& gf);
+
     /** default constructor for I/O */
     JacobianFactor();
 

gtsam/linear/SubgraphSolver.cpp

@@ -52,10 +52,29 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters
   GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
   VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
 
-  pc_ = boost::make_shared<SubgraphPreconditioner>(
+  // Convert or cast Ab1 to JacobianFactors
-      Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(),
+  boost::shared_ptr<FactorGraph<JacobianFactor> > Ab1Jacobians = boost::make_shared<FactorGraph<JacobianFactor> >();
-      Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),
+  Ab1Jacobians->reserve(Ab1->size());
-      Rc1, xbar);
+  BOOST_FOREACH(const boost::shared_ptr<GaussianFactor>& factor, *Ab1) {
+    if(boost::shared_ptr<JacobianFactor> jf =
+      boost::dynamic_pointer_cast<JacobianFactor>(factor))
+      Ab1Jacobians->push_back(jf);
+    else
+      Ab1Jacobians->push_back(boost::make_shared<JacobianFactor>(*factor));
+  }
+
+  // Convert or cast Ab2 to JacobianFactors
+  boost::shared_ptr<FactorGraph<JacobianFactor> > Ab2Jacobians = boost::make_shared<FactorGraph<JacobianFactor> >();
+  Ab1Jacobians->reserve(Ab2->size());
+  BOOST_FOREACH(const boost::shared_ptr<GaussianFactor>& factor, *Ab2) {
+    if(boost::shared_ptr<JacobianFactor> jf =
+      boost::dynamic_pointer_cast<JacobianFactor>(factor))
+      Ab2Jacobians->push_back(jf);
+    else
+      Ab2Jacobians->push_back(boost::make_shared<JacobianFactor>(*factor));
+  }
+
+  pc_ = boost::make_shared<SubgraphPreconditioner>(Ab1Jacobians, Ab2Jacobians, Rc1, xbar);
 }
 
 VectorValues SubgraphSolver::optimize() {

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -50,10 +50,8 @@ TEST(GaussianFactorGraph, initialization) {
 	fg.add(0, -5*eye(2), 2, 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
 	fg.add(1, -5*eye(2), 2, 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
 
-	FactorGraph<JacobianFactor> graph = *fg.dynamicCastFactors<FactorGraph<JacobianFactor> >();
+	EXPECT_LONGS_EQUAL(4, fg.size());
-
+	JacobianFactor factor = *boost::dynamic_pointer_cast<JacobianFactor>(fg[0]);
-	EXPECT_LONGS_EQUAL(4, graph.size());
-	JacobianFactor factor = *graph[0];
 
 	// Test sparse, which takes a vector and returns a matrix, used in MATLAB
 	// Note that this the augmented vector and the RHS is in column 7
@@ -155,8 +153,14 @@ TEST(GaussianFactorGraph, Combine2)
   gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
   gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
 
-  JacobianFactor actual = *CombineJacobians(*gfg.dynamicCastFactors<FactorGraph<
+  // Convert to Jacobians, inefficient copy of all factors instead of selectively converting only Hessians
-			JacobianFactor> > (), VariableSlots(gfg));
+  FactorGraph<JacobianFactor> jacobians;
+  BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) {
+    jacobians.push_back(boost::make_shared<JacobianFactor>(*factor));
+  }
+
+  // Combine Jacobians into a single dense factor
+  JacobianFactor actual = *CombineJacobians(jacobians, VariableSlots(gfg));
 
   Matrix zero3x3 = zeros(3,3);
   Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
@@ -213,8 +217,7 @@ TEST(GaussianFactor, CombineAndEliminate)
 
   GaussianConditional::shared_ptr actualBN;
 	GaussianFactor::shared_ptr actualFactor;
-	boost::tie(actualBN, actualFactor) = //
+	boost::tie(actualBN, actualFactor) = EliminateQR(gfg, 1);
-			EliminateQR(*gfg.dynamicCastFactors<FactorGraph<JacobianFactor> > (), 1);
 	JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast<
 			JacobianFactor>(actualFactor);
 
@@ -410,9 +413,14 @@ TEST(GaussianFactor, eliminateFrontals)
   Matrix actualDense = factors.denseJacobian();
   EXPECT(assert_equal(2.0 * Ab, actualDense));
 
+  // Convert to Jacobians, inefficient copy of all factors instead of selectively converting only Hessians
+  FactorGraph<JacobianFactor> jacobians;
+  BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, factors) {
+    jacobians.push_back(boost::make_shared<JacobianFactor>(*factor));
+  }
+
   // Create combined factor
-  JacobianFactor combined(*CombineJacobians(*factors.dynamicCastFactors<FactorGraph<
+  JacobianFactor combined(*CombineJacobians(jacobians, VariableSlots(factors)));
-			JacobianFactor> > (), VariableSlots(factors)));
 
   // Copies factors as they will be eliminated in place
   JacobianFactor actualFactor_QR = combined;

gtsam/linear/tests/testHessianFactor.cpp

@@ -414,10 +414,20 @@ TEST_UNSAFE(HessianFactor, CombineAndEliminate)
   // create expected Hessian after elimination
   HessianFactor expectedCholeskyFactor(expectedFactor);
 
-  GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(
+  // Convert all factors to hessians
-			*gfg.convertCastFactors<FactorGraph<HessianFactor> > (), 1);
+  FactorGraph<HessianFactor> hessians;
-	HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<
+  BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, gfg) {
-			HessianFactor>(actualCholesky.second);
+    if(boost::shared_ptr<HessianFactor> hf = boost::dynamic_pointer_cast<HessianFactor>(factor))
+      hessians.push_back(hf);
+    else if(boost::shared_ptr<JacobianFactor> jf = boost::dynamic_pointer_cast<JacobianFactor>(factor))
+      hessians.push_back(boost::make_shared<HessianFactor>(*jf));
+    else
+      CHECK(false);
+  }
+
+  // Eliminate
+  GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(gfg, 1);
+	HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<HessianFactor>(actualCholesky.second);
 
 	EXPECT(assert_equal(*expectedBN, *actualCholesky.first, 1e-6));
   EXPECT(assert_equal(expectedCholeskyFactor, *actualFactor, 1e-6));

gtsam/nonlinear/GradientDescentOptimizer.cpp

@@ -25,10 +25,16 @@ void gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values, cons
 
   // Linearize graph
   GaussianFactorGraph::shared_ptr linear = nfg.linearize(values, ordering);
-  const FactorGraph<JacobianFactor>::shared_ptr jfg = linear->dynamicCastFactors<FactorGraph<JacobianFactor> >();
+  FactorGraph<JacobianFactor> jfg;  jfg.reserve(linear->size());
+  BOOST_FOREACH(const GaussianFactorGraph::sharedFactor& factor, *linear) {
+    if(boost::shared_ptr<JacobianFactor> jf = boost::dynamic_pointer_cast<JacobianFactor>(factor))
+      jfg.push_back((jf));
+    else
+      jfg.push_back(boost::make_shared<JacobianFactor>(*factor));
+  }
 
   // compute the gradient direction
-  gradientAtZero(*jfg, g);
+  gradientAtZero(jfg, g);
 }
 
 

gtsam/nonlinear/NonlinearISAM.cpp

@@ -58,8 +58,7 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors,
       BOOST_FOREACH(Key key, factor->keys())
         ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present
 
-    boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(
+    boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors = newFactors.linearize(linPoint_, ordering_);
-        newFactors.linearize(linPoint_, ordering_)->dynamicCastFactors<GaussianFactorGraph>());
 
     // Update ISAM
     isam_.update(*linearizedNewFactors);
@@ -81,8 +80,7 @@ void NonlinearISAM::reorder_relinearize() {
     ordering_ = *factors_.orderingCOLAMD(newLinPoint);
 
     // Create a linear factor graph at the new linearization point
-    boost::shared_ptr<GaussianFactorGraph> gfg(
+    boost::shared_ptr<GaussianFactorGraph> gfg = factors_.linearize(newLinPoint, ordering_);
-        factors_.linearize(newLinPoint, ordering_)->dynamicCastFactors<GaussianFactorGraph>());
 
     // Just recreate the whole BayesTree
     isam_.update(*gfg);

gtsam/slam/tests/testPlanarSLAM.cpp

@@ -174,12 +174,10 @@ TEST( planarSLAM, constructor )
 	Vector expected2 = Vector_(1, -0.1);
 	Vector expected3 = Vector_(1, 0.22);
 	// Get NoiseModelFactors
-	FactorGraph<NoiseModelFactor > GNM =
+	EXPECT(assert_equal(expected0, boost::dynamic_pointer_cast<NoiseModelFactor>(G[0])->unwhitenedError(c)));
-	    *G.dynamicCastFactors<FactorGraph<NoiseModelFactor > >();
+  EXPECT(assert_equal(expected1, boost::dynamic_pointer_cast<NoiseModelFactor>(G[1])->unwhitenedError(c)));
-	EXPECT(assert_equal(expected0, GNM[0]->unwhitenedError(c)));
+  EXPECT(assert_equal(expected2, boost::dynamic_pointer_cast<NoiseModelFactor>(G[2])->unwhitenedError(c)));
-  EXPECT(assert_equal(expected1, GNM[1]->unwhitenedError(c)));
+  EXPECT(assert_equal(expected3, boost::dynamic_pointer_cast<NoiseModelFactor>(G[3])->unwhitenedError(c)));
-  EXPECT(assert_equal(expected2, GNM[2]->unwhitenedError(c)));
-  EXPECT(assert_equal(expected3, GNM[3]->unwhitenedError(c)));
 }
 
 /* ************************************************************************* */

tests/smallExample.cpp

@@ -138,23 +138,23 @@ namespace example {
 	/* ************************************************************************* */
 	JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
 		// Create empty graph
-		GaussianFactorGraph fg;
+		JacobianFactorGraph fg;
 
 		SharedDiagonal unit2 = noiseModel::Unit::Create(2);
 
 		// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
-		fg.add(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2);
+		fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2));
 
 		// odometry between x1 and x2: x2-x1=[0.2;-0.1]
-		fg.add(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2);
+		fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2));
 
     // measurement between x1 and l1: l1-x1=[0.0;0.2]
-		fg.add(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2);
+		fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2));
 
 		// measurement between x2 and l1: l1-x2=[-0.2;0.3]
-		fg.add(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
+		fg.push_back(boost::make_shared<JacobianFactor>(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2));
 
-		return *fg.dynamicCastFactors<JacobianFactorGraph >();
+		return fg;
 	}
 
 	/* ************************************************************************* */