diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 85bd7aeb8..343ec7a56 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -424,7 +424,7 @@ boost::shared_ptr > ISAM2::recalculat /* ************************************************************************* */ template ISAM2Result ISAM2::update( - const GRAPH& newFactors, const Values& newTheta, bool force_relinearize) { + const GRAPH& newFactors, const Values& newTheta, const FastVector& 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::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::update( tic(3,"gather involved keys"); // 3. Mark linear update FastSet markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors + // Also mark keys involved in removed factors + { + FastSet 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. diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 2007ba79e..544031a93 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/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 newFactorsIndices; }; template @@ -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& removeFactorIndices = FastVector(), bool force_relinearize = false); /** Access the current linearization point */ diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 179deed62..5c59f8c28 100644 --- a/tests/testGaussianISAM2.cpp +++ b/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 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 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::sharedClique sharedClique; + BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { + // Compute expected gradient + FactorGraph 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(isam), expectedGradient); + VectorValues expectedGradient2(gradient(FactorGraph(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);} /* ************************************************************************* */