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

Fine-grained ordering constraints in iSAM2

release/4.3a0
Richard Roberts 2012-03-24 16:52:55 +00:00
parent 51f79e0adf
commit 3baba11815
5 changed files with 51 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -15,6 +15,8 @@
* @author Michael Kaess, Richard Roberts * @author Michael Kaess, Richard Roberts
*/ */
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <gtsam/nonlinear/ISAM2-impl.h> #include <gtsam/nonlinear/ISAM2-impl.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
@ -163,7 +165,7 @@ ISAM2::Impl::PartialSolveResult
ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
const FastSet<Index>& keys, const ReorderingMode& reorderingMode) { const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
static const bool debug = ISDEBUG("ISAM2 recalculate"); const bool debug = ISDEBUG("ISAM2 recalculate");
PartialSolveResult result; PartialSolveResult result;
@ -200,9 +202,15 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
vector<int> cmember(affectedKeysSelector.size(), 0); vector<int> cmember(affectedKeysSelector.size(), 0);
if(reorderingMode.constrain == ReorderingMode::CONSTRAIN_LAST) { if(reorderingMode.constrain == ReorderingMode::CONSTRAIN_LAST) {
assert(reorderingMode.constrainedKeys); assert(reorderingMode.constrainedKeys);
if(keys.size() > reorderingMode.constrainedKeys->size()) { if(!reorderingMode.constrainedKeys->empty()) {
BOOST_FOREACH(Index var, *reorderingMode.constrainedKeys) { typedef std::pair<const Index,int> Index_Group;
cmember[affectedKeysSelectorInverse[var]] = 1; if(keys.size() > reorderingMode.constrainedKeys->size()) { // Only if some variables are unconstrained
BOOST_FOREACH(const Index_Group& index_group, *reorderingMode.constrainedKeys) {
cmember[affectedKeysSelectorInverse[index_group.first]] = index_group.second; }
} else {
int minGroup = *boost::range::min_element(boost::adaptors::values(*reorderingMode.constrainedKeys));
BOOST_FOREACH(const Index_Group& index_group, *reorderingMode.constrainedKeys) {
cmember[affectedKeysSelectorInverse[index_group.first]] = index_group.second - minGroup; }
} }
} }
} }

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

@ -36,7 +36,7 @@ struct ISAM2::Impl {
size_t nFullSystemVars; size_t nFullSystemVars;
enum { /*AS_ADDED,*/ COLAMD } algorithm; enum { /*AS_ADDED,*/ COLAMD } algorithm;
enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain;
boost::optional<const FastSet<Index>&> constrainedKeys; boost::optional<FastMap<Index,int> > constrainedKeys;
}; };
/** /**

51
gtsam/nonlinear/ISAM2.cpp
View File

@ -18,6 +18,8 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign; using namespace boost::assign;
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
@ -148,11 +150,11 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
boost::shared_ptr<FastSet<Index> > ISAM2::recalculate( boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors, const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
const boost::optional<FastSet<Index> >& constrainKeys, ISAM2Result& result) { const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result) {
// TODO: new factors are linearized twice, the newFactors passed in are not used. // TODO: new factors are linearized twice, the newFactors passed in are not used.
static const bool debug = ISDEBUG("ISAM2 recalculate"); const bool debug = ISDEBUG("ISAM2 recalculate");
// Input: BayesTree(this), newFactors // Input: BayesTree(this), newFactors
@ -224,13 +226,21 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
tic(1,"CCOLAMD"); tic(1,"CCOLAMD");
// Do a batch step - reorder and relinearize all variables // Do a batch step - reorder and relinearize all variables
vector<int> cmember(theta_.size(), 0); vector<int> cmember(theta_.size(), 0);
FastSet<Index> constrainedKeysSet; if(constrainKeys) {
if(constrainKeys) if(!constrainKeys->empty()) {
constrainedKeysSet = *constrainKeys; typedef std::pair<const Index,int> Index_Group;
else if(theta_.size() > constrainKeys->size()) { // Only if some variables are unconstrained
constrainedKeysSet.insert(newKeys.begin(), newKeys.end()); BOOST_FOREACH(const Index_Group& index_group, *constrainKeys) {
if(theta_.size() > constrainedKeysSet.size()) { cmember[index_group.first] = index_group.second; }
BOOST_FOREACH(Index var, constrainedKeysSet) { cmember[var] = 1; } } else {
int minGroup = *boost::range::min_element(boost::adaptors::values(*constrainKeys));
BOOST_FOREACH(const Index_Group& index_group, *constrainKeys) {
cmember[index_group.first] = index_group.second - minGroup; }
}
}
} else {
if(theta_.size() > newKeys.size()) // Only if some variables are unconstrained
BOOST_FOREACH(Index var, newKeys) { cmember[var] = 1; }
} }
Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember)); Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember));
Permutation::shared_ptr colamdInverse(colamd->inverse()); Permutation::shared_ptr colamdInverse(colamd->inverse());
@ -331,10 +341,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
reorderingMode.nFullSystemVars = ordering_.nVars(); reorderingMode.nFullSystemVars = ordering_.nVars();
reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; reorderingMode.algorithm = Impl::ReorderingMode::COLAMD;
reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST; reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST;
if(constrainKeys) if(constrainKeys) {
reorderingMode.constrainedKeys = *constrainKeys; reorderingMode.constrainedKeys = *constrainKeys;
else } else {
reorderingMode.constrainedKeys = FastSet<Index>(newKeys.begin(), newKeys.end()); reorderingMode.constrainedKeys = FastMap<Index,int>();
BOOST_FOREACH(Index var, newKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); }
}
Impl::PartialSolveResult partialSolveResult = Impl::PartialSolveResult partialSolveResult =
Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode); Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode);
toc(2,"PartialSolve"); toc(2,"PartialSolve");
@ -395,10 +407,10 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
/* ************************************************************************* */ /* ************************************************************************* */
ISAM2Result ISAM2::update( ISAM2Result ISAM2::update(
const NonlinearFactorGraph& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices, const NonlinearFactorGraph& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
const boost::optional<FastSet<Key> >& constrainedKeys, bool force_relinearize) { const boost::optional<FastMap<Key,int> >& constrainedKeys, bool force_relinearize) {
static const bool debug = ISDEBUG("ISAM2 update"); const bool debug = ISDEBUG("ISAM2 update");
static const bool verbose = ISDEBUG("ISAM2 update verbose"); const bool verbose = ISDEBUG("ISAM2 update verbose");
static int count = 0; static int count = 0;
count++; count++;
@ -521,11 +533,12 @@ ISAM2Result ISAM2::update(
tic(9,"recalculate"); tic(9,"recalculate");
// 8. Redo top of Bayes tree // 8. Redo top of Bayes tree
// Convert constrained symbols to indices // Convert constrained symbols to indices
boost::optional<FastSet<Index> > constrainedIndices; boost::optional<FastMap<Index,int> > constrainedIndices;
if(constrainedKeys) { if(constrainedKeys) {
constrainedIndices.reset(FastSet<Index>()); constrainedIndices = FastMap<Index,int>();
BOOST_FOREACH(Key key, *constrainedKeys) { typedef pair<const Key, int> Key_Group;
constrainedIndices->insert(ordering_[key]); BOOST_FOREACH(Key_Group key_group, *constrainedKeys) {
constrainedIndices->insert(make_pair(ordering_[key_group.first], key_group.second));
} }
} }
boost::shared_ptr<FastSet<Index> > replacedKeys; boost::shared_ptr<FastSet<Index> > replacedKeys;

4
gtsam/nonlinear/ISAM2.h
View File

@ -405,7 +405,7 @@ public:
*/ */
ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(), const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
const boost::optional<FastSet<Key> >& constrainedKeys = boost::none, const boost::optional<FastMap<Key,int> >& constrainedKeys = boost::none,
bool force_relinearize = false); bool force_relinearize = false);
/** Access the current linearization point */ /** Access the current linearization point */
@ -464,7 +464,7 @@ private:
boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys, boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys,
const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
const boost::optional<FastSet<size_t> >& constrainKeys, ISAM2Result& result); const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
// void linear_update(const GaussianFactorGraph& newFactors); // void linear_update(const GaussianFactorGraph& newFactors);
void updateDelta(bool forceFullSolve = false) const; void updateDelta(bool forceFullSolve = false) const;

7
tests/testGaussianISAM2.cpp
View File

@ -837,7 +837,9 @@ TEST(ISAM2, constrained_ordering)
planarSLAM::Graph fullgraph; planarSLAM::Graph fullgraph;
// We will constrain x3 and x4 to the end // We will constrain x3 and x4 to the end
FastSet<Key> constrained; constrained.insert(planarSLAM::PoseKey(3)); constrained.insert(planarSLAM::PoseKey(4)); FastMap<Key, int> constrained;
constrained.insert(make_pair(planarSLAM::PoseKey(3), 1));
constrained.insert(make_pair(planarSLAM::PoseKey(4), 2));
// i keeps track of the time step // i keeps track of the time step
size_t i = 0; size_t i = 0;
@ -926,8 +928,7 @@ TEST(ISAM2, constrained_ordering)
EXPECT(isam_check(fullgraph, fullinit, isam)); EXPECT(isam_check(fullgraph, fullinit, isam));
// Check that x3 and x4 are last, but either can come before the other // 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) || 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 // Check gradient at each node
typedef ISAM2::sharedClique sharedClique; typedef ISAM2::sharedClique sharedClique;