diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 20aacb1e4..5e71ee22b 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -31,7 +31,7 @@ namespace gtsam { void ISAM2::Impl::AddVariables( const Values& newTheta, Values& theta, VectorValues& delta, VectorValues& deltaNewton, VectorValues& deltaGradSearch, vector& replacedKeys, - Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) { + Ordering& ordering, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); @@ -58,15 +58,15 @@ void ISAM2::Impl::AddVariables( assert(ordering.nVars() == delta.size()); assert(ordering.size() == delta.size()); } - assert(ordering.nVars() >= nodes.size()); replacedKeys.resize(ordering.nVars(), false); - nodes.resize(ordering.nVars()); } /* ************************************************************************* */ -void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, Values& theta, VariableIndex& variableIndex, - VectorValues& delta, VectorValues& deltaNewton, VectorValues& deltaGradSearch, - std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes) { +void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Clique::shared_ptr& root, + Values& theta, VariableIndex& variableIndex, + VectorValues& delta, VectorValues& deltaNewton, VectorValues& deltaGradSearch, + std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, + GaussianFactorGraph& linearFactors) { // Get indices of unused keys vector unusedIndices; unusedIndices.reserve(unusedKeys.size()); @@ -96,16 +96,18 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, Values& theta, VectorValues newDeltaNewton(dims); VectorValues newDeltaGradSearch(dims); std::vector newReplacedKeys(replacedKeys.size() - unusedIndices.size()); - Base::Nodes newNodes(nodes.size() - unusedIndices.size()); + Base::Nodes newNodes(nodes.size()); // We still keep unused keys at the end until later in ISAM2::recalculate - for(size_t j = 0; j < newNodes.size(); ++j) { + for(size_t j = 0; j < dims.size(); ++j) { newDelta[j] = delta[unusedToEnd[j]]; newDeltaNewton[j] = deltaNewton[unusedToEnd[j]]; newDeltaGradSearch[j] = deltaGradSearch[unusedToEnd[j]]; newReplacedKeys[j] = replacedKeys[unusedToEnd[j]]; - newNodes[j] = nodes[unusedToEnd[j]]; } + // Permute the nodes index so the unused variables are the end + unusedToEnd.applyToCollection(newNodes, nodes); + // Swap the new data structures with the outputs of this function delta.swap(newDelta); deltaNewton.swap(newDeltaNewton); @@ -120,6 +122,11 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, Values& theta, ordering.pop_back(key); theta.erase(key); } + + // Finally, permute references to variables + if(root) + root->permuteWithInverse(unusedToEndInverse); + linearFactors.permuteWithInverse(unusedToEndInverse); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 69caf7329..b873c87bf 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -48,14 +48,15 @@ struct ISAM2::Impl { */ static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta, VectorValues& deltaNewton, VectorValues& deltaGradSearch, std::vector& replacedKeys, - Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + Ordering& ordering, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** * Remove variables from the ISAM2 system. */ - static void RemoveVariables(const FastSet& unusedKeys, Values& theta, VariableIndex& variableIndex, - VectorValues& delta, VectorValues& deltaNewton, VectorValues& deltaGradSearch, - std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes); + static void RemoveVariables(const FastSet& unusedKeys, const ISAM2Clique::shared_ptr& root, + Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, + VectorValues& deltaGradSearch, std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, + GaussianFactorGraph& linearFactors); /** * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 0c3c198aa..e47127edd 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -249,6 +249,9 @@ boost::shared_ptr > ISAM2::recalculate( this->removeTop(markedKeys, affectedBayesNet, orphans); toc(1, "removetop"); + // Now that the top is removed, correct the size of the Nodes index + this->nodes_.resize(delta_.size()); + if(debug) affectedBayesNet.print("Removed top: "); if(debug) orphans.print("Orphans: "); @@ -266,6 +269,14 @@ boost::shared_ptr > ISAM2::recalculate( // ordering provides all keys in conditionals, there cannot be others because path to root included tic(2,"affectedKeys"); FastList affectedKeys = affectedBayesNet.ordering(); + // The removed top also contained removed variables, which will be ordered + // higher than the number of variables in the system since unused variables + // were already removed in ISAM2::update. + for(FastList::iterator key = affectedKeys.begin(); key != affectedKeys.end(); ) + if(*key >= delta_.size()) + affectedKeys.erase(key++); + else + ++key; toc(2,"affectedKeys"); boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result @@ -539,15 +550,46 @@ ISAM2Result ISAM2::update( BOOST_FOREACH(size_t index, removeFactorIndices) { removeFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); + if(params_.cacheLinearizedFactors) + linearFactors_.remove(index); } // Remove removed factors from the variable index so we do not attempt to relinearize them variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_)); + + // We now need to start keeping track of the marked keys involved in added or + // removed factors. + FastSet markedKeys; + + // Remove unused keys and add keys from removed factors that are still used + // in other factors to markedKeys. + { + // Get keys from removed factors + FastSet removedFactorSymbKeys = removeFactors.keys(); + + // For each key, if still used in other factors, add to markedKeys to be + // recalculated, or if not used, add to unusedKeys to be removed from the + // system. Note that unusedKeys stores Key while markedKeys stores Index. + FastSet unusedKeys; + BOOST_FOREACH(Key key, removedFactorSymbKeys) { + Index index = ordering_[key]; + if(variableIndex_[index].empty()) + unusedKeys.insert(key); + else + markedKeys.insert(index); + } + + // Remove unused keys. We must hold on to the new nodes index for now + // instead of placing it into the tree because removeTop will need to + // update it. + Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, + deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_); + } toc(1,"push_back factors"); tic(2,"add new variables"); // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. - Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_, Base::nodes_); + Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_); // New keys for detailed results if(params_.enableDetailedResults) { inverseOrdering_ = ordering_.invert(); @@ -561,13 +603,12 @@ ISAM2Result ISAM2::update( tic(4,"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 - } - // Observed keys for detailed results + { + FastSet newFactorIndices = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors + markedKeys.insert(newFactorIndices.begin(), newFactorIndices.end()); + } + + // Observed keys for detailed results if(params_.enableDetailedResults) { BOOST_FOREACH(Index index, markedKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 2fd3e0b23..1fa893819 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -516,8 +516,7 @@ private: GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, - const FastVector& observedKeys, - const boost::optional >& constrainKeys, ISAM2Result& result); + const FastVector& observedKeys, const boost::optional >& constrainKeys, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); void updateDelta(bool forceFullSolve = false) const; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 2d2f68b71..f63d2add5 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -160,8 +160,6 @@ TEST_UNSAFE(ISAM2, ImplAddVariables) { Ordering ordering; ordering += 100, 0; - ISAM2::Nodes nodes(2); - // Verify initial state LONGS_EQUAL(0, ordering[100]); LONGS_EQUAL(1, ordering[0]); @@ -193,10 +191,8 @@ TEST_UNSAFE(ISAM2, ImplAddVariables) { Ordering orderingExpected; orderingExpected += 100, 0, 1; - ISAM2::Nodes nodesExpected(3, ISAM2::sharedClique()); - // Expand initial state - ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes); + ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering); EXPECT(assert_equal(thetaExpected, theta)); EXPECT(assert_equal(deltaExpected, delta)); @@ -276,13 +272,15 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) { ISAM2::Nodes nodesExpected(2); - // Expand initial state + // Reduce initial state FastSet unusedKeys; unusedKeys.insert(1); vector removedFactorsI; removedFactorsI.push_back(1); SymbolicFactorGraph removedFactors; removedFactors.push_back(sfg[1]); variableIndex.remove(removedFactorsI, removedFactors); - ISAM2::Impl::RemoveVariables(unusedKeys, theta, variableIndex, delta, deltaNewton, deltaRg, replacedKeys, ordering, nodes); + GaussianFactorGraph linearFactors; + ISAM2::Impl::RemoveVariables(unusedKeys, ISAM2::sharedClique(), theta, variableIndex, delta, deltaNewton, deltaRg, + replacedKeys, ordering, nodes, linearFactors); EXPECT(assert_equal(thetaExpected, theta)); EXPECT(assert_equal(variableIndexExpected, variableIndex)); @@ -602,132 +600,27 @@ TEST(ISAM2, removeFactors) } /* ************************************************************************* */ -//TEST(ISAM2, removeVariables) -//{ -// -// // These variables will be reused and accumulate factors and values -// ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); -// Values fullinit; -// planarSLAM::Graph fullgraph; -// -// vector factorsToRemove; -// -// // i keeps track of the time step -// size_t i = 0; -// -// // Add a prior at time 0 and update isam -// { -// planarSLAM::Graph newfactors; -// newfactors.addPosePrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); -// fullgraph.push_back(newfactors); -// -// Values init; -// init.insert((0), Pose2(0.01, 0.01, 0.01)); -// fullinit.insert((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.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); -// fullgraph.push_back(newfactors); -// -// Values init; -// init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); -// fullinit.insert((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.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); -// newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); -// newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); -// fullgraph.push_back(newfactors); -// -// Values init; -// init.insert((i+1), Pose2(1.01, 0.01, 0.01)); -// init.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); -// init.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); -// fullinit.insert((i+1), Pose2(1.01, 0.01, 0.01)); -// fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); -// fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); -// -// ISAM2Result result = isam.update(newfactors, init); -// factorsToRemove.push_back(result.newFactorsIndices[1]); -// ++ i; -// } -// -// // Add odometry from time 6 to time 10 -// for( ; i<10; ++i) { -// planarSLAM::Graph newfactors; -// newfactors.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); -// fullgraph.push_back(newfactors); -// -// Values init; -// init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); -// fullinit.insert((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.addRelativePose(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); -// newfactors.addBearingRange(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); -// newfactors.addBearingRange(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); -// fullgraph.push_back(newfactors); -// -// Values init; -// init.insert((i+1), Pose2(6.9, 0.1, 0.01)); -// fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01)); -// -// ISAM2Result result = isam.update(newfactors, init); -// factorsToRemove.push_back(result.newFactorsIndices[1]); -// ++ i; -// } -// -// // Compare solutions -// fullgraph.remove(factorsToRemove[0]); -// fullgraph.remove(factorsToRemove[1]); -// fullinit.erase(100); -// CHECK(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; -// } -// 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_UNSAFE(ISAM2, removeVariables) +{ + // These variables will be reused and accumulate factors and values + Values fullinit; + planarSLAM::Graph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + + // Remove the measurement on landmark 0 (Key 100) + FastVector toRemove; + toRemove.push_back(7); + toRemove.push_back(14); + isam.update(planarSLAM::Graph(), Values(), toRemove); + + // Remove the factors and variable from the full system + fullgraph.remove(7); + fullgraph.remove(14); + fullinit.erase(100); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} /* ************************************************************************* */ TEST_UNSAFE(ISAM2, swapFactors)