Fixed indexing problem in KalmanFilter - linear variable index was incremented, resulting in allocating larger and larger data structures with each step. Now shifting indices back to 0 each step.

gtsam/linear/KalmanFilter.cpp

@@ -59,7 +59,12 @@ namespace gtsam {
     factorGraph.push_back(GaussianFactor::shared_ptr(newFactor));
 
     // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
-    return solve(factorGraph, useQR);
+    GaussianDensity::shared_ptr result = solve(factorGraph, useQR);
+
+    // Change key in result to 0 to start back at variable index 0 and return
+    assert(result->nrFrontals() == 1);
+    result->frontals().front() = 0;
+    return result;
   }
 
   /* ************************************************************************* */
@@ -94,8 +99,7 @@ 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
-    Index k = step(p);
-    return fuse(p, new JacobianFactor(k, -F, k + 1, I_, B * u, model), useQR());
+    return fuse(p, new JacobianFactor(0, -F, 1, I_, B * u, model), useQR());
   }
 
   /* ************************************************************************* */
@@ -119,8 +123,7 @@ 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);
-    Index k = step(p);
-    return fuse(p, new HessianFactor(k, k + 1, G11, G12, g1, G22, g2, f),
+    return fuse(p, new HessianFactor(0, 1, G11, G12, g1, G22, g2, f),
         useQR());
   }
 
@@ -129,8 +132,7 @@ namespace gtsam {
       const Matrix& A1, const Vector& b, const SharedDiagonal& model) {
     // 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
-    Index k = step(p);
-    return fuse(p, new JacobianFactor(k, A0, k + 1, A1, b, model), useQR());
+    return fuse(p, new JacobianFactor(0, A0, 1, A1, b, model), useQR());
   }
 
   /* ************************************************************************* */
@@ -139,19 +141,17 @@ namespace gtsam {
     // 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
-    Index k = step(p);
-    return fuse(p, new JacobianFactor(k, H, z, model), useQR());
+    return fuse(p, new JacobianFactor(0, H, z, model), useQR());
   }
 
   /* ************************************************************************* */
   KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, const Vector& z,
       const Matrix& Q) {
-    Index k = step(p);
     Matrix M = inverse(Q), Ht = trans(H);
     Matrix G = Ht * M * H;
     Vector g = Ht * M * z;
     double f = dot(z, M * z);
-    return fuse(p, new HessianFactor(k, G, g, f), useQR());
+    return fuse(p, new HessianFactor(0, G, g, f), useQR());
   }
 
 /* ************************************************************************* */

gtsam/linear/KalmanFilter.h

@@ -86,11 +86,6 @@ namespace gtsam {
     /// print
     void print(const std::string& s = "") const;
 
-    /** Return step index k, starts at 0, incremented at each predict. */
-    static Index step(const State& p) {
-      return p->firstFrontalKey();
-    }
-
     /**
      * Predict the state P(x_{t+1}|Z^t)
      *   In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}

gtsam/linear/tests/testKalmanFilter.cpp

@@ -124,11 +124,9 @@ TEST( KalmanFilter, linear1 ) {
   // Run iteration 3
   KalmanFilter::State p3p = kf.predict(p2, F, B, u, modelQ);
   EXPECT(assert_equal(expected3, p3p->mean()));
-  LONGS_EQUAL(3, KalmanFilter::step(p3p));
 
   KalmanFilter::State p3 = kf.update(p3p, H, z3, modelR);
   EXPECT(assert_equal(expected3, p3->mean()));
-  LONGS_EQUAL(3, KalmanFilter::step(p3));
 }
 
 /* ************************************************************************* */