Comments and formatting, and much cleaner solve

gtsam/linear/KalmanFilter.cpp

@@ -21,7 +21,7 @@
  */
 
 #include <gtsam/linear/KalmanFilter.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/HessianFactor.h>
 #include <gtsam/base/Testable.h>
@@ -34,32 +34,36 @@ using namespace std;
 
 namespace gtsam {
 
-  /// Auxiliary function to solve factor graph and return pointer to root conditional
+// Auxiliary function to solve factor graph and return pointer to root conditional
-  KalmanFilter::State solve(const GaussianFactorGraph& factorGraph, const GaussianFactorGraph::Eliminate& function)
+static KalmanFilter::State solve(const GaussianFactorGraph& factorGraph,
-  {
+    const GaussianFactorGraph::Eliminate& function) {
-    // Do a dense solve, e.g. don't use the multifrontal framework
-    // Eliminate the keys in increasing order so that the last key is the one we want to keep.
-    FastSet<Key> keysToEliminate = factorGraph.keys();
-    FastSet<Key>::const_iterator lastKeyPos = keysToEliminate.end();
-    -- lastKeyPos;
-    Key remainingKey = *lastKeyPos;
-    keysToEliminate.erase(lastKeyPos);
-    GaussianFactorGraph::EliminationResult result = function(factorGraph, Ordering(keysToEliminate));
 
-    // As this is a filter, all we need is the posterior P(x_t).  Eliminate it to be
+  // We will eliminate the keys in increasing order, except the last key
-    // upper-triangular.
+  Ordering ordering1(factorGraph.keys());
+  Key lastKey = ordering1.back();
+  ordering1.pop_back();
+
+  // Eliminate the graph using the provided Eliminate function
+  GaussianConditional::shared_ptr conditional;
+  GaussianFactor::shared_ptr factor;
+  boost::tie(conditional, factor) = function(factorGraph, ordering1);
+
+  // As this is a filter, all we need is the posterior P(x_t).
   GaussianFactorGraph graphOfRemainingFactor;
-    graphOfRemainingFactor += result.second;
+  graphOfRemainingFactor += factor;
-    GaussianDensity::shared_ptr state = boost::make_shared<GaussianDensity>(
-      *function(graphOfRemainingFactor, Ordering(cref_list_of<1>(remainingKey))).first);
 
-    return state;
+  // Eliminate it to be upper-triangular.
+  Ordering ordering2;
+  ordering2 += lastKey;
+  boost::tie(conditional, factor) = function(graphOfRemainingFactor, ordering2);
+
+  return boost::make_shared<GaussianDensity>(*conditional);
 }
 
 /* ************************************************************************* */
-  KalmanFilter::State fuse(const KalmanFilter::State& p, GaussianFactor::shared_ptr newFactor,
+static KalmanFilter::State fuse(const KalmanFilter::State& p,
-    const GaussianFactorGraph::Eliminate& function)
+    GaussianFactor::shared_ptr newFactor,
-  {
+    const GaussianFactorGraph::Eliminate& function) {
   // Create a factor graph
   GaussianFactorGraph factorGraph;
   factorGraph += p, newFactor;
@@ -70,13 +74,14 @@ namespace gtsam {
 
 /* ************************************************************************* */
 GaussianFactorGraph::Eliminate KalmanFilter::factorization() const {
-    return method_ == QR ?
+  return
-      GaussianFactorGraph::Eliminate(EliminateQR) :
+      method_ == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
           GaussianFactorGraph::Eliminate(EliminateCholesky);
 }
 
 /* ************************************************************************* */
-  KalmanFilter::State KalmanFilter::init(const Vector& x0, const SharedDiagonal& P0) {
+KalmanFilter::State KalmanFilter::init(const Vector& x0,
+    const SharedDiagonal& P0) {
 
   // Create a factor graph f(x0), eliminate it into P(x0)
   GaussianFactorGraph factorGraph;
@@ -105,7 +110,9 @@ namespace gtsam {
   // 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
   Key k = step(p);
-    return fuse(p, boost::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model), factorization());
+  return fuse(p,
+      boost::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model),
+      factorization());
 }
 
 /* ************************************************************************* */
@@ -130,7 +137,9 @@ namespace gtsam {
   Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
   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), factorization());
+  return fuse(p,
+      boost::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f),
+      factorization());
 }
 
 /* ************************************************************************* */
@@ -139,7 +148,8 @@ namespace gtsam {
   // 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), factorization());
+  return fuse(p, boost::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model),
+      factorization());
 }
 
 /* ************************************************************************* */
@@ -149,12 +159,13 @@ namespace gtsam {
   // 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), factorization());
+  return fuse(p, boost::make_shared<JacobianFactor>(k, H, z, model),
+      factorization());
 }
 
 /* ************************************************************************* */
-  KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, const Vector& z,
+KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
-      const Matrix& Q) {
+    const Vector& z, const Matrix& Q) {
   Key k = step(p);
   Matrix M = inverse(Q), Ht = trans(H);
   Matrix G = Ht * M * H;

gtsam/linear/KalmanFilter.h

@@ -36,7 +36,7 @@ namespace gtsam {
  * measurement models. It uses the square-root information form, as usual in GTSAM.
  *
  * The filter is functional, in that it does not have state: you call init() to create
-   * an initial state, then predict() and update() that create new states out of old.
+ * an initial state, then predict() and update() that create new states out of an old state.
  */
 class GTSAM_EXPORT KalmanFilter {
 
@@ -65,7 +65,7 @@ namespace gtsam {
 
 public:
 
-    // private constructor
+  // Constructor
   KalmanFilter(size_t n, Factorization method =
       KALMANFILTER_DEFAULT_FACTORIZATION) :
       n_(n), I_(eye(n_, n_)), method_(method) {
@@ -73,7 +73,7 @@ namespace gtsam {
 
   /**
    * Create initial state, i.e., prior density at time k=0
-     * In Kalman Filter notation, this are is x_{0|0} and P_{0|0}
+   * In Kalman Filter notation, these are x_{0|0} and P_{0|0}
    * @param x0 estimate at time 0
    * @param P0 covariance at time 0, given as a diagonal Gaussian 'model'
    */
@@ -93,7 +93,6 @@ namespace gtsam {
   /**
    * Predict the state P(x_{t+1}|Z^t)
    *   In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
-     *   After the call, that is the density that can be queried.
    * Details and parameters:
    *   In a linear Kalman Filter, the motion model is f(x_{t}) = F*x_{t} + B*u_{t} + w
    *   where F is the state transition model/matrix, B is the control input model,
@@ -128,7 +127,7 @@ namespace gtsam {
    * will be of the form h(x_{t}) = H*x_{t} + v
    * where H is the observation model/matrix, and v is zero-mean,
    * Gaussian white noise with covariance R.
-     * Currently, R is restricted to diagonal Gaussians (model parameter)
+   * In this version, R is restricted to diagonal Gaussians (model parameter)
    */
   State update(const State& p, const Matrix& H, const Vector& z,
       const SharedDiagonal& model);