removed checking const variable each time and created appropriate Eliminate function in constructor instead.

gtsam/linear/KalmanFilter.cpp

@@ -34,14 +34,15 @@ using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
 // Auxiliary function to solve factor graph and return pointer to root conditional
-static KalmanFilter::State solve(const GaussianFactorGraph& factorGraph,
+KalmanFilter::State //
-    const GaussianFactorGraph::Eliminate& function) {
+KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {
 
   // Eliminate the graph using the provided Eliminate function
   Ordering ordering(factorGraph.keys());
   GaussianBayesNet::shared_ptr bayesNet = //
-      factorGraph.eliminateSequential(ordering, function);
+      factorGraph.eliminateSequential(ordering, function_);
 
   // As this is a filter, all we need is the posterior P(x_t).
   // This is the last GaussianConditional in the resulting BayesNet
@@ -50,22 +51,16 @@ static KalmanFilter::State solve(const GaussianFactorGraph& factorGraph,
 }
 
 /* ************************************************************************* */
-static KalmanFilter::State fuse(const KalmanFilter::State& p,
+// Auxiliary function to create a small graph for predict or update and solve
-    GaussianFactor::shared_ptr newFactor,
+KalmanFilter::State //
-    const GaussianFactorGraph::Eliminate& function) {
+KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) {
+
   // Create a factor graph
   GaussianFactorGraph factorGraph;
   factorGraph += p, newFactor;
 
   // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
-  return solve(factorGraph, function);
+  return solve(factorGraph);
-}
-
-/* ************************************************************************* */
-GaussianFactorGraph::Eliminate KalmanFilter::factorization() const {
-  return
-      method_ == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
-          GaussianFactorGraph::Eliminate(EliminateCholesky);
 }
 
 /* ************************************************************************* */
@@ -75,7 +70,7 @@ KalmanFilter::State KalmanFilter::init(const Vector& x0,
   // Create a factor graph f(x0), eliminate it into P(x0)
   GaussianFactorGraph factorGraph;
   factorGraph += JacobianFactor(0, I_, x0, P0); // |x-x0|^2_diagSigma
-  return solve(factorGraph, factorization());
+  return solve(factorGraph);
 }
 
 /* ************************************************************************* */
@@ -84,7 +79,7 @@ KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) {
   // Create a factor graph f(x0), eliminate it into P(x0)
   GaussianFactorGraph factorGraph;
   factorGraph += HessianFactor(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0)
-  return solve(factorGraph, factorization());
+  return solve(factorGraph);
 }
 
 /* ************************************************************************* */
@@ -100,8 +95,7 @@ KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
   // 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
   Key k = step(p);
   return fuse(p,
-      boost::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model),
+      boost::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model));
-      factorization());
 }
 
 /* ************************************************************************* */
@@ -127,8 +121,7 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
   double f = dot(b, g2);
   Key k = step(p);
   return fuse(p,
-      boost::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f),
+      boost::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f));
-      factorization());
 }
 
 /* ************************************************************************* */
@@ -137,8 +130,7 @@ KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
   // Nhe factor related to the motion model is defined as
   // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
   Key k = step(p);
-  return fuse(p, boost::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model),
+  return fuse(p, boost::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model));
-      factorization());
 }
 
 /* ************************************************************************* */
@@ -148,8 +140,7 @@ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
   // 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
   Key k = step(p);
-  return fuse(p, boost::make_shared<JacobianFactor>(k, H, z, model),
+  return fuse(p, boost::make_shared<JacobianFactor>(k, H, z, model));
-      factorization());
 }
 
 /* ************************************************************************* */
@@ -160,7 +151,7 @@ KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
   Matrix G = Ht * M * H;
   Vector g = Ht * M * z;
   double f = dot(z, M * z);
-  return fuse(p, boost::make_shared<HessianFactor>(k, G, g, f), factorization());
+  return fuse(p, boost::make_shared<HessianFactor>(k, G, g, f));
 }
 
 /* ************************************************************************* */

gtsam/linear/KalmanFilter.h

@@ -59,16 +59,19 @@ private:
 
   const size_t n_; /** dimensionality of state */
   const Matrix I_; /** identity matrix of size n*n */
-  const Factorization method_; /** algorithm */
+  const GaussianFactorGraph::Eliminate function_; /** algorithm */
 
-  GaussianFactorGraph::Eliminate factorization() const;
+  State solve(const GaussianFactorGraph& factorGraph) const;
+  State fuse(const State& p, GaussianFactor::shared_ptr newFactor);
 
 public:
 
   // Constructor
   KalmanFilter(size_t n, Factorization method =
       KALMANFILTER_DEFAULT_FACTORIZATION) :
-      n_(n), I_(eye(n_, n_)), method_(method) {
+      n_(n), I_(eye(n_, n_)), function_(
+          method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
+              GaussianFactorGraph::Eliminate(EliminateCholesky)) {
   }
 
   /**