starting to add remove functionality in fixedLagSmoothers, test does not pass..

gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp

@@ -52,7 +52,7 @@ Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const {
 /* ************************************************************************* */
 FixedLagSmoother::Result BatchFixedLagSmoother::update(
     const NonlinearFactorGraph& newFactors, const Values& newTheta,
-    const KeyTimestampMap& timestamps) {
+    const KeyTimestampMap& timestamps, const FastVector<size_t>& factorToRemove) {
 
   // Update all of the internal variables with the new information
   gttic(augment_system);

gtsam_unstable/nonlinear/BatchFixedLagSmoother.h

@@ -48,7 +48,7 @@ public:
 
   /** Add new factors, updating the solution and relinearizing as needed. */
   Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
-      const KeyTimestampMap& timestamps = KeyTimestampMap());
+      const KeyTimestampMap& timestamps = KeyTimestampMap(), const FastVector<size_t>& factorToRemove = FastVector<size_t>());
 
   /** Compute an estimate from the incomplete linear delta computed during the last update.
    * This delta is incomplete because it was not updated below wildfire_threshold.  If only

gtsam_unstable/nonlinear/FixedLagSmoother.h

@@ -91,7 +91,7 @@ public:
 
   /** Add new factors, updating the solution and relinearizing as needed. */
   virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
-      const KeyTimestampMap& timestamps = KeyTimestampMap()) = 0;
+      const KeyTimestampMap& timestamps = KeyTimestampMap(), const FastVector<size_t>& factorToRemove = FastVector<size_t>()) = 0;
 
   /** Compute an estimate from the incomplete linear delta computed during the last update.
    * This delta is incomplete because it was not updated below wildfire_threshold.  If only

gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp

@@ -65,7 +65,7 @@ bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs,
 /* ************************************************************************* */
 FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
     const NonlinearFactorGraph& newFactors, const Values& newTheta,
-    const KeyTimestampMap& timestamps) {
+    const KeyTimestampMap& timestamps, const FastVector<size_t>& factorToRemove) {
 
   const bool debug = ISDEBUG("IncrementalFixedLagSmoother update");
 
@@ -126,7 +126,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
 
   // Update iSAM2
   ISAM2Result isamResult = isam_.update(newFactors, newTheta,
-      FactorIndices(), constrainedKeys, boost::none, additionalMarkedKeys);
+		  factorToRemove, constrainedKeys, boost::none, additionalMarkedKeys);
 
   if (debug) {
     PrintSymbolicTree(isam_,

gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h

@@ -62,7 +62,8 @@ public:
    */
   Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
       const Values& newTheta = Values(), //
-      const KeyTimestampMap& timestamps = KeyTimestampMap());
+      const KeyTimestampMap& timestamps = KeyTimestampMap(),
+      const FastVector<size_t>& factorToRemove = FactorIndices());
 
   /** Compute an estimate from the incomplete linear delta computed during the last update.
    * This delta is incomplete because it was not updated below wildfire_threshold.  If only

gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp

@@ -71,8 +71,6 @@ TEST( IncrementalFixedLagSmoother, Example )
   Values fullinit;
   NonlinearFactorGraph fullgraph;
 
-
-
   // i keeps track of the time step
   size_t i = 0;
 
@@ -177,6 +175,64 @@ TEST( IncrementalFixedLagSmoother, Example )
     ++i;
   }
 
+  // add/remove an extra factor
+  {
+	  Key key1 = MakeKey(i-1);
+	  Key key2 = MakeKey(i);
+
+	  NonlinearFactorGraph newFactors;
+	  Values newValues;
+	  Timestamps newTimestamps;
+
+	  // add 2 odometry factors
+	  newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
+	  newFactors.push_back(BetweenFactor<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
+	  newValues.insert(key2, Point2(double(i)+0.1, -0.1));
+	  newTimestamps[key2] = double(i);
+
+	  fullgraph.push_back(newFactors);
+	  fullinit.insert(newValues);
+
+	  // Update the smoother
+	  smoother.update(newFactors, newValues, newTimestamps);
+
+	  // Check
+	  CHECK(check_smoother(fullgraph, fullinit, smoother, key2));
+
+	  // now remove one of the two and try again
+	  size_t factorIndex = fullgraph.size()-2; // any index that does not break connectivity of the graph
+	  FastVector<size_t> factorToRemove;
+	  factorToRemove.push_back(factorIndex);
+
+	  NonlinearFactorGraph emptyNewFactors;
+	  Values emptyNewValues;
+	  Timestamps emptyNewTimestamps;
+
+	  //smoother.getFactors().print();
+
+	  const NonlinearFactorGraph& allFactors = smoother.getFactors();
+	  size_t nrFactorsBeforeRemoval = allFactors.size();
+	  NonlinearFactorGraph expected;
+	  for(size_t i=0; i< allFactors.size(); i++){
+		 // if(i != factorIndex)
+		//	  expected.push_back( allFactors[i] );
+		 // else
+		  std::cout << "ind: " << i << std::endl;
+			  allFactors[i]->print();
+	  }
+
+	  // remove factor
+	  smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps, factorToRemove);
+	  size_t nrFactorsAfterRemoval = smoother.getFactors().size();
+
+	  // check that the number of factors is right
+	  DOUBLES_EQUAL(nrFactorsBeforeRemoval-1, nrFactorsAfterRemoval, 1e-5);
+
+	  // check that the factors in the smoother are right
+	  // NonlinearFactorGraph actual = smoother.getFactors();
+	  //CHECK(assert_equal(expected,actual));
+	  //smoother.getFactors().print();
+  }
 }
 
 /* ************************************************************************* */