Ability to manually constrain specified variables to end of ordering in iSAM2
Richard Roberts
parent
0cea1bb323
commit
18a1a98859
|
|
@ -32,9 +32,9 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
|
|||
|
||||
struct ReorderingMode {
|
||||
size_t nFullSystemVars;
|
||||
enum { AS_ADDED, COLAMD } algorithm;
|
||||
enum { NO_CONSTRAINT, LATEST_LAST } constrain;
|
||||
boost::optional<const FastSet<Index>&> latestKeys;
|
||||
enum { /*AS_ADDED,*/ COLAMD } algorithm;
|
||||
enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain;
|
||||
boost::optional<const FastSet<Index>&> constrainedKeys;
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -296,10 +296,10 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& facto
|
|||
toc(2,"variable index");
|
||||
tic(3,"ccolamd");
|
||||
vector<int> cmember(affectedKeysSelector.size(), 0);
|
||||
if(reorderingMode.constrain == ReorderingMode::LATEST_LAST) {
|
||||
assert(reorderingMode.latestKeys);
|
||||
if(keys.size() > reorderingMode.latestKeys->size()) {
|
||||
BOOST_FOREACH(Index var, *reorderingMode.latestKeys) {
|
||||
if(reorderingMode.constrain == ReorderingMode::CONSTRAIN_LAST) {
|
||||
assert(reorderingMode.constrainedKeys);
|
||||
if(keys.size() > reorderingMode.constrainedKeys->size()) {
|
||||
BOOST_FOREACH(Index var, *reorderingMode.constrainedKeys) {
|
||||
cmember[affectedKeysSelectorInverse[var]] = 1;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -37,8 +37,6 @@ using namespace std;
|
|||
|
||||
static const bool disableReordering = false;
|
||||
static const double batchThreshold = 0.65;
|
||||
static const bool latestLast = true;
|
||||
static const bool structuralLast = false;
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
|
|
@ -155,7 +153,8 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
|
|||
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculate(
|
||||
const FastSet<Index>& markedKeys, const FastSet<Index>& structuralKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors, ISAM2Result& result) {
|
||||
const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
|
||||
const boost::optional<FastSet<Index> >& constrainKeys, ISAM2Result& result) {
|
||||
|
||||
// TODO: new factors are linearized twice, the newFactors passed in are not used.
|
||||
|
||||
|
|
@ -181,26 +180,6 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
|
|||
counter++;
|
||||
#endif
|
||||
|
||||
FastSet<Index> affectedStructuralKeys;
|
||||
if(structuralLast) {
|
||||
tic(0, "affectedStructuralKeys");
|
||||
affectedStructuralKeys.insert(structuralKeys.begin(), structuralKeys.end());
|
||||
// For each structural variable, collect the variables up the path to the root,
|
||||
// which will be constrained to the back of the ordering.
|
||||
BOOST_FOREACH(Index key, structuralKeys) {
|
||||
sharedClique clique = this->nodes_[key];
|
||||
while(clique) {
|
||||
affectedStructuralKeys.insert((*clique)->beginFrontals(), (*clique)->endFrontals());
|
||||
#ifndef NDEBUG // This is because BayesTreeClique stores pointers to BayesTreeClique but we actually have the derived type ISAM2Clique
|
||||
clique = boost::dynamic_pointer_cast<Clique>(clique->parent_.lock());
|
||||
#else
|
||||
clique = boost::static_pointer_cast<Clique>(clique->parent_.lock());
|
||||
#endif
|
||||
}
|
||||
}
|
||||
toc(0, "affectedStructuralKeys");
|
||||
}
|
||||
|
||||
if(debug) {
|
||||
cout << "markedKeys: ";
|
||||
BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; }
|
||||
|
|
@ -208,12 +187,6 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
|
|||
cout << "newKeys: ";
|
||||
BOOST_FOREACH(const Index key, newKeys) { cout << key << " "; }
|
||||
cout << endl;
|
||||
cout << "structuralKeys: ";
|
||||
BOOST_FOREACH(const Index key, structuralKeys) { cout << key << " "; }
|
||||
cout << endl;
|
||||
cout << "affectedStructuralKeys: ";
|
||||
BOOST_FOREACH(const Index key, affectedStructuralKeys) { cout << key << " "; }
|
||||
cout << endl;
|
||||
}
|
||||
|
||||
// 1. Remove top of Bayes tree and convert to a factor graph:
|
||||
|
|
@ -257,16 +230,13 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
|
|||
tic(1,"CCOLAMD");
|
||||
// Do a batch step - reorder and relinearize all variables
|
||||
vector<int> cmember(theta_.size(), 0);
|
||||
if(structuralLast) {
|
||||
if(theta_.size() > affectedStructuralKeys.size()) {
|
||||
BOOST_FOREACH(Index var, affectedStructuralKeys) { cmember[var] = 1; }
|
||||
if(latestLast) { BOOST_FOREACH(Index var, newKeys) { cmember[var] = 2; } }
|
||||
}
|
||||
} else if(latestLast) {
|
||||
FastSet<Index> newKeysSet(newKeys.begin(), newKeys.end());
|
||||
if(theta_.size() > newKeysSet.size()) {
|
||||
BOOST_FOREACH(Index var, newKeys) { cmember[var] = 1; }
|
||||
}
|
||||
FastSet<Index> constrainedKeysSet;
|
||||
if(constrainKeys)
|
||||
constrainedKeysSet = *constrainKeys;
|
||||
else
|
||||
constrainedKeysSet.insert(newKeys.begin(), newKeys.end());
|
||||
if(theta_.size() > constrainedKeysSet.size()) {
|
||||
BOOST_FOREACH(Index var, constrainedKeysSet) { cmember[var] = 1; }
|
||||
}
|
||||
Permutation::shared_ptr colamd(Inference::PermutationCOLAMD_(variableIndex_, cmember));
|
||||
Permutation::shared_ptr colamdInverse(colamd->inverse());
|
||||
|
|
@ -364,8 +334,11 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
|
|||
typename Impl::ReorderingMode reorderingMode;
|
||||
reorderingMode.nFullSystemVars = ordering_.nVars();
|
||||
reorderingMode.algorithm = Impl::ReorderingMode::COLAMD;
|
||||
reorderingMode.constrain = Impl::ReorderingMode::LATEST_LAST;
|
||||
reorderingMode.latestKeys = FastSet<Index>(newKeys.begin(), newKeys.end());
|
||||
reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST;
|
||||
if(constrainKeys)
|
||||
reorderingMode.constrainedKeys = *constrainKeys;
|
||||
else
|
||||
reorderingMode.constrainedKeys = FastSet<Index>(newKeys.begin(), newKeys.end());
|
||||
typename Impl::PartialSolveResult partialSolveResult =
|
||||
Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode);
|
||||
toc(2,"PartialSolve");
|
||||
|
|
@ -424,7 +397,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
|
|||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
|
||||
const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices, bool force_relinearize) {
|
||||
const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
|
||||
const boost::optional<FastSet<Symbol> >& constrainedKeys, bool force_relinearize) {
|
||||
|
||||
static const bool debug = ISDEBUG("ISAM2 update");
|
||||
static const bool verbose = ISDEBUG("ISAM2 update verbose");
|
||||
|
|
@ -488,8 +462,6 @@ ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
|
|||
// is a vector of size_t, so the constructor unintentionally resolves to
|
||||
// vector(size_t count, Index value) instead of the iterator constructor.
|
||||
FastVector<Index> newKeys; newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them
|
||||
FastSet<Index> structuralKeys;
|
||||
if(structuralLast) structuralKeys = markedKeys; // If we're using structural-last ordering, make another copy
|
||||
toc(3,"gather involved keys");
|
||||
|
||||
// Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
|
||||
|
|
@ -543,9 +515,17 @@ ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
|
|||
|
||||
tic(8,"recalculate");
|
||||
// 8. Redo top of Bayes tree
|
||||
// Convert constrained symbols to indices
|
||||
boost::optional<FastSet<Index> > constrainedIndices;
|
||||
if(constrainedKeys) {
|
||||
constrainedIndices.reset(FastSet<Index>());
|
||||
BOOST_FOREACH(const Symbol& key, *constrainedKeys) {
|
||||
constrainedIndices->insert(ordering_[key]);
|
||||
}
|
||||
}
|
||||
boost::shared_ptr<FastSet<Index> > replacedKeys;
|
||||
if(!markedKeys.empty() || !newKeys.empty())
|
||||
replacedKeys = recalculate(markedKeys, structuralKeys, newKeys, linearFactors, result);
|
||||
replacedKeys = recalculate(markedKeys, newKeys, linearFactors, constrainedIndices, result);
|
||||
toc(8,"recalculate");
|
||||
|
||||
tic(9,"solve");
|
||||
|
|
|
|||
|
|
@ -346,7 +346,9 @@ public:
|
|||
* (Params::relinearizeSkip).
|
||||
* @return An ISAM2Result struct containing information about the update
|
||||
*/
|
||||
ISAM2Result update(const GRAPH& newFactors = GRAPH(), const VALUES& newTheta = VALUES(), const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
|
||||
ISAM2Result update(const GRAPH& newFactors = GRAPH(), const VALUES& newTheta = VALUES(),
|
||||
const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
|
||||
const boost::optional<FastSet<Symbol> >& constrainedKeys = boost::none,
|
||||
bool force_relinearize = false);
|
||||
|
||||
/** Access the current linearization point */
|
||||
|
|
@ -403,8 +405,9 @@ private:
|
|||
FactorGraph<GaussianFactor>::shared_ptr relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const;
|
||||
FactorGraph<CacheFactor> getCachedBoundaryFactors(Cliques& orphans);
|
||||
|
||||
boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys, const FastSet<Index>& structuralKeys,
|
||||
const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors, ISAM2Result& result);
|
||||
boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys,
|
||||
const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
|
||||
const boost::optional<FastSet<size_t> >& constrainKeys, ISAM2Result& result);
|
||||
// void linear_update(const GaussianFactorGraph& newFactors);
|
||||
|
||||
}; // ISAM2
|
||||
|
|
|
|||
|
|
@ -766,6 +766,149 @@ TEST(ISAM2, removeFactors)
|
|||
EXPECT(assert_equal(expectedGradient, actualGradient));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ISAM2, constrained_ordering)
|
||||
{
|
||||
|
||||
// SETDEBUG("ISAM2 update", true);
|
||||
// SETDEBUG("ISAM2 update verbose", true);
|
||||
// SETDEBUG("ISAM2 recalculate", true);
|
||||
|
||||
// Pose and landmark key types from planarSLAM
|
||||
typedef planarSLAM::PoseKey PoseKey;
|
||||
typedef planarSLAM::PointKey PointKey;
|
||||
|
||||
// Set up parameters
|
||||
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
|
||||
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
|
||||
|
||||
// These variables will be reused and accumulate factors and values
|
||||
GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
|
||||
planarSLAM::Values fullinit;
|
||||
planarSLAM::Graph fullgraph;
|
||||
|
||||
// We will constrain x3 and x4 to the end
|
||||
FastSet<Symbol> constrained; constrained.insert(planarSLAM::PoseKey(3)); constrained.insert(planarSLAM::PoseKey(4));
|
||||
|
||||
// i keeps track of the time step
|
||||
size_t i = 0;
|
||||
|
||||
// Add a prior at time 0 and update isam
|
||||
{
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
planarSLAM::Values init;
|
||||
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
|
||||
fullinit.insert(PoseKey(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.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
planarSLAM::Values init;
|
||||
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
|
||||
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
|
||||
|
||||
if(i >= 3)
|
||||
isam.update(newfactors, init, FastVector<size_t>(), constrained);
|
||||
else
|
||||
isam.update(newfactors, init);
|
||||
}
|
||||
|
||||
// Add odometry from time 5 to 6 and landmark measurement at time 5
|
||||
{
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise);
|
||||
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
planarSLAM::Values init;
|
||||
init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
|
||||
init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
|
||||
init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
|
||||
fullinit.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01));
|
||||
fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
|
||||
fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
|
||||
|
||||
isam.update(newfactors, init, FastVector<size_t>(), constrained);
|
||||
++ i;
|
||||
}
|
||||
|
||||
// Add odometry from time 6 to time 10
|
||||
for( ; i<10; ++i) {
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
planarSLAM::Values init;
|
||||
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
|
||||
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
|
||||
|
||||
isam.update(newfactors, init, FastVector<size_t>(), constrained);
|
||||
}
|
||||
|
||||
// Add odometry from time 10 to 11 and landmark measurement at time 10
|
||||
{
|
||||
planarSLAM::Graph newfactors;
|
||||
newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise);
|
||||
newfactors.addBearingRange(i, 0, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
|
||||
newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise);
|
||||
fullgraph.push_back(newfactors);
|
||||
|
||||
planarSLAM::Values init;
|
||||
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
|
||||
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
|
||||
|
||||
isam.update(newfactors, init, FastVector<size_t>(), constrained);
|
||||
++ i;
|
||||
}
|
||||
|
||||
// Compare solutions
|
||||
EXPECT(isam_check(fullgraph, fullinit, isam));
|
||||
|
||||
// Check that x3 and x4 are last, but either can come before the other
|
||||
EXPECT((isam.getOrdering()[planarSLAM::PoseKey(3)] == 12 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 13) ||
|
||||
(isam.getOrdering()[planarSLAM::PoseKey(3)] == 13 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 12));
|
||||
|
||||
// Check gradient at each node
|
||||
typedef GaussianISAM2<planarSLAM::Values>::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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue