leaner and meaner

gtsam/linear/KalmanFilter.cpp

@@ -48,6 +48,20 @@ namespace gtsam {
 			n_(n), I_(eye(n_, n_)), density_(density) {
 	}
 
+	/* ************************************************************************* */
+	KalmanFilter KalmanFilter::add(GaussianFactor* newFactor) {
+
+		// Create a factor graph
+		GaussianFactorGraph factorGraph;
+
+		// push back previous solution and new factor
+		factorGraph.push_back(density_->toFactor());
+		factorGraph.push_back(GaussianFactor::shared_ptr(newFactor));
+
+		// Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
+		return KalmanFilter(n_, solve(factorGraph));
+	}
+
 	/* ************************************************************************* */
 	KalmanFilter::KalmanFilter(const Vector& x0, const SharedDiagonal& P0) :
 			n_(x0.size()), I_(eye(n_, n_)) {
@@ -91,27 +105,17 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	KalmanFilter KalmanFilter::predict(const Matrix& F, const Matrix& B, const Vector& u,
+	KalmanFilter KalmanFilter::predict(const Matrix& F, const Matrix& B,
-			const SharedDiagonal& model) {
+			const Vector& u, const SharedDiagonal& model) {
-		// We will create a small factor graph f1-(x0)-f2-(x1)
-		// where factor f1 is just the prior from time t0, P(x0)
-		// and   factor f2 is from the motion model
-		GaussianFactorGraph factorGraph;
-
-		// push back f1
-		factorGraph.push_back(density_->toFactor());
 
 		// The factor related to the motion model is defined as
 		// f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
-		factorGraph.add(0, -F, 1, I_, B * u, model);
+		return add(new JacobianFactor(0, -F, 1, I_, B * u, model));
-
-		// Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
-		return KalmanFilter(n_,solve(factorGraph));
 	}
 
 	/* ************************************************************************* */
-	KalmanFilter KalmanFilter::predictQ(const Matrix& F, const Matrix& B, const Vector& u,
+	KalmanFilter KalmanFilter::predictQ(const Matrix& F, const Matrix& B,
-			const Matrix& Q) {
+			const Vector& u, const Matrix& Q) {
 
 #ifndef NDEBUG
 		int n = F.cols();
@@ -122,10 +126,6 @@ namespace gtsam {
 		assert(Q.cols() == n);
 #endif
 
-		// Same scheme as in predict:
-		GaussianFactorGraph factorGraph;
-		factorGraph.push_back(density_->toFactor());
-
 		// The factor related to the motion model is defined as
 		// f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
 		// See documentation in HessianFactor, we have A1 = -F,  A2 = I_, b = B*u:
@@ -134,45 +134,24 @@ namespace gtsam {
 		Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M;
 		Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
 		double f = dot(b, g2);
-		HessianFactor::shared_ptr factor(new HessianFactor(0, 1, G11, G12, g1, G22, g2, f));
+		return add(new HessianFactor(0, 1, G11, G12, g1, G22, g2, f));
-		factorGraph.push_back(factor);
-
-		// Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
-		return KalmanFilter(n_,solve(factorGraph));
 	}
 
 	/* ************************************************************************* */
-	KalmanFilter KalmanFilter::predict2(const Matrix& A0, const Matrix& A1, const Vector& b,
+	KalmanFilter KalmanFilter::predict2(const Matrix& A0, const Matrix& A1,
-			const SharedDiagonal& model) {
+			const Vector& b, const SharedDiagonal& model) {
-
+		// Nhe factor related to the motion model is defined as
-		// Same scheme as in predict:
-		GaussianFactorGraph factorGraph;
-		factorGraph.push_back(density_->toFactor());
-
-		// However, now the factor related to the motion model is defined as
 		// f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
-		factorGraph.add(0, A0, 1, A1, b, model);
+		return add(new JacobianFactor(0, A0, 1, A1, b, model));
-		return KalmanFilter(n_,solve(factorGraph));
 	}
 
 	/* ************************************************************************* */
 	KalmanFilter KalmanFilter::update(const Matrix& H, const Vector& z,
 			const SharedDiagonal& model) {
-		// We will create a small factor graph f1-(x0)-f2
-		// where factor f1 is the predictive density
-		// and   factor f2 is from the measurement model
-		GaussianFactorGraph factorGraph;
-
-		// push back f1
-		factorGraph.push_back(density_->toFactor());
-
 		// The factor related to the measurements would be defined as
 		// f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T
 		//    = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
-		factorGraph.add(0, H, z, model);
+		return add(new JacobianFactor(0, H, z, model));
-
-		// Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
-		return KalmanFilter(n_,solve(factorGraph));
 	}
 
 /* ************************************************************************* */

gtsam/linear/KalmanFilter.h

@@ -20,6 +20,7 @@
  * @author Frank Dellaert
  */
 
+#include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/linear/GaussianConditional.h>
 
 namespace gtsam {
@@ -42,6 +43,9 @@ namespace gtsam {
 		/// private constructor
 		KalmanFilter(size_t n, GaussianConditional* density);
 
+		/// add a new factor and marginalize to new Kalman filter
+		KalmanFilter add(GaussianFactor* newFactor);
+
 	public:
 
 		/**