tested and documented ability to swap factors with ISAM2

gtsam/nonlinear/ISAM2.cpp

@@ -346,8 +346,11 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
     lastAffectedFactorCount = linearFactors_.size();
 
     // Reeliminated keys for detailed results
-    if(params_.enableDetailedResults)
+    if(params_.enableDetailedResults) {
-      BOOST_FOREACH(Key key, theta_.keys()) { result.detail->variableStatus[key].isReeliminated = true; }
+      BOOST_FOREACH(Key key, theta_.keys()) {
+      	result.detail->variableStatus[key].isReeliminated = true;
+      }
+    }
 
     toc(3,"batch");
 
@@ -367,8 +370,11 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
     if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; }
 
     // Reeliminated keys for detailed results
-    if(params_.enableDetailedResults)
+    if(params_.enableDetailedResults) {
-      BOOST_FOREACH(Index index, affectedAndNewKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true; }
+      BOOST_FOREACH(Index index, affectedAndNewKeys) {
+      	result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true;
+      }
+    }
 
     result.variablesReeliminated = affectedAndNewKeys.size();
     lastAffectedMarkedCount = markedKeys.size();
@@ -477,8 +483,11 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
   }
 
   // Root clique variables for detailed results
-  if(params_.enableDetailedResults)
+  if(params_.enableDetailedResults) {
-    BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) { result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true; }
+    BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) {
+    	result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true;
+    }
+  }
 
   return affectedKeysSet;
 }
@@ -561,8 +570,11 @@ ISAM2Result ISAM2::update(
     markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys
   }
   // Observed keys for detailed results
-  if(params_.enableDetailedResults)
+  if(params_.enableDetailedResults) {
-    BOOST_FOREACH(Index index, markedKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true; }
+    BOOST_FOREACH(Index index, markedKeys) {
+    	result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true;
+    }
+  }
   // NOTE: we use assign instead of the iterator constructor here because this
   // is a vector of size_t, so the constructor unintentionally resolves to
   // vector(size_t count, Index value) instead of the iterator constructor.

gtsam/nonlinear/ISAM2.h

@@ -429,6 +429,9 @@ public:
   /**
    * Add new factors, updating the solution and relinearizing as needed.
    *
+   * Optionally, this function remove existing factors from the system to enable
+   * behaviors such as swapping existing factors with new ones.
+   *
    * Add new measurements, and optionally new variables, to the current system.
    * This runs a full step of the ISAM2 algorithm, relinearizing and updating
    * the solution as needed, according to the wildfire and relinearize
@@ -439,6 +442,7 @@ public:
    * You must include here all new variables occuring in newFactors (which were not already
    * in the system).  There must not be any variables here that do not occur in newFactors,
    * and additionally, variables that were already in the system must not be included here.
+   * @param removeFactorIndices Indices of factors to remove from system
    * @param force_relinearize Relinearize any variables whose delta magnitude is sufficiently
    * large (Params::relinearizeThreshold), regardless of the relinearization interval
    * (Params::relinearizeSkip).

tests/testGaussianISAM2.cpp

@@ -28,7 +28,10 @@ using boost::shared_ptr;
 
 const double tol = 1e-4;
 
-ISAM2 createSlamlikeISAM2() {
+ISAM2 createSlamlikeISAM2(
+		boost::optional<Values&> init_values = boost::none,
+		boost::optional<planarSLAM::Graph&> full_graph = boost::none,
+		const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)) {
 
   // Pose and landmark key types from planarSLAM
   using planarSLAM::PoseKey;
@@ -39,7 +42,8 @@ ISAM2 createSlamlikeISAM2() {
   SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
 
   // These variables will be reused and accumulate factors and values
-  ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
+  ISAM2 isam(params);
+//  ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
   Values fullinit;
   planarSLAM::Graph fullgraph;
 
@@ -121,6 +125,12 @@ ISAM2 createSlamlikeISAM2() {
     ++ i;
   }
 
+  if (full_graph)
+  	*full_graph = fullgraph;
+
+  if (init_values)
+  	*init_values = fullinit;
+
   return isam;
 }
 
@@ -1071,6 +1081,7 @@ TEST(ISAM2, removeFactors)
 
     // Remove the measurement on landmark 0
     FastVector<size_t> toRemove;
+    EXPECT_LONGS_EQUAL(isam.getFactorsUnsafe().size()-2, result.newFactorsIndices[1]);
     toRemove.push_back(result.newFactorsIndices[1]);
     isam.update(planarSLAM::Graph(), Values(), toRemove);
   }
@@ -1107,6 +1118,76 @@ TEST(ISAM2, removeFactors)
   EXPECT(assert_equal(expectedGradient, actualGradient));
 }
 
+/* ************************************************************************* */
+TEST(ISAM2, swapFactors)
+{
+
+//  SETDEBUG("ISAM2 update", true);
+//  SETDEBUG("ISAM2 update verbose", true);
+//  SETDEBUG("ISAM2 recalculate", true);
+
+  // This test builds a graph in the same way as the "slamlike" test above, but
+  // then swaps the 2nd-to-last landmark measurement with a different one
+
+  // Pose and landmark key types from planarSLAM
+  using planarSLAM::PoseKey;
+  using planarSLAM::PointKey;
+
+  // Set up parameters
+  SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
+  SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
+
+  Values fullinit;
+  planarSLAM::Graph fullgraph;
+  ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph);
+
+  // Remove the measurement on landmark 0 and replace with a different one
+  {
+  	size_t swap_idx = isam.getFactorsUnsafe().size()-2;
+  	FastVector<size_t> toRemove;
+  	toRemove.push_back(swap_idx);
+  	fullgraph.remove(swap_idx);
+
+  	planarSLAM::Graph swapfactors;
+//  	swapfactors.addBearingRange(10, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); // original factor
+  	swapfactors.addBearingRange(10, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise);
+  	fullgraph.push_back(swapfactors);
+  	isam.update(swapfactors, Values(), toRemove);
+  }
+
+  // Compare solutions
+  EXPECT(assert_equal(fullgraph, planarSLAM::Graph(isam.getFactorsUnsafe())));
+  EXPECT(isam_check(fullgraph, fullinit, isam));
+
+  // Check gradient at each node
+  typedef ISAM2::sharedClique sharedClique;
+  BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
+    // Compute expected gradient
+    FactorGraph<JacobianFactor> jfg;
+    jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
+    VectorValues expectedGradient(*allocateVectorValues(isam));
+    gradientAtZero(jfg, expectedGradient);
+    // Compare with actual gradients
+    int variablePosition = 0;
+    for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
+      const int dim = clique->conditional()->dim(jit);
+      Vector actual = clique->gradientContribution().segment(variablePosition, dim);
+      EXPECT(assert_equal(expectedGradient[*jit], actual));
+      variablePosition += dim;
+    }
+    EXPECT_LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
+  }
+
+  // Check gradient
+  VectorValues expectedGradient(*allocateVectorValues(isam));
+  gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
+  VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
+  VectorValues actualGradient(*allocateVectorValues(isam));
+  gradientAtZero(isam, actualGradient);
+  EXPECT(assert_equal(expectedGradient2, expectedGradient));
+  EXPECT(assert_equal(expectedGradient, actualGradient));
+}
+
 /* ************************************************************************* */
 TEST(ISAM2, constrained_ordering)
 {