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

Reverted r14178, r14179, r14195, r14196 (isam2 chain optimizations) to move them to a branch

release/4.3a0
Richard Roberts 2012-11-11 20:51:03 +00:00
parent 43778a6ed3
commit 0c3761a2a9
19 changed files with 56 additions and 279 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam.h
View File

@ -765,6 +765,7 @@ class IndexConditional {
gtsam::IndexFactor* toFactor() const;
//Advanced interface
bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
};
@ -787,6 +788,7 @@ virtual class BayesNet {
void push_front(This& conditional);
void pop_front();
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
};
#include <gtsam/inference/BayesTree.h>
@ -830,6 +832,7 @@ virtual class BayesTreeClique {
// derived_ptr parent() const { return parent_.lock(); }
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
// FIXME: need wrapped versions graphs, BayesNet
// BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
@ -856,6 +859,7 @@ virtual class SymbolicBayesNet : gtsam::SymbolicBayesNetBase {
//Advanced Interface
void pop_front();
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
};
#include <gtsam/inference/SymbolicFactorGraph.h>
@ -1764,6 +1768,7 @@ virtual class ISAM2Clique {
void print(string s);
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
bool permuteSeparatorWithInverse(const gtsam::Permutation& inversePermutation);
};
class ISAM2Result {

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,44 +132,24 @@ 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>::reduceSeparatorWithInverse(
const internal::Reduction& inverseReduction)
{
bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction);
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->reduceSeparatorWithInverse(inverseReduction) == false); }
assert(child->permuteSeparatorWithInverse(inversePermutation) == false);
}
}
#endif
if(changed) {
if (changed) {
BOOST_FOREACH(const derived_ptr& child, children_) {
(void) child->reduceSeparatorWithInverse(inverseReduction); }
(void) child->permuteSeparatorWithInverse(inversePermutation);
}
}
assertInvariants();
return changed;

9
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -183,14 +183,7 @@ namespace gtsam {
* traversing the whole tree. Returns whether any separator variables in
* this subtree were reordered.
*/
//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);
bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
/** 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,33 +44,16 @@ namespace gtsam {
}
/* ************************************************************************* */
//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 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()) {
internal::Reduction::const_iterator it = inverseReduction.find(parent);
if(it != inverseReduction.end()) {
KeyType newParent = inversePermutation[parent];
if(parent != newParent) {
parentChanged = true;
parent = it->second;
parent = newParent;
}
}
assertInvariants();

8
gtsam/inference/IndexConditional.h
View File

@ -112,13 +112,7 @@ namespace gtsam {
* Returns true if any reordered variables appeared in the separator and
* false if not.
*/
//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);
bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
/**
* Permutes the Conditional, but for efficiency requires the permutation

21
gtsam/inference/Permutation.cpp
View File

@ -161,16 +161,6 @@ 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) {
@ -193,10 +183,10 @@ namespace internal {
}
/* ************************************************************************* */
const Index& Reduction::operator[](const Index& j) {
Index& Reduction::operator[](const Index& j) {
iterator it = this->find(j);
if(it == this->end())
return j;
throw std::out_of_range("Index to Reduction::operator[] not present");
else
return it->second;
}
@ -205,7 +195,7 @@ namespace internal {
const Index& Reduction::operator[](const Index& j) const {
const_iterator it = this->find(j);
if(it == this->end())
return j;
throw std::out_of_range("Index to Reduction::operator[] not present");
else
return it->second;
}
@ -217,11 +207,6 @@ 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,14 +192,12 @@ 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;
const Index& operator[](const Index& j);
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,20 +76,6 @@ 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,9 +135,6 @@ 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,7 +25,6 @@
using namespace gtsam;
using namespace std;
/* ************************************************************************* */
TEST(Permutation, Identity) {
Permutation expected(5);
expected[0] = 0;
@ -39,7 +38,6 @@ TEST(Permutation, Identity) {
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Permutation, PullToFront) {
Permutation expected(5);
expected[0] = 4;
@ -57,7 +55,6 @@ TEST(Permutation, PullToFront) {
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Permutation, PushToBack) {
Permutation expected(5);
expected[0] = 1;
@ -75,7 +72,6 @@ TEST(Permutation, PushToBack) {
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Permutation, compose) {
Permutation p1(5);
p1[0] = 1;
@ -108,25 +104,6 @@ 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,9 +353,11 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse());
if(debug) affectedColamd->print("affectedColamd: ");
if(debug) affectedColamdInverse->print("affectedColamdInverse: ");
result.reorderingSelector = affectedKeysSelector;
result.reorderingPermutation = *affectedColamd;
result.reorderingInverse = internal::Reduction::CreateFromPartialPermutation(affectedKeysSelector, *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: ");
gttoc(ccolamd_permutations);
gttic(permute_affected_variable_index);

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

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

16
gtsam/nonlinear/ISAM2.cpp
View File

@ -481,24 +481,22 @@ 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.reorderingSelector, partialSolveResult.reorderingPermutation);
variableIndex_.permuteInPlace(partialSolveResult.fullReordering);
gttoc(permute_global_variable_index);
gttic(permute_delta);
const Permutation fullReordering = *Permutation::Identity(delta_.size()).
partialPermutation(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
delta_ = delta_.permute(fullReordering);
deltaNewton_ = deltaNewton_.permute(fullReordering);
RgProd_ = RgProd_.permute(fullReordering);
delta_ = delta_.permute(partialSolveResult.fullReordering);
deltaNewton_ = deltaNewton_.permute(partialSolveResult.fullReordering);
RgProd_ = RgProd_.permute(partialSolveResult.fullReordering);
gttoc(permute_delta);
gttic(permute_ordering);
ordering_.reduceWithInverse(partialSolveResult.reorderingInverse);
ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
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]->reduceWithInverse(partialSolveResult.reorderingInverse);
linearFactors_[idx]->permuteWithInverse(partialSolveResult.fullReorderingInverse);
}
gttoc(permute_cached_linear);
}
@ -516,7 +514,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
gttic(orphans);
gttic(permute);
BOOST_FOREACH(sharedClique orphan, orphans) {
(void)orphan->reduceSeparatorWithInverse(partialSolveResult.reorderingInverse);
(void)orphan->permuteSeparatorWithInverse(partialSolveResult.fullReorderingInverse);
}
gttoc(permute);
gttic(insert);

12
gtsam/nonlinear/ISAM2.h
View File

@ -405,15 +405,9 @@ public:
Base::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);
bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
bool changed = Base::permuteSeparatorWithInverse(inversePermutation);
if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation);
return changed;
}

7
gtsam/nonlinear/Ordering.cpp
View File

@ -39,13 +39,6 @@ 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,13 +203,6 @@ 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); }

100
tests/timeiSAM2Chain.cpp
View File

@ -1,100 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file timeiSAM2Chain.cpp
* @brief Times each iteration of a long chain in iSAM2, to measure asymptotic performance
* @author Richard Roberts
*/
#include <gtsam/base/timing.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Marginals.h>
#include <fstream>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/export.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
typedef Pose2 Pose;
typedef NoiseModelFactor1<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2;
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(Pose::Dim());
int main(int argc, char *argv[]) {
const size_t steps = 50000;
cout << "Playing forward " << steps << " time steps..." << endl;
ISAM2 isam2;
// Times
vector<double> times(steps);
for(size_t step=0; step < steps; ++step) {
Values newVariables;
NonlinearFactorGraph newFactors;
// Collect measurements and new variables for the current step
gttic_(Create_measurements);
if(step == 0) {
// Add prior
newFactors.add(PriorFactor<Pose>(0, Pose(), noiseModel::Unit::Create(Pose::Dim())));
newVariables.insert(0, Pose());
} else {
Vector eta = Vector::Random(Pose::Dim()) * 0.1;
Pose2 between = Pose().retract(eta);
// Add between
newFactors.add(BetweenFactor<Pose>(step-1, step, between, model));
newVariables.insert(step, isam2.calculateEstimate<Pose>(step-1) * between);
}
gttoc_(Create_measurements);
// Update iSAM2
gttic_(Update_ISAM2);
isam2.update(newFactors, newVariables);
gttoc_(Update_ISAM2);
tictoc_finishedIteration_();
tictoc_getNode(node, Update_ISAM2);
times[step] = node->time();
if(step % 1000 == 0) {
cout << "Step " << step << endl;
tictoc_print_();
}
}
tictoc_print_();
// Write times
ofstream timesFile("times.txt");
BOOST_FOREACH(double t, times) {
timesFile << t << "\n"; }
return 0;
}