Automatic removal of unused variables in iSAM2 working in unit tests!
Richard Roberts
parent
86f19362ab
commit
96fc5991db
|
|
@ -31,7 +31,7 @@ namespace gtsam {
|
|||
void ISAM2::Impl::AddVariables(
|
||||
const Values& newTheta, Values& theta, VectorValues& delta,
|
||||
VectorValues& deltaNewton, VectorValues& deltaGradSearch, vector<bool>& 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<Key>& unusedKeys, Values& theta, VariableIndex& variableIndex,
|
||||
void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Clique::shared_ptr& root,
|
||||
Values& theta, VariableIndex& variableIndex,
|
||||
VectorValues& delta, VectorValues& deltaNewton, VectorValues& deltaGradSearch,
|
||||
std::vector<bool>& replacedKeys, Ordering& ordering, Base::Nodes& nodes) {
|
||||
std::vector<bool>& replacedKeys, Ordering& ordering, Base::Nodes& nodes,
|
||||
GaussianFactorGraph& linearFactors) {
|
||||
|
||||
// Get indices of unused keys
|
||||
vector<Index> unusedIndices; unusedIndices.reserve(unusedKeys.size());
|
||||
|
|
@ -96,16 +96,18 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, Values& theta,
|
|||
VectorValues newDeltaNewton(dims);
|
||||
VectorValues newDeltaGradSearch(dims);
|
||||
std::vector<bool> 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<Key>& unusedKeys, Values& theta,
|
|||
ordering.pop_back(key);
|
||||
theta.erase(key);
|
||||
}
|
||||
|
||||
// Finally, permute references to variables
|
||||
if(root)
|
||||
root->permuteWithInverse(unusedToEndInverse);
|
||||
linearFactors.permuteWithInverse(unusedToEndInverse);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -48,14 +48,15 @@ struct ISAM2::Impl {
|
|||
*/
|
||||
static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta,
|
||||
VectorValues& deltaNewton, VectorValues& deltaGradSearch, std::vector<bool>& 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<Key>& unusedKeys, Values& theta, VariableIndex& variableIndex,
|
||||
VectorValues& delta, VectorValues& deltaNewton, VectorValues& deltaGradSearch,
|
||||
std::vector<bool>& replacedKeys, Ordering& ordering, Base::Nodes& nodes);
|
||||
static void RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Clique::shared_ptr& root,
|
||||
Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton,
|
||||
VectorValues& deltaGradSearch, std::vector<bool>& replacedKeys, Ordering& ordering, Base::Nodes& nodes,
|
||||
GaussianFactorGraph& linearFactors);
|
||||
|
||||
/**
|
||||
* Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol
|
||||
|
|
|
|||
|
|
@ -249,6 +249,9 @@ boost::shared_ptr<FastSet<Index> > 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<FastSet<Index> > ISAM2::recalculate(
|
|||
// ordering provides all keys in conditionals, there cannot be others because path to root included
|
||||
tic(2,"affectedKeys");
|
||||
FastList<Index> 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<Index>::iterator key = affectedKeys.begin(); key != affectedKeys.end(); )
|
||||
if(*key >= delta_.size())
|
||||
affectedKeys.erase(key++);
|
||||
else
|
||||
++key;
|
||||
toc(2,"affectedKeys");
|
||||
|
||||
boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Index>()); // 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<Index> 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<Key> 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<Key> 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,12 +603,11 @@ ISAM2Result ISAM2::update(
|
|||
|
||||
tic(4,"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
|
||||
FastSet<Index> 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) {
|
||||
|
|
|
|||
|
|
@ -516,8 +516,7 @@ private:
|
|||
GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
|
||||
|
||||
boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys,
|
||||
const FastVector<Index>& observedKeys,
|
||||
const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
|
||||
const FastVector<Index>& observedKeys, const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
|
||||
// void linear_update(const GaussianFactorGraph& newFactors);
|
||||
void updateDelta(bool forceFullSolve = false) const;
|
||||
|
||||
|
|
|
|||
|
|
@ -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<Key> unusedKeys;
|
||||
unusedKeys.insert(1);
|
||||
vector<size_t> 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<size_t> 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<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));
|
||||
//}
|
||||
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<size_t> 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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue