diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index e00c93cae..2f7ac8aaf 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -346,8 +346,11 @@ boost::shared_ptr > ISAM2::recalculate( lastAffectedFactorCount = linearFactors_.size(); // Reeliminated keys for detailed results - if(params_.enableDetailedResults) - BOOST_FOREACH(Key key, theta_.keys()) { result.detail->variableStatus[key].isReeliminated = true; } + if(params_.enableDetailedResults) { + BOOST_FOREACH(Key key, theta_.keys()) { + result.detail->variableStatus[key].isReeliminated = true; + } + } toc(3,"batch"); @@ -367,8 +370,11 @@ boost::shared_ptr > 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) - BOOST_FOREACH(Index index, affectedAndNewKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true; } + if(params_.enableDetailedResults) { + 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 > ISAM2::recalculate( } // Root clique variables for detailed results - if(params_.enableDetailedResults) - BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) { result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true; } + if(params_.enableDetailedResults) { + 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) - BOOST_FOREACH(Index index, markedKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true; } + if(params_.enableDetailedResults) { + 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. diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index c03bb9eef..96de41f8f 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/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). diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 9b1bd546f..40e1ca4e1 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -28,7 +28,10 @@ using boost::shared_ptr; const double tol = 1e-4; -ISAM2 createSlamlikeISAM2() { +ISAM2 createSlamlikeISAM2( + boost::optional init_values = boost::none, + boost::optional 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 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 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 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(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)); +} + /* ************************************************************************* */ TEST(ISAM2, constrained_ordering) {