From 20cf1da5a9befb39cd37b8d8df38931371a4b19a Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Sun, 1 Jul 2012 18:00:33 +0000 Subject: [PATCH] Merge branch 'master' into new_wrap_local --- CppUnitLite/Test.h | 2 +- gtsam/inference/Permutation.h | 15 +- gtsam/inference/VariableIndex.cpp | 11 + gtsam/inference/VariableIndex.h | 15 + gtsam/inference/tests/testVariableIndex.cpp | 4 +- gtsam/linear/VectorValues.cpp | 14 + gtsam/linear/VectorValues.h | 8 + gtsam/nonlinear/ISAM2-impl.cpp | 72 +- gtsam/nonlinear/ISAM2-impl.h | 10 +- gtsam/nonlinear/ISAM2.cpp | 57 +- gtsam/nonlinear/ISAM2.h | 3 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 4 +- gtsam/nonlinear/NonlinearFactorGraph.h | 2 +- tests/testGaussianISAM2.cpp | 861 +++++--------------- tests/testNonlinearFactorGraph.cpp | 4 +- 15 files changed, 380 insertions(+), 702 deletions(-) diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 22566131e..89d96f463 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -110,7 +110,7 @@ protected: { try { condition; \ result_.addFailure (Failure (name_, __FILE__,__LINE__, SimpleString("Didn't throw: ") + StringFrom(#condition))); \ return; } \ - catch (exception_name& e) {} \ + catch (exception_name&) {} \ catch (...) { \ result_.addFailure (Failure (name_, __FILE__,__LINE__, SimpleString("Wrong exception: ") + StringFrom(#condition) + StringFrom(", expected: ") + StringFrom(#exception_name))); \ return; } } diff --git a/gtsam/inference/Permutation.h b/gtsam/inference/Permutation.h index 3a5b9b0da..39c4447d2 100644 --- a/gtsam/inference/Permutation.h +++ b/gtsam/inference/Permutation.h @@ -133,8 +133,8 @@ public: static Permutation PullToFront(const std::vector& toFront, size_t size, bool filterDuplicates = false); /** - * Create a permutation that pulls the given variables to the front while - * pushing the rest to the back. + * Create a permutation that pushes the given variables to the back while + * pulling the rest to the front. */ static Permutation PushToBack(const std::vector& toBack, size_t size, bool filterDuplicates = false); @@ -154,6 +154,17 @@ public: const_iterator begin() const { return rangeIndices_.begin(); } ///< Iterate through the indices const_iterator end() const { return rangeIndices_.end(); } ///< Iterate through the indices + /** Apply the permutation to a collection, which must have operator[] defined. + * Note that permutable gtsam data structures typically have their own + * permute function to apply a permutation. Permutation::applyToCollection is + * a generic function, e.g. for STL classes. + * @param input The input collection. + * @param output The preallocated output collection, which is assigned output[i] = input[permutation[i]] + */ + template + void applyToCollection(OUTPUT_COLLECTION& output, const INPUT_COLLECTION& input) const { + for(size_t i = 0; i < output.size(); ++i) output[i] = input[(*this)[i]]; } + /// @} /// @name Advanced Interface diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index a29d5e432..4e5eb8649 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -76,4 +76,15 @@ void VariableIndex::permuteInPlace(const Permutation& permutation) { index_.swap(newIndex); } +/* ************************************************************************* */ +void VariableIndex::removeUnusedAtEnd(size_t nToRemove) { +#ifndef NDEBUG + for(size_t i = this->size() - nToRemove; i < this->size(); ++i) + if(!(*this)[i].empty()) + throw std::invalid_argument("Attempting to remove non-empty variables with VariableIndex::removeUnusedAtEnd()"); +#endif + + index_.resize(this->size() - nToRemove); +} + } diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index be47cbd5a..739067dd2 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -119,6 +119,11 @@ public: /** * Remove entries corresponding to the specified factors. + * NOTE: We intentionally do not decrement nFactors_ because the factor + * indices need to remain consistent. Removing factors from a factor graph + * does not shift the indices of other factors. Also, we keep nFactors_ + * one greater than the highest-numbered factor referenced in a VariableIndex. + * * @param indices The indices of the factors to remove, which must match \c factors * @param factors The factors being removed, which must symbolically correspond * exactly to the factors with the specified \c indices that were added. @@ -128,6 +133,12 @@ public: /// Permute the variables in the VariableIndex according to the given permutation void permuteInPlace(const Permutation& permutation); + /** Remove unused empty variables at the end of the ordering (in debug mode + * verifies they are empty). + * @param nToRemove The number of unused variables at the end to remove + */ + void removeUnusedAtEnd(size_t nToRemove); + protected: Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); } /// void VariableIndex::remove(const CONTAINER& indices, const FactorGraph& factors) { + // NOTE: We intentionally do not decrement nFactors_ because the factor + // indices need to remain consistent. Removing factors from a factor graph + // does not shift the indices of other factors. Also, we keep nFactors_ + // one greater than the highest-numbered factor referenced in a VariableIndex. for(size_t fi=0; fikeys().size(); ++ji) { diff --git a/gtsam/inference/tests/testVariableIndex.cpp b/gtsam/inference/tests/testVariableIndex.cpp index 7c8e6fa9e..412b3fa04 100644 --- a/gtsam/inference/tests/testVariableIndex.cpp +++ b/gtsam/inference/tests/testVariableIndex.cpp @@ -44,7 +44,9 @@ TEST(VariableIndex, augment) { VariableIndex actual(fg1); actual.augment(fg2); - CHECK(assert_equal(expected, actual)); + LONGS_EQUAL(16, actual.nEntries()); + LONGS_EQUAL(8, actual.nFactors()); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 15925c644..fd7f2757c 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -45,6 +45,14 @@ VectorValues VectorValues::Zero(const VectorValues& x) { return cloned; } +/* ************************************************************************* */ +vector VectorValues::dims() const { + std::vector result(this->size()); + for(Index j = 0; j < this->size(); ++j) + result[j] = this->dim(j); + return result; +} + /* ************************************************************************* */ void VectorValues::insert(Index j, const Vector& value) { // Make sure j does not already exist @@ -188,4 +196,10 @@ VectorValues VectorValues::permute(const Permutation& permutation) const { return lhs; } +/* ************************************************************************* */ +void VectorValues::swap(VectorValues& other) { + this->values_.swap(other.values_); + this->maps_.swap(other.maps_); +} + } \ No newline at end of file diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 463080db9..aa9c1aa6d 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -134,6 +134,9 @@ namespace gtsam { /** Return the summed dimensionality of all variables. */ size_t dim() const { return values_.rows(); } + /** Return the dimension of each vector in this container */ + std::vector dims() const; + /** Check whether a variable with index \c j exists. */ bool exists(Index j) const { return j < size() && maps_[j].rows() > 0; } @@ -296,6 +299,11 @@ namespace gtsam { */ VectorValues permute(const Permutation& permutation) const; + /** + * Swap the data in this VectorValues with another. + */ + void swap(VectorValues& other); + /// @} private: diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 3c373e0ba..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,9 +58,75 @@ 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, 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()); + BOOST_FOREACH(Key key, unusedKeys) { unusedIndices.push_back(ordering[key]); } + + // Create a permutation that shifts the unused variables to the end + Permutation unusedToEnd = Permutation::PushToBack(unusedIndices, delta.size()); + Permutation unusedToEndInverse = *unusedToEnd.inverse(); + + // Use the permutation to remove unused variables while shifting all the others to take up the space + variableIndex.permuteInPlace(unusedToEnd); + variableIndex.removeUnusedAtEnd(unusedIndices.size()); + { + // Create a vector of variable dimensions with the unused ones removed + // by applying the unusedToEnd permutation to the original vector of + // variable dimensions. We only allocate space in the shifted dims + // vector for the used variables, so that the unused ones are dropped + // when the permutation is applied. + vector originalDims = delta.dims(); + vector dims(delta.size() - unusedIndices.size()); + unusedToEnd.applyToCollection(dims, originalDims); + + // Copy from the old data structures to new ones, only iterating up to + // the number of used variables, and applying the unusedToEnd permutation + // in order to remove the unused variables. + VectorValues newDelta(dims); + VectorValues newDeltaNewton(dims); + VectorValues newDeltaGradSearch(dims); + std::vector newReplacedKeys(replacedKeys.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 < dims.size(); ++j) { + newDelta[j] = delta[unusedToEnd[j]]; + newDeltaNewton[j] = deltaNewton[unusedToEnd[j]]; + newDeltaGradSearch[j] = deltaGradSearch[unusedToEnd[j]]; + newReplacedKeys[j] = replacedKeys[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); + deltaGradSearch.swap(newDeltaGradSearch); + replacedKeys.swap(newReplacedKeys); + nodes.swap(newNodes); + } + + // Reorder and remove from ordering and solution + ordering.permuteWithInverse(unusedToEndInverse); + BOOST_REVERSE_FOREACH(Key key, unusedKeys) { + 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 fdb39d855..b873c87bf 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -48,8 +48,16 @@ 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, 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 * in each NonlinearFactor, obtains the index by calling ordering[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/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 8c4076c56..969e40677 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -54,8 +54,8 @@ double NonlinearFactorGraph::error(const Values& c) const { } /* ************************************************************************* */ -std::set NonlinearFactorGraph::keys() const { - std::set keys; +FastSet NonlinearFactorGraph::keys() const { + FastSet keys; BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) keys.insert(factor->begin(), factor->end()); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 1742c7b78..85cc0e63f 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -47,7 +47,7 @@ namespace gtsam { void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** return keys as an ordered set - ordering is by key value */ - std::set keys() const; + FastSet keys() const; /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */ double error(const Values& c) const; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 994077777..f63d2add5 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -43,7 +44,6 @@ ISAM2 createSlamlikeISAM2( // These variables will be reused and accumulate factors and values ISAM2 isam(params); -// ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); Values fullinit; planarSLAM::Graph fullgraph; @@ -135,7 +135,7 @@ ISAM2 createSlamlikeISAM2( } /* ************************************************************************* */ -TEST_UNSAFE(ISAM2, AddVariables) { +TEST_UNSAFE(ISAM2, ImplAddVariables) { // Create initial state Values theta; @@ -160,8 +160,6 @@ TEST_UNSAFE(ISAM2, AddVariables) { 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, AddVariables) { 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)); @@ -205,6 +201,95 @@ TEST_UNSAFE(ISAM2, AddVariables) { EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); EXPECT(assert_equal(orderingExpected, ordering)); } +/* ************************************************************************* */ +TEST_UNSAFE(ISAM2, ImplRemoveVariables) { + + // Create initial state + Values theta; + theta.insert(0, Pose2(.1, .2, .3)); + theta.insert(1, Pose2(.6, .7, .8)); + theta.insert(100, Point2(.4, .5)); + + SymbolicFactorGraph sfg; + sfg.push_back(boost::make_shared(Index(0), Index(2))); + sfg.push_back(boost::make_shared(Index(0), Index(1))); + VariableIndex variableIndex(sfg); + + VectorValues delta; + delta.insert(0, Vector_(3, .1, .2, .3)); + delta.insert(1, Vector_(3, .4, .5, .6)); + delta.insert(2, Vector_(2, .7, .8)); + + VectorValues deltaNewton; + deltaNewton.insert(0, Vector_(3, .1, .2, .3)); + deltaNewton.insert(1, Vector_(3, .4, .5, .6)); + deltaNewton.insert(2, Vector_(2, .7, .8)); + + VectorValues deltaRg; + deltaRg.insert(0, Vector_(3, .1, .2, .3)); + deltaRg.insert(1, Vector_(3, .4, .5, .6)); + deltaRg.insert(2, Vector_(2, .7, .8)); + + vector replacedKeys(3, false); + replacedKeys[0] = true; + replacedKeys[1] = false; + replacedKeys[2] = true; + + Ordering ordering; ordering += 100, 1, 0; + + ISAM2::Nodes nodes(3); + + // Verify initial state + LONGS_EQUAL(0, ordering[100]); + LONGS_EQUAL(1, ordering[1]); + LONGS_EQUAL(2, ordering[0]); + + // Create expected state + Values thetaExpected; + thetaExpected.insert(0, Pose2(.1, .2, .3)); + thetaExpected.insert(100, Point2(.4, .5)); + + SymbolicFactorGraph sfgRemoved; + sfgRemoved.push_back(boost::make_shared(Index(0), Index(1))); + sfgRemoved.push_back(SymbolicFactorGraph::sharedFactor()); // Add empty factor to keep factor indices consistent + VariableIndex variableIndexExpected(sfgRemoved); + + VectorValues deltaExpected; + deltaExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaExpected.insert(1, Vector_(2, .7, .8)); + + VectorValues deltaNewtonExpected; + deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaNewtonExpected.insert(1, Vector_(2, .7, .8)); + + VectorValues deltaRgExpected; + deltaRgExpected.insert(0, Vector_(3, .1, .2, .3)); + deltaRgExpected.insert(1, Vector_(2, .7, .8)); + + vector replacedKeysExpected(2, true); + + Ordering orderingExpected; orderingExpected += 100, 0; + + ISAM2::Nodes nodesExpected(2); + + // 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); + 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)); + EXPECT(assert_equal(deltaExpected, delta)); + EXPECT(assert_equal(deltaNewtonExpected, deltaNewton)); + EXPECT(assert_equal(deltaRgExpected, deltaRg)); + EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); + EXPECT(assert_equal(orderingExpected, ordering)); +} /* ************************************************************************* */ //TEST(ISAM2, IndicesFromFactors) { @@ -293,7 +378,11 @@ TEST(ISAM2, optimize2) { } /* ************************************************************************* */ -bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam) { +bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { + + TestResult& result_ = result; + const SimpleString name_ = test.getName(); + Values actual = isam.calculateEstimate(); Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first; GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); @@ -303,488 +392,94 @@ bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, cons VectorValues delta = optimize(gbn); Values expected = fullinit.retract(delta, ordering); - return assert_equal(expected, actual); + bool isamEqual = assert_equal(expected, actual); + + // The following two checks make sure that the cached gradients are maintained and used correctly + + // Check gradient at each node + bool nodeGradientsOk = true; + 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); + bool gradOk = assert_equal(expectedGradient[*jit], actual); + EXPECT(gradOk); + nodeGradientsOk = nodeGradientsOk && gradOk; + variablePosition += dim; + } + bool dimOk = clique->gradientContribution().rows() == variablePosition; + EXPECT(dimOk); + nodeGradientsOk = nodeGradientsOk && dimOk; + } + + // 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); + bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient); + EXPECT(expectedGradOk); + bool totalGradOk = assert_equal(expectedGradient, actualGradient); + EXPECT(totalGradOk); + + return nodeGradientsOk && expectedGradOk && totalGradOk; } /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_gaussnewton) { - // 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; - - // 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))); - - isam.update(newfactors, init); - ++ 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)); - - isam.update(newfactors, init); - ++ i; - } + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); // Compare solutions - 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)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_dogleg) { // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); 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.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))); - - isam.update(newfactors, init); - ++ 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)); - - isam.update(newfactors, init); - ++ i; - } + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); // Compare solutions - 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)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_gaussnewton_qr) { // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR)); Values fullinit; planarSLAM::Graph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, false, ISAM2Params::QR)); - // 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))); - - isam.update(newfactors, init); - ++ 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)); - - isam.update(newfactors, init); - ++ i; - } - - // Compare solutions - 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)); + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ TEST(ISAM2, slamlike_solution_dogleg_qr) { // These variables will be reused and accumulate factors and values - ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR)); 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.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))); - - isam.update(newfactors, init); - ++ 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)); - - isam.update(newfactors, init); - ++ i; - } + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false, false, ISAM2Params::QR)); // Compare solutions - 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)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ @@ -888,127 +583,43 @@ TEST(ISAM2, removeFactors) // then removes the 2nd-to-last landmark measurement // 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; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - // i keeps track of the time step - size_t i = 0; + // Remove the 2nd measurement on landmark 0 (Key 100) + FastVector toRemove; + toRemove.push_back(12); + isam.update(planarSLAM::Graph(), Values(), toRemove); - // 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))); - - isam.update(newfactors, init); - ++ 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[0]); - fullgraph.push_back(newfactors[2]); // Don't add measurement on landmark 0 - - 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); - ++ i; - - // 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); - } + // Remove the factor from the full system + fullgraph.remove(12); // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); +} - // 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); - } +/* ************************************************************************* */ +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)); - // 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)); + // 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_)); } /* ************************************************************************* */ @@ -1037,7 +648,7 @@ TEST_UNSAFE(ISAM2, swapFactors) // Compare solutions EXPECT(assert_equal(fullgraph, planarSLAM::Graph(isam.getFactorsUnsafe()))); - EXPECT(isam_check(fullgraph, fullinit, isam)); + EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); // Check gradient at each node typedef ISAM2::sharedClique sharedClique; @@ -1097,7 +708,7 @@ TEST(ISAM2, constrained_ordering) isam.update(newfactors, init); } - CHECK(isam_check(fullgraph, fullinit, isam)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); // Add odometry from time 0 to time 5 for( ; i<5; ++i) { @@ -1165,7 +776,7 @@ TEST(ISAM2, constrained_ordering) } // Compare solutions - EXPECT(isam_check(fullgraph, fullinit, isam)); + EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); // Check that x3 and x4 are last, but either can come before the other EXPECT(isam.getOrdering()[(3)] == 12 && isam.getOrdering()[(4)] == 13); @@ -1204,122 +815,14 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check) { // These variables will be reused and accumulate factors and values - ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); - params.enablePartialRelinearizationCheck = true; - ISAM2 isam(params); 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.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))); - - isam.update(newfactors, init); - ++ 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)); - - isam.update(newfactors, init); - ++ i; - } + ISAM2Params params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false); + params.enablePartialRelinearizationCheck = true; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, params); // Compare solutions - 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)); + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); } /* ************************************************************************* */ diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index f44113f93..f677eb0dc 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -65,9 +65,9 @@ TEST( Graph, error ) TEST( Graph, keys ) { Graph fg = createNonlinearFactorGraph(); - set actual = fg.keys(); + FastSet actual = fg.keys(); LONGS_EQUAL(3, actual.size()); - set::const_iterator it = actual.begin(); + FastSet::const_iterator it = actual.begin(); LONGS_EQUAL(L(1), *(it++)); LONGS_EQUAL(X(1), *(it++)); LONGS_EQUAL(X(2), *(it++));