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

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

@ -118,16 +118,16 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
//template<class CONDITIONAL> template<class CONDITIONAL>
//bool BayesNet<CONDITIONAL>::permuteSeparatorWithInverse( bool BayesNet<CONDITIONAL>::permuteSeparatorWithInverse(
// const Permutation& inversePermutation) { const Permutation& inversePermutation) {
// bool separatorChanged = false; bool separatorChanged = false;
// BOOST_FOREACH(sharedConditional conditional, conditionals_) { BOOST_FOREACH(sharedConditional conditional, conditionals_) {
// if (conditional->permuteSeparatorWithInverse(inversePermutation)) if (conditional->permuteSeparatorWithInverse(inversePermutation))
// separatorChanged = true; separatorChanged = true;
// } }
// return separatorChanged; return separatorChanged;
//} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> 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 * Returns true if any reordered variables appeared in the separator and
* false if not. * false if not.
*/ */
//bool permuteSeparatorWithInverse(const Permutation& inversePermutation); bool permuteSeparatorWithInverse(const Permutation& inversePermutation);
iterator begin() {return conditionals_.begin();} ///<TODO: comment iterator begin() {return conditionals_.begin();} ///<TODO: comment
iterator end() {return conditionals_.end();} ///<TODO: comment iterator end() {return conditionals_.end();} ///<TODO: comment

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

@ -132,44 +132,24 @@ namespace gtsam {
assertInvariants(); 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> template<class DERIVED, class CONDITIONAL>
bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::reduceSeparatorWithInverse( bool BayesTreeCliqueBase<DERIVED, CONDITIONAL>::permuteSeparatorWithInverse(
const internal::Reduction& inverseReduction) const Permutation& inversePermutation) {
{ bool changed = conditional_->permuteSeparatorWithInverse(
bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction); inversePermutation);
#ifndef NDEBUG #ifndef NDEBUG
if(!changed) { if(!changed) {
BOOST_FOREACH(Index& separatorKey, conditional_->parents()) {assert(separatorKey == inversePermutation[separatorKey]);}
BOOST_FOREACH(const derived_ptr& child, children_) { BOOST_FOREACH(const derived_ptr& child, children_) {
assert(child->reduceSeparatorWithInverse(inverseReduction) == false); } assert(child->permuteSeparatorWithInverse(inversePermutation) == false);
}
} }
#endif #endif
if(changed) { if (changed) {
BOOST_FOREACH(const derived_ptr& child, children_) { BOOST_FOREACH(const derived_ptr& child, children_) {
(void) child->reduceSeparatorWithInverse(inverseReduction); } (void) child->permuteSeparatorWithInverse(inversePermutation);
}
} }
assertInvariants(); assertInvariants();
return changed; 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 * traversing the whole tree. Returns whether any separator variables in
* this subtree were reordered. * 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 */ /** return the conditional P(S|Root) on the separator given the root */
BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const; 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) { bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
//#ifndef NDEBUG #ifndef NDEBUG
// BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); } BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); }
//#endif #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; bool parentChanged = false;
BOOST_FOREACH(KeyType& parent, parents()) { BOOST_FOREACH(KeyType& parent, parents()) {
internal::Reduction::const_iterator it = inverseReduction.find(parent); KeyType newParent = inversePermutation[parent];
if(it != inverseReduction.end()) { if(parent != newParent) {
parentChanged = true; parentChanged = true;
parent = it->second; parent = newParent;
} }
} }
assertInvariants(); 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 * Returns true if any reordered variables appeared in the separator and
* false if not. * 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 * Permutes the Conditional, but for efficiency requires the permutation

21
gtsam/inference/Permutation.cpp
View File

@ -161,16 +161,6 @@ namespace internal {
return result; 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 { void Reduction::applyInverse(std::vector<Index>& js) const {
BOOST_FOREACH(Index& j, js) { 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); iterator it = this->find(j);
if(it == this->end()) if(it == this->end())
return j; throw std::out_of_range("Index to Reduction::operator[] not present");
else else
return it->second; return it->second;
} }
@ -205,7 +195,7 @@ namespace internal {
const Index& Reduction::operator[](const Index& j) const { const Index& Reduction::operator[](const Index& j) const {
const_iterator it = this->find(j); const_iterator it = this->find(j);
if(it == this->end()) if(it == this->end())
return j; throw std::out_of_range("Index to Reduction::operator[] not present");
else else
return it->second; return it->second;
} }
@ -217,11 +207,6 @@ namespace internal {
cout << " " << p.first << " : " << p.second << endl; 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 createReducingPermutation(const std::set<Index>& indices) {
Permutation p(indices.size()); Permutation p(indices.size());

4
gtsam/inference/Permutation.h
View File

@ -192,14 +192,12 @@ namespace internal {
typedef gtsam::FastMap<Index,Index> Base; typedef gtsam::FastMap<Index,Index> Base;
static Reduction CreateAsInverse(const Permutation& p); static Reduction CreateAsInverse(const Permutation& p);
static Reduction CreateFromPartialPermutation(const Permutation& selector, const Permutation& p);
void applyInverse(std::vector<Index>& js) const; void applyInverse(std::vector<Index>& js) const;
Permutation inverse() const; Permutation inverse() const;
const Index& operator[](const Index& j); Index& operator[](const Index& j);
const Index& operator[](const Index& j) const; const Index& operator[](const Index& j) const;
void print(const std::string& s="") 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 // 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); 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) { void VariableIndex::removeUnusedAtEnd(size_t nToRemove) {
#ifndef NDEBUG #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 /// Permute the variables in the VariableIndex according to the given permutation
void permuteInPlace(const Permutation& 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 /** Remove unused empty variables at the end of the ordering (in debug mode
* verifies they are empty). * verifies they are empty).
* @param nToRemove The number of unused variables at the end to remove * @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 gtsam;
using namespace std; using namespace std;
/* ************************************************************************* */
TEST(Permutation, Identity) { TEST(Permutation, Identity) {
Permutation expected(5); Permutation expected(5);
expected[0] = 0; expected[0] = 0;
@ -39,7 +38,6 @@ TEST(Permutation, Identity) {
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */
TEST(Permutation, PullToFront) { TEST(Permutation, PullToFront) {
Permutation expected(5); Permutation expected(5);
expected[0] = 4; expected[0] = 4;
@ -57,7 +55,6 @@ TEST(Permutation, PullToFront) {
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */
TEST(Permutation, PushToBack) { TEST(Permutation, PushToBack) {
Permutation expected(5); Permutation expected(5);
expected[0] = 1; expected[0] = 1;
@ -75,7 +72,6 @@ TEST(Permutation, PushToBack) {
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
/* ************************************************************************* */
TEST(Permutation, compose) { TEST(Permutation, compose) {
Permutation p1(5); Permutation p1(5);
p1[0] = 1; p1[0] = 1;
@ -108,25 +104,6 @@ TEST(Permutation, compose) {
LONGS_EQUAL(p1[p2[4]], actual[4]); 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); } 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()); Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse());
if(debug) affectedColamd->print("affectedColamd: "); if(debug) affectedColamd->print("affectedColamd: ");
if(debug) affectedColamdInverse->print("affectedColamdInverse: "); if(debug) affectedColamdInverse->print("affectedColamdInverse: ");
result.reorderingSelector = affectedKeysSelector; result.fullReordering =
result.reorderingPermutation = *affectedColamd; *Permutation::Identity(reorderingMode.nFullSystemVars).partialPermutation(affectedKeysSelector, *affectedColamd);
result.reorderingInverse = internal::Reduction::CreateFromPartialPermutation(affectedKeysSelector, *affectedColamdInverse); result.fullReorderingInverse =
*Permutation::Identity(reorderingMode.nFullSystemVars).partialPermutation(affectedKeysSelector, *affectedColamdInverse);
if(debug) result.fullReordering.print("partialReordering: ");
gttoc(ccolamd_permutations); gttoc(ccolamd_permutations);
gttic(permute_affected_variable_index); gttic(permute_affected_variable_index);

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

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

12
gtsam/nonlinear/ISAM2.h
View File

@ -405,15 +405,9 @@ public:
Base::permuteWithInverse(inversePermutation); Base::permuteWithInverse(inversePermutation);
} }
//bool permuteSeparatorWithInverse(const Permutation& inversePermutation) { bool permuteSeparatorWithInverse(const Permutation& inversePermutation) {
// bool changed = Base::permuteSeparatorWithInverse(inversePermutation); bool changed = Base::permuteSeparatorWithInverse(inversePermutation);
// if(changed) if(cachedFactor_) cachedFactor_->permuteWithInverse(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; 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 { void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const {
cout << str; cout << str;

7
gtsam/nonlinear/Ordering.h
View File

@ -203,13 +203,6 @@ public:
*/ */
void permuteWithInverse(const Permutation& inversePermutation); 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) /// Synonym for operator[](Key)
Index& at(Key key) { return 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;
}