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

Some usage (still more to do) of sparse and partial permutations in iSAM2 partial reordering to improve asymptotic performance.

release/4.3a0
Richard Roberts 2012-11-03 21:10:58 +00:00
parent 10f69cfca0
commit 4512e4c85d
17 changed files with 179 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/inference/BayesNet-inl.h
View File

@ -118,16 +118,16 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class CONDITIONAL>
bool BayesNet<CONDITIONAL>::permuteSeparatorWithInverse(
const Permutation& inversePermutation) {
bool separatorChanged = false;
BOOST_FOREACH(sharedConditional conditional, conditionals_) {
if (conditional->permuteSeparatorWithInverse(inversePermutation))
separatorChanged = true;
}
return separatorChanged;
}
//template<class CONDITIONAL>
//bool BayesNet<CONDITIONAL>::permuteSeparatorWithInverse(
// const Permutation& inversePermutation) {
// bool separatorChanged = false;
// BOOST_FOREACH(sharedConditional conditional, conditionals_) {
// if (conditional->permuteSeparatorWithInverse(inversePermutation))
// separatorChanged = true;
// }
// return separatorChanged;
//}
/* ************************************************************************* */
template<class CONDITIONAL>

2
gtsam/inference/BayesNet.h
View File

@ -225,7 +225,7 @@ public:
* Returns true if any reordered variables appeared in the separator and
* false if not.
*/
bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
//bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
iterator begin() {return conditionals_.begin();} ///<TODO: comment
iterator end() {return conditionals_.end();} ///<TODO: comment

40
gtsam/inference/BayesTreeCliqueBase-inl.h
View File

@ -132,24 +132,44 @@ namespace gtsam {
assertInvariants();
}
/* ************************************************************************* */
// template<class DERIVED, class CONDITIONAL>
// bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::permuteSeparatorWithInverse(
// const Permutation& inversePermutation) {
// bool changed = conditional_->permuteSeparatorWithInverse(
// inversePermutation);
//#ifndef NDEBUG
// if(!changed) {
// BOOST_FOREACH(Index& separatorKey, conditional_->parents()) {assert(separatorKey == inversePermutation[separatorKey]);}
// BOOST_FOREACH(const derived_ptr& child, children_) {
// assert(child->permuteSeparatorWithInverse(inversePermutation) == false);
// }
// }
//#endif
// if (changed) {
// BOOST_FOREACH(const derived_ptr& child, children_) {
// (void) child->permuteSeparatorWithInverse(inversePermutation);
// }
// }
// assertInvariants();
// return changed;
// }
/* ************************************************************************* */
template<class DERIVED, class CONDITIONAL>
bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::permuteSeparatorWithInverse(
const Permutation& inversePermutation) {
bool changed = conditional_->permuteSeparatorWithInverse(
inversePermutation);
bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::reduceSeparatorWithInverse(
const internal::Reduction& inverseReduction)
{
bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction);
#ifndef NDEBUG
if(!changed) {
BOOST_FOREACH(Index& separatorKey, conditional_->parents()) {assert(separatorKey == inversePermutation[separatorKey]);}
BOOST_FOREACH(const derived_ptr& child, children_) {
assert(child->permuteSeparatorWithInverse(inversePermutation) == false);
}
assert(child->reduceSeparatorWithInverse(inverseReduction) == false); }
}
#endif
if (changed) {
if(changed) {
BOOST_FOREACH(const derived_ptr& child, children_) {
(void) child->permuteSeparatorWithInverse(inversePermutation);
}
(void) child->reduceSeparatorWithInverse(inverseReduction); }
}
assertInvariants();
return changed;

9
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -183,7 +183,14 @@ namespace gtsam {
* traversing the whole tree. Returns whether any separator variables in
* this subtree were reordered.
*/
bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
//bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
/** Permute variables when they only appear in the separators. In this
* case the running intersection property will be used to prevent always
* traversing the whole tree. Returns whether any separator variables in
* this subtree were reordered.
*/
bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction);
/** return the conditional P(S|Root) on the separator given the root */
BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;

31
gtsam/inference/IndexConditional.cpp
View File

@ -44,16 +44,33 @@ namespace gtsam {
}
/* ************************************************************************* */
bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
#ifndef NDEBUG
BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); }
#endif
//bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
//#ifndef NDEBUG
// BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); }
//#endif
// bool parentChanged = false;
// BOOST_FOREACH(KeyType& parent, parents()) {
// KeyType newParent = inversePermutation[parent];
// if(parent != newParent) {
// parentChanged = true;
// parent = newParent;
// }
// }
// assertInvariants();
// return parentChanged;
//}
/* ************************************************************************* */
bool IndexConditional::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) {
#ifndef NDEBUG
BOOST_FOREACH(KeyType key, frontals()) { assert(inverseReduction.find(key) == inverseReduction.end()); }
#endif
bool parentChanged = false;
BOOST_FOREACH(KeyType& parent, parents()) {
KeyType newParent = inversePermutation[parent];
if(parent != newParent) {
internal::Reduction::const_iterator it = inverseReduction.find(parent);
if(it != inverseReduction.end()) {
parentChanged = true;
parent = newParent;
parent = it->second;
}
}
assertInvariants();

8
gtsam/inference/IndexConditional.h
View File

@ -112,7 +112,13 @@ namespace gtsam {
* Returns true if any reordered variables appeared in the separator and
* false if not.
*/
bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
//bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
/** Permute the variables when only separator variables need to be permuted.
* Returns true if any reordered variables appeared in the separator and
* false if not.
*/
bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction);
/**
* Permutes the Conditional, but for efficiency requires the permutation

21
gtsam/inference/Permutation.cpp
View File

@ -161,6 +161,16 @@ namespace internal {
return result;
}
/* ************************************************************************* */
Reduction Reduction::CreateFromPartialPermutation(const Permutation& selector, const Permutation& p) {
if(selector.size() != p.size())
throw invalid_argument("internal::Reduction::CreateFromPartialPermutation called with selector and permutation of different sizes");
Reduction result;
for(size_t dstSlot = 0; dstSlot < p.size(); ++dstSlot)
result.insert(make_pair(selector[dstSlot], selector[p[dstSlot]]));
return result;
}
/* ************************************************************************* */
void Reduction::applyInverse(std::vector<Index>& js) const {
BOOST_FOREACH(Index& j, js) {
@ -183,10 +193,10 @@ namespace internal {
}
/* ************************************************************************* */
Index& Reduction::operator[](const Index& j) {
const Index& Reduction::operator[](const Index& j) {
iterator it = this->find(j);
if(it == this->end())
throw std::out_of_range("Index to Reduction::operator[] not present");
return j;
else
return it->second;
}
@ -195,7 +205,7 @@ namespace internal {
const Index& Reduction::operator[](const Index& j) const {
const_iterator it = this->find(j);
if(it == this->end())
throw std::out_of_range("Index to Reduction::operator[] not present");
return j;
else
return it->second;
}
@ -207,6 +217,11 @@ namespace internal {
cout << " " << p.first << " : " << p.second << endl;
}
/* ************************************************************************* */
bool Reduction::equals(const Reduction& other, double tol) const {
return (const Base&)(*this) == (const Base&)other;
}
/* ************************************************************************* */
Permutation createReducingPermutation(const std::set<Index>& indices) {
Permutation p(indices.size());

4
gtsam/inference/Permutation.h
View File

@ -192,12 +192,14 @@ namespace internal {
typedef gtsam::FastMap<Index,Index> Base;
static Reduction CreateAsInverse(const Permutation& p);
static Reduction CreateFromPartialPermutation(const Permutation& selector, const Permutation& p);
void applyInverse(std::vector<Index>& js) const;
Permutation inverse() const;
Index& operator[](const Index& j);
const Index& operator[](const Index& j);
const Index& operator[](const Index& j) const;
void print(const std::string& s="") const;
bool equals(const Reduction& other, double tol = 1e-9) const;
};
// Reduce the variable indices so that those in the set are mapped to start at zero

14
gtsam/inference/VariableIndex.cpp
View File

@ -76,6 +76,20 @@ void VariableIndex::permuteInPlace(const Permutation& permutation) {
index_.swap(newIndex);
}
/* ************************************************************************* */
void VariableIndex::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
if(selector.size() != permutation.size())
throw invalid_argument("VariableIndex::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
// Create new index the size of the permuted entries
vector<VariableIndex::Factors> newIndex(selector.size());
// Permute the affected entries into the new index
for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
newIndex[dstSlot].swap(this->index_[selector[permutation[dstSlot]]]);
// Put the affected entries back in the new order
for(size_t slot = 0; slot < selector.size(); ++slot)
this->index_[selector[slot]].swap(newIndex[slot]);
}
/* ************************************************************************* */
void VariableIndex::removeUnusedAtEnd(size_t nToRemove) {
#ifndef NDEBUG

3
gtsam/inference/VariableIndex.h
View File

@ -135,6 +135,9 @@ public:
/// Permute the variables in the VariableIndex according to the given permutation
void permuteInPlace(const Permutation& permutation);
/// Permute the variables in the VariableIndex according to the given partial permutation
void permuteInPlace(const Permutation& selector, 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

23
gtsam/inference/tests/testPermutation.cpp
View File

@ -25,6 +25,7 @@
using namespace gtsam;
using namespace std;
/* ************************************************************************* */
TEST(Permutation, Identity) {
Permutation expected(5);
expected[0] = 0;
@ -38,6 +39,7 @@ TEST(Permutation, Identity) {
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Permutation, PullToFront) {
Permutation expected(5);
expected[0] = 4;
@ -55,6 +57,7 @@ TEST(Permutation, PullToFront) {
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Permutation, PushToBack) {
Permutation expected(5);
expected[0] = 1;
@ -72,6 +75,7 @@ TEST(Permutation, PushToBack) {
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Permutation, compose) {
Permutation p1(5);
p1[0] = 1;
@ -104,6 +108,25 @@ TEST(Permutation, compose) {
LONGS_EQUAL(p1[p2[4]], actual[4]);
}
/* ************************************************************************* */
TEST(Reduction, CreateFromPartialPermutation) {
Permutation selector(3);
selector[0] = 2;
selector[1] = 4;
selector[2] = 6;
Permutation p(3);
p[0] = 2;
p[1] = 0;
p[2] = 1;
internal::Reduction expected;
expected.insert(make_pair(2,6));
expected.insert(make_pair(4,2));
expected.insert(make_pair(6,4));
internal::Reduction actual = internal::Reduction::CreateFromPartialPermutation(selector, p);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

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

@ -353,11 +353,9 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse());
if(debug) affectedColamd->print("affectedColamd: ");
if(debug) affectedColamdInverse->print("affectedColamdInverse: ");
result.fullReordering =
*Permutation::Identity(reorderingMode.nFullSystemVars).partialPermutation(affectedKeysSelector, *affectedColamd);
result.fullReorderingInverse =
*Permutation::Identity(reorderingMode.nFullSystemVars).partialPermutation(affectedKeysSelector, *affectedColamdInverse);
if(debug) result.fullReordering.print("partialReordering: ");
result.reorderingSelector = affectedKeysSelector;
result.reorderingPermutation = *affectedColamd;
result.reorderingInverse = internal::Reduction::CreateFromPartialPermutation(affectedKeysSelector, *affectedColamdInverse);
gttoc(ccolamd_permutations);
gttic(permute_affected_variable_index);

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

@ -26,8 +26,9 @@ struct ISAM2::Impl {
struct PartialSolveResult {
ISAM2::sharedClique bayesTree;
Permutation fullReordering;
Permutation fullReorderingInverse;
Permutation reorderingSelector;
Permutation reorderingPermutation;
internal::Reduction reorderingInverse;
};
struct ReorderingMode {

16
gtsam/nonlinear/ISAM2.cpp
View File

@ -481,22 +481,24 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
// re-eliminate. The reordered variables are also mentioned in the
// orphans and the leftover cached factors.
gttic(permute_global_variable_index);
variableIndex_.permuteInPlace(partialSolveResult.fullReordering);
variableIndex_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
gttoc(permute_global_variable_index);
gttic(permute_delta);
delta_ = delta_.permute(partialSolveResult.fullReordering);
deltaNewton_ = deltaNewton_.permute(partialSolveResult.fullReordering);
RgProd_ = RgProd_.permute(partialSolveResult.fullReordering);
const Permutation fullReordering = *Permutation::Identity(delta_.size()).
partialPermutation(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
delta_ = delta_.permute(fullReordering);
deltaNewton_ = deltaNewton_.permute(fullReordering);
RgProd_ = RgProd_.permute(fullReordering);
gttoc(permute_delta);
gttic(permute_ordering);
ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
ordering_.reduceWithInverse(partialSolveResult.reorderingInverse);
gttoc(permute_ordering);
if(params_.cacheLinearizedFactors) {
gttic(permute_cached_linear);
//linearFactors_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
FastList<size_t> permuteLinearIndices = getAffectedFactors(affectedAndNewKeys);
BOOST_FOREACH(size_t idx, permuteLinearIndices) {
linearFactors_[idx]->permuteWithInverse(partialSolveResult.fullReorderingInverse);
linearFactors_[idx]->reduceWithInverse(partialSolveResult.reorderingInverse);
}
gttoc(permute_cached_linear);
}
@ -514,7 +516,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
gttic(orphans);
gttic(permute);
BOOST_FOREACH(sharedClique orphan, orphans) {
(void)orphan->permuteSeparatorWithInverse(partialSolveResult.fullReorderingInverse);
(void)orphan->reduceSeparatorWithInverse(partialSolveResult.reorderingInverse);
}
gttoc(permute);
gttic(insert);

12
gtsam/nonlinear/ISAM2.h
View File

@ -405,9 +405,15 @@ public:
Base::permuteWithInverse(inversePermutation);
}
bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
bool changed = Base::permuteSeparatorWithInverse(inversePermutation);
if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
//bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
// bool changed = Base::permuteSeparatorWithInverse(inversePermutation);
// if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
// return changed;
//}
bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) {
bool changed = Base::reduceSeparatorWithInverse(inverseReduction);
if(changed) if(cachedFactor_) cachedFactor_->reduceWithInverse(inverseReduction);
return changed;
}

7
gtsam/nonlinear/Ordering.cpp
View File

@ -39,6 +39,13 @@ void Ordering::permuteWithInverse(const Permutation& inversePermutation) {
}
}
/* ************************************************************************* */
void Ordering::reduceWithInverse(const internal::Reduction& inverseReduction) {
BOOST_FOREACH(Ordering::value_type& key_order, *this) {
key_order.second = inverseReduction[key_order.second];
}
}
/* ************************************************************************* */
void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const {
cout << str;

7
gtsam/nonlinear/Ordering.h
View File

@ -203,6 +203,13 @@ public:
*/
void permuteWithInverse(const Permutation& inversePermutation);
/**
* Reorder the variables with a permutation. This is typically used
* internally, permuting an initial key-sorted ordering into a fill-reducing
* ordering.
*/
void reduceWithInverse(const internal::Reduction& inverseReduction);
/// Synonym for operator[](Key)
Index& at(Key key) { return operator[](key); }