Returning GaussianFactor instead of JacobianFactor from NonlinearFactor::linearize and NonlinearFactorGraph::linearize, so that HessianFactors may be returned as well.

gtsam/nonlinear/NonlinearEquality.h

@@ -130,13 +130,13 @@ namespace gtsam {
 		}
 
 		// Linearize is over-written, because base linearization tries to whiten
-		virtual JacobianFactor::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const {
+		virtual GaussianFactor::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const {
 			const T& xj = x[this->key_];
 			Matrix A;
 			Vector b = evaluateError(xj, A);
 			// TODO pass unwhitened + noise model to Gaussian factor
 			SharedDiagonal model = noiseModel::Constrained::All(b.size());
-			return JacobianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model));
+			return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model));
 		}
 
 	private:

gtsam/nonlinear/NonlinearFactor.h

@@ -145,7 +145,7 @@ namespace gtsam {
 		}
 
 		/** linearize to a GaussianFactor */
-		virtual boost::shared_ptr<JacobianFactor>
+		virtual boost::shared_ptr<GaussianFactor>
 		linearize(const VALUES& c, const Ordering& ordering) const = 0;
 
 		/**
@@ -237,7 +237,7 @@ namespace gtsam {
 		 * Ax-b \approx h(x0+dx)-z = h(x0) + A*dx - z
 		 * Hence b = z - h(x0) = - error_vector(x)
 		 */
-		virtual boost::shared_ptr<JacobianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
+		virtual boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
 			const X& xj = x[key_];
 			Matrix A;
 			Vector b = - evaluateError(xj, A);
@@ -246,10 +246,10 @@ namespace gtsam {
 			SharedDiagonal constrained =
 					boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
 			if (constrained.get() != NULL)
-				return JacobianFactor::shared_ptr(new JacobianFactor(var, A, b, constrained));
+				return GaussianFactor::shared_ptr(new JacobianFactor(var, A, b, constrained));
 			this->noiseModel_->WhitenInPlace(A);
 			this->noiseModel_->whitenInPlace(b);
-			return JacobianFactor::shared_ptr(new JacobianFactor(var, A, b,
+			return GaussianFactor::shared_ptr(new JacobianFactor(var, A, b,
 					noiseModel::Unit::Create(b.size())));
 		}
 
@@ -348,7 +348,7 @@ namespace gtsam {
 		 * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z
 		 * Hence b = z - h(x1,x2) = - error_vector(x)
 		 */
-		boost::shared_ptr<JacobianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
+		boost::shared_ptr<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
 			const X1& x1 = c[key1_];
 			const X2& x2 = c[key2_];
 			Matrix A1, A2;
@@ -365,10 +365,10 @@ namespace gtsam {
 			this->noiseModel_->WhitenInPlace(A2);
 			this->noiseModel_->whitenInPlace(b);
 			if(var1 < var2)
-			  return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, var2,
+			  return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, var2,
 			      A2, b, noiseModel::Unit::Create(b.size())));
 			else
-			  return JacobianFactor::shared_ptr(new JacobianFactor(var2, A2, var1,
+			  return GaussianFactor::shared_ptr(new JacobianFactor(var2, A2, var1,
 			      A1, b, noiseModel::Unit::Create(b.size())));
 		}
 
@@ -492,7 +492,7 @@ namespace gtsam {
      * Ax-b \approx h(x1+dx1,x2+dx2,x3+dx3)-z = h(x1,x2,x3) + A2*dx1 + A2*dx2 + A3*dx3 - z
      * Hence b = z - h(x1,x2,x3) = - error_vector(x)
      */
-    boost::shared_ptr<JacobianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
+    boost::shared_ptr<GaussianFactor> linearize(const VALUES& c, const Ordering& ordering) const {
       const X1& x1 = c[key1_];
       const X2& x2 = c[key2_];
       const X3& x3 = c[key3_];
@@ -503,7 +503,7 @@ namespace gtsam {
       SharedDiagonal constrained =
           boost::shared_dynamic_cast<noiseModel::Constrained>(this->noiseModel_);
       if (constrained.get() != NULL) {
-        return JacobianFactor::shared_ptr(
+        return GaussianFactor::shared_ptr(
             new JacobianFactor(var1, A1, var2, A2, var3, A3, b, constrained));
       }
       this->noiseModel_->WhitenInPlace(A1);
@@ -511,26 +511,26 @@ namespace gtsam {
       this->noiseModel_->WhitenInPlace(A3);
       this->noiseModel_->whitenInPlace(b);
       if(var1 < var2 && var2 < var3)
-        return JacobianFactor::shared_ptr(
+        return GaussianFactor::shared_ptr(
             new JacobianFactor(var1, A1, var2, A2, var3, A3, b, noiseModel::Unit::Create(b.size())));
       else if(var2 < var1 && var1 < var3)
-        return JacobianFactor::shared_ptr(
+        return GaussianFactor::shared_ptr(
             new JacobianFactor(var2, A2, var1, A1, var3, A3, b, noiseModel::Unit::Create(b.size())));
       else if(var1 < var3 && var3 < var2)
-        return JacobianFactor::shared_ptr(
+        return GaussianFactor::shared_ptr(
             new JacobianFactor(var1, A1, var3, A3, var2, A2, b, noiseModel::Unit::Create(b.size())));
       else if(var2 < var3 && var3 < var1)
-        return JacobianFactor::shared_ptr(
+        return GaussianFactor::shared_ptr(
             new JacobianFactor(var2, A2, var3, A3, var1, A1, b, noiseModel::Unit::Create(b.size())));
       else if(var3 < var1 && var1 < var2)
-        return JacobianFactor::shared_ptr(
+        return GaussianFactor::shared_ptr(
             new JacobianFactor(var3, A3, var1, A1, var2, A2, b, noiseModel::Unit::Create(b.size())));
       else if(var3 < var2 && var2 < var1)
-        return JacobianFactor::shared_ptr(
+        return GaussianFactor::shared_ptr(
             new JacobianFactor(var3, A3, var2, A2, var1, A1, b, noiseModel::Unit::Create(b.size())));
       else {
         assert(false);
-        return JacobianFactor::shared_ptr();
+        return GaussianFactor::shared_ptr();
       }
     }
 

gtsam/nonlinear/NonlinearFactorGraph-inl.h

@@ -139,21 +139,20 @@ namespace gtsam {
 
 	/* ************************************************************************* */
 	template<class VALUES>
-	typename FactorGraph<JacobianFactor>::shared_ptr NonlinearFactorGraph<VALUES>::linearize(
+	typename FactorGraph<GaussianFactor>::shared_ptr NonlinearFactorGraph<VALUES>::linearize(
 			const VALUES& config, const Ordering& ordering) const {
 
 		// create an empty linear FG
-		typename FactorGraph<JacobianFactor>::shared_ptr linearFG(new FactorGraph<
+		typename FactorGraph<GaussianFactor>::shared_ptr linearFG(new FactorGraph<GaussianFactor>);
-				JacobianFactor> );
 		linearFG->reserve(this->size());
 
 		// linearize all factors
 		BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
 		  if(factor) {
-		    JacobianFactor::shared_ptr lf = factor->linearize(config, ordering);
+		    GaussianFactor::shared_ptr lf = factor->linearize(config, ordering);
 		    if (lf) linearFG->push_back(lf);
 		  } else
-		    linearFG->push_back(FactorGraph<JacobianFactor>::sharedFactor());
+		    linearFG->push_back(GaussianFactor::shared_ptr());
 		}
 
 		return linearFG;

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -89,7 +89,7 @@ namespace gtsam {
 		/**
 		 * linearize a nonlinear factor graph
 		 */
-		boost::shared_ptr<FactorGraph<JacobianFactor> >
+		boost::shared_ptr<FactorGraph<GaussianFactor> >
 				linearize(const VALUES& config, const Ordering& ordering) const;
 
 	private:

gtsam/slam/smallExample.cpp

@@ -273,7 +273,7 @@ namespace example {
 	}
 
 	/* ************************************************************************* */
-	pair<FactorGraph<JacobianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering) {
+	pair<FactorGraph<GaussianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering) {
 		Graph nlfg;
 		Values poses;
 		boost::tie(nlfg, poses) = createNonlinearSmoother(T);
@@ -431,7 +431,7 @@ namespace example {
 	}
 
 	/* ************************************************************************* */
-	boost::tuple<FactorGraph<JacobianFactor>, Ordering, VectorValues> planarGraph(size_t N) {
+	boost::tuple<FactorGraph<GaussianFactor>, Ordering, VectorValues> planarGraph(size_t N) {
 
 		// create empty graph
 		NonlinearFactorGraph<Values> nlfg;

gtsam/slam/smallExample.h

@@ -93,7 +93,7 @@ namespace gtsam {
 		 * Create a Kalman smoother by linearizing a non-linear factor graph
 		 * @param T number of time-steps
 		 */
-		std::pair<FactorGraph<JacobianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering = boost::none);
+		std::pair<FactorGraph<GaussianFactor>, Ordering> createSmoother(int T, boost::optional<Ordering> ordering = boost::none);
 
 		/* ******************************************************* */
 		// Linear Constrained Examples
@@ -133,7 +133,7 @@ namespace gtsam {
 		 * -x11-x21-x31
 		 * with x11 clamped at (1,1), and others related by 2D odometry.
 		 */
-		boost::tuple<FactorGraph<JacobianFactor>, Ordering, VectorValues> planarGraph(size_t N);
+		boost::tuple<FactorGraph<GaussianFactor>, Ordering, VectorValues> planarGraph(size_t N);
 
 		/*
 		 * Create canonical ordering for planar graph that also works for tree

gtsam/slam/tests/testPose2Factor.cpp

@@ -52,7 +52,8 @@ TEST( Pose2Factor, error )
 
 	// Actual linearization
 	Ordering ordering(*x0.orderingArbitrary());
-	boost::shared_ptr<JacobianFactor> linear = factor.linearize(x0, ordering);
+	boost::shared_ptr<JacobianFactor> linear =
+	    boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
 
 	// Check error at x0, i.e. delta = zero !
 	VectorValues delta(x0.dims(ordering));
@@ -88,7 +89,8 @@ TEST( Pose2Factor, rhs )
 
 	// Actual linearization
 	Ordering ordering(*x0.orderingArbitrary());
-	boost::shared_ptr<JacobianFactor> linear = factor.linearize(x0, ordering);
+	boost::shared_ptr<JacobianFactor> linear =
+	    boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
 
 	// Check RHS
 	Pose2 hx0 = p1.between(p2);
@@ -134,7 +136,8 @@ TEST( Pose2Factor, linearize )
 	JacobianFactor expected(ordering["x1"], expectedH1, ordering["x2"], expectedH2, expected_b, probModel1);
 
 	// Actual linearization
-	boost::shared_ptr<JacobianFactor> actual = factor.linearize(x0, ordering);
+	boost::shared_ptr<JacobianFactor> actual =
+	    boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
 	CHECK(assert_equal(expected,*actual));
 
 	// Numerical do not work out because BetweenFactor is approximate ?

gtsam/slam/tests/testPose2Prior.cpp

@@ -44,7 +44,8 @@ TEST( Pose2Prior, error )
 
 	// Actual linearization
 	Ordering ordering(*x0.orderingArbitrary());
-	boost::shared_ptr<JacobianFactor> linear = factor.linearize(x0, ordering);
+	boost::shared_ptr<JacobianFactor> linear =
+	    boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
 
 	// Check error at x0, i.e. delta = zero !
 	VectorValues delta(x0.dims(ordering));
@@ -86,7 +87,8 @@ TEST( Pose2Prior, linearize )
 
 	// Actual linearization
 	Ordering ordering(*x0.orderingArbitrary());
-	boost::shared_ptr<JacobianFactor> actual = factor.linearize(x0, ordering);
+	boost::shared_ptr<JacobianFactor> actual =
+	    boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(x0, ordering));
 
 	// Test with numerical derivative
 	Matrix numericalH = numericalDerivative11(h, prior);

gtsam/slam/tests/testPose2SLAM.cpp

@@ -74,11 +74,11 @@ TEST( Pose2Graph, linearization )
 	config.insert(2,p2);
 	// Linearize
 	Ordering ordering(*config.orderingArbitrary());
-	boost::shared_ptr<FactorGraph<JacobianFactor> > lfg_linearized = graph.linearize(config, ordering);
+	boost::shared_ptr<FactorGraph<GaussianFactor> > lfg_linearized = graph.linearize(config, ordering);
 	//lfg_linearized->print("lfg_actual");
 
 	// the expected linear factor
-	FactorGraph<JacobianFactor> lfg_expected;
+	FactorGraph<GaussianFactor> lfg_expected;
 	Matrix A1 = Matrix_(3,3,
 	    0.0,-2.0, -4.2,
 	    2.0, 0.0, -4.2,

gtsam/slam/tests/testVSLAMFactor.cpp

@@ -67,15 +67,16 @@ TEST( ProjectionFactor, error )
 	Vector b = Vector_(2,3.,0.);
 	SharedDiagonal probModel1 = noiseModel::Unit::Create(2);
 	JacobianFactor expected(ordering["x1"], Ax1, ordering["l1"], Al1, b, probModel1);
-	JacobianFactor::shared_ptr actual = factor->linearize(config, ordering);
+	JacobianFactor::shared_ptr actual =
+	    boost::dynamic_pointer_cast<JacobianFactor>(factor->linearize(config, ordering));
 	CHECK(assert_equal(expected,*actual,1e-3));
 
 	// linearize graph
 	Graph graph;
 	graph.push_back(factor);
-	FactorGraph<JacobianFactor> expected_lfg;
+	FactorGraph<GaussianFactor> expected_lfg;
 	expected_lfg.push_back(actual);
-	boost::shared_ptr<FactorGraph<JacobianFactor> > actual_lfg = graph.linearize(config, ordering);
+	boost::shared_ptr<FactorGraph<GaussianFactor> > actual_lfg = graph.linearize(config, ordering);
 	CHECK(assert_equal(expected_lfg,*actual_lfg));
 
 	// expmap on a config

tests/testInference.cpp

@@ -85,7 +85,7 @@ TEST( Inference, marginals2)
   init.insert(PointKey(0), Point2(1.0,1.0));
 
   Ordering ordering(*fg.orderingCOLAMD(init));
-  FactorGraph<JacobianFactor>::shared_ptr gfg(fg.linearize(init, ordering));
+  FactorGraph<GaussianFactor>::shared_ptr gfg(fg.linearize(init, ordering));
   GaussianMultifrontalSolver solver(*gfg);
   solver.marginalFactor(ordering[PointKey(0)]);
 }

tests/testNonlinearEquality.cpp

@@ -50,8 +50,8 @@ TEST ( NonlinearEquality, linearization ) {
 	// check linearize
 	SharedDiagonal constraintModel = noiseModel::Constrained::All(3);
 	JacobianFactor expLF(0, eye(3), zero(3), constraintModel);
-	JacobianFactor::shared_ptr actualLF = nle->linearize(linearize, *linearize.orderingArbitrary());
+	GaussianFactor::shared_ptr actualLF = nle->linearize(linearize, *linearize.orderingArbitrary());
-	EXPECT(assert_equal(*actualLF, expLF));
+	EXPECT(assert_equal(*actualLF, (const GaussianFactor&)expLF));
 }
 
 /* ********************************************************************** */

tests/testNonlinearFactor.cpp

@@ -200,13 +200,13 @@ TEST( NonlinearFactor, linearize_constraint1 )
 
 	Values config;
 	config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
-	JacobianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary());
+	GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary());
 
 	// create expected
 	Ordering ord(*config.orderingArbitrary());
 	Vector b = Vector_(2, 0., -3.);
 	JacobianFactor expected(ord["x1"], eye(2), b, constraint);
-	CHECK(assert_equal(expected, *actual));
+	CHECK(assert_equal((const GaussianFactor&)expected, *actual));
 }
 
 /* ************************************************************************* */
@@ -221,13 +221,13 @@ TEST( NonlinearFactor, linearize_constraint2 )
 	Values config;
 	config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0));
 	config.insert(simulated2D::PointKey(1), Point2(5.0, 4.0));
-	JacobianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary());
+	GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary());
 
 	// create expected
 	Ordering ord(*config.orderingArbitrary());
 	Vector b = Vector_(2, -3., -3.);
 	JacobianFactor expected(ord["x1"], -1*eye(2), ord["l1"], eye(2), b, constraint);
-	CHECK(assert_equal(expected, *actual));
+	CHECK(assert_equal((const GaussianFactor&)expected, *actual));
 }
 
 /* ************************************************************************* */

tests/testNonlinearFactorGraph.cpp

@@ -96,8 +96,8 @@ TEST( Graph, linearize )
 {
 	Graph fg = createNonlinearFactorGraph();
 	Values initial = createNoisyValues();
-	boost::shared_ptr<FactorGraph<JacobianFactor> > linearized = fg.linearize(initial, *initial.orderingArbitrary());
+	boost::shared_ptr<FactorGraph<GaussianFactor> > linearized = fg.linearize(initial, *initial.orderingArbitrary());
-	FactorGraph<JacobianFactor> expected = createGaussianFactorGraph(*initial.orderingArbitrary());
+	FactorGraph<GaussianFactor> expected = createGaussianFactorGraph(*initial.orderingArbitrary());
 	CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations
 }