Ability to remove factors from iSAM2

gtsam/nonlinear/ISAM2-inl.h

@@ -424,7 +424,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
 /* ************************************************************************* */
 template<class CONDITIONAL, class VALUES, class GRAPH>
 ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
-    const GRAPH& newFactors, const Values& newTheta, bool force_relinearize) {
+    const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices, bool force_relinearize) {
 
   static const bool debug = ISDEBUG("ISAM2 update");
   static const bool verbose = ISDEBUG("ISAM2 update verbose");
@@ -446,9 +446,24 @@ ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
   }
 
   tic(0,"push_back factors");
+  // Add the new factor indices to the result struct
+  result.newFactorsIndices.resize(newFactors.size());
+  for(size_t i=0; i<newFactors.size(); ++i)
+    result.newFactorsIndices[i] = i + nonlinearFactors_.size();
+
   // 1. Add any new factors \Factors:=\Factors\cup\Factors'.
   if(debug || verbose) newFactors.print("The new factors are: ");
   nonlinearFactors_.push_back(newFactors);
+
+  // Remove the removed factors
+  GRAPH removeFactors; removeFactors.reserve(removeFactorIndices.size());
+  BOOST_FOREACH(size_t index, removeFactorIndices) {
+    removeFactors.push_back(nonlinearFactors_[index]);
+    nonlinearFactors_.remove(index);
+  }
+
+  // Remove removed factors from the variable index so we do not attempt to relinearize them
+  variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_));
   toc(0,"push_back factors");
 
   tic(1,"add new variables");
@@ -464,6 +479,11 @@ ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
   tic(3,"gather involved keys");
   // 3. Mark linear update
   FastSet<Index> markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
+  // Also mark keys involved in removed factors
+  {
+    FastSet<Index> markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors
+    markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys
+  }
   // 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

@@ -148,6 +148,12 @@ struct ISAM2Result {
 
   /** The number of cliques in the Bayes' Tree */
   size_t cliques;
+
+  /** The indices of the newly-added factors, in 1-to-1 correspondence with the
+   * factors passed as \c newFactors to ISAM2::update().  These indices may be
+   * used later to refer to the factors in order to remove them.
+   */
+  FastVector<size_t> newFactorsIndices;
 };
 
 template<class CONDITIONAL>
@@ -340,7 +346,7 @@ public:
    * (Params::relinearizeSkip).
    * @return An ISAM2Result struct containing information about the update
    */
-  ISAM2Result update(const GRAPH& newFactors = GRAPH(), const VALUES& newTheta = VALUES(),
+  ISAM2Result update(const GRAPH& newFactors = GRAPH(), const VALUES& newTheta = VALUES(), const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
       bool force_relinearize = false);
 
   /** Access the current linearization point */

tests/testGaussianISAM2.cpp

@@ -624,6 +624,148 @@ TEST(ISAM2, permute_cached) {
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST(ISAM2, removeFactors)
+{
+
+//  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 removes the 2nd-to-last landmark measurement
+
+  // Pose and landmark key types from planarSLAM
+  typedef planarSLAM::PoseKey PoseKey;
+  typedef planarSLAM::PointKey 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));
+
+  // These variables will be reused and accumulate factors and values
+  GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
+  planarSLAM::Values fullinit;
+  planarSLAM::Graph fullgraph;
+
+  // i keeps track of the time step
+  size_t i = 0;
+
+  // Add a prior at time 0 and update isam
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
+    fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  CHECK(isam_check(fullgraph, fullinit, isam));
+
+  // Add odometry from time 0 to time 5
+  for( ; i<5; ++i) {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  // Add odometry from time 5 to 6 and landmark measurement at time 5
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
+    newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
+    fullgraph.push_back(newfactors);
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
+    init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
+    init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
+    fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
+    fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
+    fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
+
+    isam.update(newfactors, init);
+    ++ i;
+  }
+
+  // Add odometry from time 6 to time 10
+  for( ; i<10; ++i) {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    fullgraph.push_back(newfactors);
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
+
+    isam.update(newfactors, init);
+  }
+
+  // Add odometry from time 10 to 11 and landmark measurement at time 10
+  {
+    planarSLAM::Graph newfactors;
+    newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
+    newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
+    newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
+    fullgraph.push_back(newfactors[0]);
+    fullgraph.push_back(newfactors[2]); // Don't add measurement on landmark 0
+
+    planarSLAM::Values init;
+    init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
+    fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
+
+    ISAM2Result result = isam.update(newfactors, init);
+    ++ i;
+
+    // Remove the measurement on landmark 0
+    FastVector<size_t> toRemove;
+    toRemove.push_back(result.newFactorsIndices[1]);
+    isam.update(planarSLAM::Graph(), planarSLAM::Values(), toRemove);
+  }
+
+  // Compare solutions
+  CHECK(isam_check(fullgraph, fullinit, isam));
+
+  // Check gradient at each node
+  typedef GaussianISAM2<planarSLAM::Values>::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;
+    }
+    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));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */