diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index a0ede5f74..24595552a 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/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& factorToRemove) { // Update all of the internal variables with the new information gttic(augment_system); diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 27fe94451..1d882d7aa 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/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& factorToRemove = FastVector()); /** 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 diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index 4868a7cf1..2d080ad4d 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/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& factorToRemove = FastVector()) = 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 diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index d3c34ff5e..98a3110aa 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/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& 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_, diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index 77d052a21..00ad5d13b 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/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& 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 diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 0b86a60e9..4dd3b62a0 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/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(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.push_back(BetweenFactor(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 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(); + } } /* ************************************************************************* */