12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged from branch 'branches/isam2-remove-vars'

release/4.3a0
Richard Roberts 2012-06-30 23:57:34 +00:00
parent 32ad753a38 db344aaeb1
commit deb2ecdcd0
15 changed files with 380 additions and 702 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CppUnitLite/Test.h
View File

@ -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; } }

15
gtsam/inference/Permutation.h
View File

@ -133,8 +133,8 @@ public:
static Permutation PullToFront(const std::vector<Index>& 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<Index>& 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<typename INPUT_COLLECTION, typename OUTPUT_COLLECTION>
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

11
gtsam/inference/VariableIndex.cpp
View File

@ -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);
}
}

15
gtsam/inference/VariableIndex.h
View File

@ -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(); } ///<TODO: comment
Factor_iterator factorsEnd(Index variable) { checkVar(variable); return index_[variable].end(); } ///<TODO: comment
@ -235,6 +246,10 @@ void VariableIndex::augment(const FactorGraph& factors) {
/* ************************************************************************* */
template<typename CONTAINER, class FactorGraph>
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; fi<factors.size(); ++fi)
if(factors[fi]) {
for(size_t ji = 0; ji < factors[fi]->keys().size(); ++ji) {

4
gtsam/inference/tests/testVariableIndex.cpp
View File

@ -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));
}
/* ************************************************************************* */

14
gtsam/linear/VectorValues.cpp
View File

@ -45,6 +45,14 @@ VectorValues VectorValues::Zero(const VectorValues& x) {
return cloned;
}
/* ************************************************************************* */
vector<size_t> VectorValues::dims() const {
std::vector<size_t> 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_);
}
}

8
gtsam/linear/VectorValues.h
View File

@ -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<size_t> 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:

72
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -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,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<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) {
// Get indices of unused keys
vector<Index> 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<size_t> originalDims = delta.dims();
vector<size_t> 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<bool> 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);
}
/* ************************************************************************* */

10
gtsam/nonlinear/ISAM2-impl.h
View File

@ -48,7 +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, 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

51
gtsam/nonlinear/ISAM2.cpp
View File

@ -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) {

3
gtsam/nonlinear/ISAM2.h
View File

@ -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;

4
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -54,8 +54,8 @@ double NonlinearFactorGraph::error(const Values& c) const {
}
/* ************************************************************************* */
std::set<Key> NonlinearFactorGraph::keys() const {
std::set<Key> keys;
FastSet<Key> NonlinearFactorGraph::keys() const {
FastSet<Key> keys;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor)
keys.insert(factor->begin(), factor->end());

2
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -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<Key> keys() const;
FastSet<Key> 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;

827
tests/testGaussianISAM2.cpp
View File

@ -6,6 +6,7 @@
#include <gtsam/base/debug.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
@ -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<IndexFactor>(Index(0), Index(2)));
sfg.push_back(boost::make_shared<IndexFactor>(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<bool> 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<IndexFactor>(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<bool> replacedKeysExpected(2, true);
Ordering orderingExpected; orderingExpected += 100, 0;
ISAM2::Nodes nodesExpected(2);
// 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);
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,102 +392,12 @@ 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);
/* ************************************************************************* */
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;
}
// Compare solutions
CHECK(isam_check(fullgraph, fullinit, isam));
// 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
@ -411,10 +410,14 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
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));
bool gradOk = assert_equal(expectedGradient[*jit], actual);
EXPECT(gradOk);
nodeGradientsOk = nodeGradientsOk && gradOk;
variablePosition += dim;
}
LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition);
bool dimOk = clique->gradientContribution().rows() == variablePosition;
EXPECT(dimOk);
nodeGradientsOk = nodeGradientsOk && dimOk;
}
// Check gradient
@ -423,368 +426,60 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
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));
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
Values fullinit;
planarSLAM::Graph fullgraph;
ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
// Compare solutions
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<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));
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;
// 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, 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<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));
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<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));
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;
// 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
// Remove the 2nd measurement on landmark 0 (Key 100)
FastVector<size_t> toRemove;
EXPECT_LONGS_EQUAL(isam.getFactorsUnsafe().size()-2, result.newFactorsIndices[1]);
toRemove.push_back(result.newFactorsIndices[1]);
toRemove.push_back(12);
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 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(isam_check(fullgraph, fullinit, isam, *this, result_));
}
// 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_));
}
/* ************************************************************************* */
@ -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<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));
CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
}
/* ************************************************************************* */

4
tests/testNonlinearFactorGraph.cpp
View File

@ -65,9 +65,9 @@ TEST( Graph, error )
TEST( Graph, keys )
{
Graph fg = createNonlinearFactorGraph();
set<Key> actual = fg.keys();
FastSet<Key> actual = fg.keys();
LONGS_EQUAL(3, actual.size());
set<Key>::const_iterator it = actual.begin();
FastSet<Key>::const_iterator it = actual.begin();
LONGS_EQUAL(L(1), *(it++));
LONGS_EQUAL(X(1), *(it++));
LONGS_EQUAL(X(2), *(it++));