Maintain reverse-lookup inside of Ordering, to allow fast partial permutations, and modified iSAM2 to use partial permutations on Ordering. Removed InvertedOrdering and updated other GTSAM code and unit tests to use the Ordering reverse-lookup function "key" instead of calculating and inverse ordering.
Richard Roberts
parent
6f9601f5e0
commit
013705232c
|
|
@ -55,10 +55,9 @@ void ISAM2::Impl::AddVariables(
|
|||
if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl;
|
||||
++ nextVar;
|
||||
}
|
||||
assert(ordering.nVars() == delta.size());
|
||||
assert(ordering.size() == delta.size());
|
||||
}
|
||||
replacedKeys.resize(ordering.nVars(), false);
|
||||
replacedKeys.resize(ordering.size(), false);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -115,9 +114,10 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Cli
|
|||
}
|
||||
|
||||
// Reorder and remove from ordering and solution
|
||||
ordering.permuteWithInverse(unusedToEndInverse);
|
||||
ordering.permuteInPlace(unusedToEnd);
|
||||
BOOST_REVERSE_FOREACH(Key key, unusedKeys) {
|
||||
ordering.pop_back(key);
|
||||
Ordering::value_type removed = ordering.pop_back();
|
||||
assert(removed.first == key);
|
||||
theta.erase(key);
|
||||
}
|
||||
|
||||
|
|
@ -188,14 +188,14 @@ void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double thres
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const VectorValues& delta, const Ordering::InvertedMap& decoder, const ISAM2Clique::shared_ptr& clique) {
|
||||
void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const VectorValues& delta, const Ordering& ordering, const ISAM2Clique::shared_ptr& clique) {
|
||||
|
||||
// Check the current clique for relinearization
|
||||
bool relinearize = false;
|
||||
BOOST_FOREACH(Index var, clique->conditional()->keys()) {
|
||||
|
||||
// Lookup the key associated with this index
|
||||
Key key = decoder.at(var);
|
||||
Key key = ordering.key(var);
|
||||
|
||||
// Find the threshold for this variable type
|
||||
const Vector& threshold = thresholds.find(Symbol(key).chr())->second;
|
||||
|
|
@ -214,7 +214,7 @@ void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<c
|
|||
// If this node was relinearized, also check its children
|
||||
if(relinearize) {
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) {
|
||||
CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, decoder, child);
|
||||
CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, ordering, child);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -229,8 +229,7 @@ FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::share
|
|||
if(relinearizeThreshold.type() == typeid(double)) {
|
||||
CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root);
|
||||
} else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>)) {
|
||||
Ordering::InvertedMap decoder = ordering.invert();
|
||||
CheckRelinearizationRecursiveMap(relinKeys, boost::get<FastMap<char,Vector> >(relinearizeThreshold), delta, decoder, root);
|
||||
CheckRelinearizationRecursiveMap(relinKeys, boost::get<FastMap<char,Vector> >(relinearizeThreshold), delta, ordering, root);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -267,8 +266,8 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const
|
|||
invalidateIfDebug = boost::none;
|
||||
#endif
|
||||
|
||||
assert(values.size() == ordering.nVars());
|
||||
assert(delta.size() == ordering.nVars());
|
||||
assert(values.size() == ordering.size());
|
||||
assert(delta.size() == ordering.size());
|
||||
Values::iterator key_value;
|
||||
Ordering::const_iterator key_index;
|
||||
for(key_value = values.begin(), key_index = ordering.begin();
|
||||
|
|
|
|||
|
|
@ -353,7 +353,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
|
|||
RgProd_.permuteInPlace(*colamd);
|
||||
gttoc(permute_delta);
|
||||
gttic(permute_ordering);
|
||||
ordering_.permuteWithInverse(*colamdInverse);
|
||||
ordering_.permuteInPlace(*colamd);
|
||||
gttoc(permute_ordering);
|
||||
gttoc(reorder);
|
||||
|
||||
|
|
@ -412,7 +412,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
|
|||
// Reeliminated keys for detailed results
|
||||
if(params_.enableDetailedResults) {
|
||||
BOOST_FOREACH(Index index, affectedAndNewKeys) {
|
||||
result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true;
|
||||
result.detail->variableStatus[ordering_.key(index)].isReeliminated = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -450,7 +450,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
|
|||
|
||||
gttic(PartialSolve);
|
||||
Impl::ReorderingMode reorderingMode;
|
||||
reorderingMode.nFullSystemVars = ordering_.nVars();
|
||||
reorderingMode.nFullSystemVars = ordering_.size();
|
||||
reorderingMode.algorithm = Impl::ReorderingMode::COLAMD;
|
||||
reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST;
|
||||
if(constrainKeys) {
|
||||
|
|
@ -489,7 +489,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
|
|||
RgProd_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
|
||||
gttoc(permute_delta);
|
||||
gttic(permute_ordering);
|
||||
ordering_.reduceWithInverse(partialSolveResult.reorderingInverse);
|
||||
ordering_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
|
||||
gttoc(permute_ordering);
|
||||
if(params_.cacheLinearizedFactors) {
|
||||
gttic(permute_cached_linear);
|
||||
|
|
@ -538,7 +538,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
|
|||
// Root clique variables for detailed results
|
||||
if(params_.enableDetailedResults) {
|
||||
BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) {
|
||||
result.detail->variableStatus[inverseOrdering_->at(index)].inRootClique = true;
|
||||
result.detail->variableStatus[ordering_.key(index)].inRootClique = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -628,7 +628,6 @@ ISAM2Result ISAM2::update(
|
|||
Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_);
|
||||
// New keys for detailed results
|
||||
if(params_.enableDetailedResults) {
|
||||
inverseOrdering_ = ordering_.invert();
|
||||
BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } }
|
||||
gttoc(add_new_variables);
|
||||
|
||||
|
|
@ -649,7 +648,7 @@ ISAM2Result ISAM2::update(
|
|||
// Observed keys for detailed results
|
||||
if(params_.enableDetailedResults) {
|
||||
BOOST_FOREACH(Index index, markedKeys) {
|
||||
result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true;
|
||||
result.detail->variableStatus[ordering_.key(index)].isObserved = true;
|
||||
}
|
||||
}
|
||||
// NOTE: we use assign instead of the iterator constructor here because this
|
||||
|
|
@ -666,7 +665,7 @@ ISAM2Result ISAM2::update(
|
|||
FastSet<Index> relinKeys;
|
||||
if (relinearizeThisStep) {
|
||||
gttic(gather_relinearize_keys);
|
||||
vector<bool> markedRelinMask(ordering_.nVars(), false);
|
||||
vector<bool> markedRelinMask(ordering_.size(), false);
|
||||
// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
|
||||
if(params_.enablePartialRelinearizationCheck)
|
||||
relinKeys = Impl::CheckRelinearizationPartial(root_, delta_, ordering_, params_.relinearizeThreshold);
|
||||
|
|
@ -677,8 +676,8 @@ ISAM2Result ISAM2::update(
|
|||
// Above relin threshold keys for detailed results
|
||||
if(params_.enableDetailedResults) {
|
||||
BOOST_FOREACH(Index index, relinKeys) {
|
||||
result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold = true;
|
||||
result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } }
|
||||
result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold = true;
|
||||
result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } }
|
||||
|
||||
// Add the variables being relinearized to the marked keys
|
||||
BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; }
|
||||
|
|
@ -696,9 +695,9 @@ ISAM2Result ISAM2::update(
|
|||
FastSet<Index> involvedRelinKeys;
|
||||
Impl::FindAll(this->root(), involvedRelinKeys, markedRelinMask);
|
||||
BOOST_FOREACH(Index index, involvedRelinKeys) {
|
||||
if(!result.detail->variableStatus[inverseOrdering_->at(index)].isAboveRelinThreshold) {
|
||||
result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearizeInvolved = true;
|
||||
result.detail->variableStatus[inverseOrdering_->at(index)].isRelinearized = true; } }
|
||||
if(!result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold) {
|
||||
result.detail->variableStatus[ordering_.key(index)].isRelinearizeInvolved = true;
|
||||
result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } }
|
||||
}
|
||||
}
|
||||
gttoc(fluid_find_all);
|
||||
|
|
@ -818,7 +817,7 @@ Values ISAM2::calculateEstimate() const {
|
|||
const VectorValues& delta(getDelta());
|
||||
gttoc(getDelta);
|
||||
gttic(Expmap);
|
||||
vector<bool> mask(ordering_.nVars(), true);
|
||||
vector<bool> mask(ordering_.size(), true);
|
||||
Impl::ExpmapMasked(ret, delta, ordering_, mask);
|
||||
gttoc(Expmap);
|
||||
return ret;
|
||||
|
|
|
|||
|
|
@ -502,9 +502,6 @@ protected:
|
|||
/** The current Dogleg Delta (trust region radius) */
|
||||
mutable boost::optional<double> doglegDelta_;
|
||||
|
||||
/** The inverse ordering, only used for creating ISAM2Result::DetailedResults */
|
||||
boost::optional<Ordering::InvertedMap> inverseOrdering_;
|
||||
|
||||
public:
|
||||
|
||||
typedef ISAM2 This; ///< This class
|
||||
|
|
|
|||
|
|
@ -13,14 +13,8 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void LinearContainerFactor::rekeyFactor(const Ordering& ordering) {
|
||||
Ordering::InvertedMap invOrdering = ordering.invert(); // TODO: inefficient - make more selective ordering invert
|
||||
rekeyFactor(invOrdering);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void LinearContainerFactor::rekeyFactor(const Ordering::InvertedMap& invOrdering) {
|
||||
BOOST_FOREACH(Index& idx, factor_->keys()) {
|
||||
Key fullKey = invOrdering.find(idx)->second;
|
||||
Key fullKey = ordering.key(idx);
|
||||
idx = fullKey;
|
||||
keys_.push_back(fullKey);
|
||||
}
|
||||
|
|
@ -86,34 +80,6 @@ LinearContainerFactor::LinearContainerFactor(
|
|||
initializeLinearizationPoint(linearizationPoint);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearContainerFactor::LinearContainerFactor(const JacobianFactor& factor,
|
||||
const Ordering::InvertedMap& inverted_ordering,
|
||||
const Values& linearizationPoint)
|
||||
: factor_(factor.clone()) {
|
||||
rekeyFactor(inverted_ordering);
|
||||
initializeLinearizationPoint(linearizationPoint);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearContainerFactor::LinearContainerFactor(const HessianFactor& factor,
|
||||
const Ordering::InvertedMap& inverted_ordering,
|
||||
const Values& linearizationPoint)
|
||||
: factor_(factor.clone()) {
|
||||
rekeyFactor(inverted_ordering);
|
||||
initializeLinearizationPoint(linearizationPoint);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LinearContainerFactor::LinearContainerFactor(
|
||||
const GaussianFactor::shared_ptr& factor,
|
||||
const Ordering::InvertedMap& ordering,
|
||||
const Values& linearizationPoint)
|
||||
: factor_(factor->clone()) {
|
||||
rekeyFactor(ordering);
|
||||
initializeLinearizationPoint(linearizationPoint);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void LinearContainerFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||
Base::print(s+"LinearContainerFactor", keyFormatter);
|
||||
|
|
@ -215,9 +181,8 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(
|
|||
}
|
||||
|
||||
// reset ordering
|
||||
Ordering::InvertedMap invLocalOrdering = localOrdering.invert();
|
||||
BOOST_FOREACH(Index& idx, linFactor->keys())
|
||||
idx = ordering[invLocalOrdering[idx] ];
|
||||
idx = ordering[localOrdering.key(idx)];
|
||||
return linFactor;
|
||||
}
|
||||
|
||||
|
|
@ -259,18 +224,11 @@ NonlinearFactor::shared_ptr LinearContainerFactor::negate() const {
|
|||
NonlinearFactorGraph LinearContainerFactor::convertLinearGraph(
|
||||
const GaussianFactorGraph& linear_graph, const Ordering& ordering,
|
||||
const Values& linearizationPoint) {
|
||||
return convertLinearGraph(linear_graph, ordering.invert());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
NonlinearFactorGraph LinearContainerFactor::convertLinearGraph(
|
||||
const GaussianFactorGraph& linear_graph, const InvertedOrdering& invOrdering,
|
||||
const Values& linearizationPoint) {
|
||||
NonlinearFactorGraph result;
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& f, linear_graph)
|
||||
if (f)
|
||||
result.push_back(NonlinearFactorGraph::sharedFactor(
|
||||
new LinearContainerFactor(f, invOrdering, linearizationPoint)));
|
||||
new LinearContainerFactor(f, ordering, linearizationPoint)));
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -45,21 +45,6 @@ public:
|
|||
LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
|
||||
const Values& linearizationPoint = Values());
|
||||
|
||||
/** Alternate constructor: store a linear factor and decode keys with inverted ordering*/
|
||||
LinearContainerFactor(const JacobianFactor& factor,
|
||||
const Ordering::InvertedMap& inverted_ordering,
|
||||
const Values& linearizationPoint = Values());
|
||||
|
||||
/** Alternate constructor: store a linear factor and decode keys with inverted ordering*/
|
||||
LinearContainerFactor(const HessianFactor& factor,
|
||||
const Ordering::InvertedMap& inverted_ordering,
|
||||
const Values& linearizationPoint = Values());
|
||||
|
||||
/** Constructor from shared_ptr with inverted ordering*/
|
||||
LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
|
||||
const Ordering::InvertedMap& ordering,
|
||||
const Values& linearizationPoint = Values());
|
||||
|
||||
// Access
|
||||
|
||||
const GaussianFactor::shared_ptr& factor() const { return factor_; }
|
||||
|
|
@ -130,19 +115,13 @@ public:
|
|||
|
||||
/**
|
||||
* Utility function for converting linear graphs to nonlinear graphs
|
||||
* consisting of LinearContainerFactors. Two versions are available, using
|
||||
* either the ordering the linear graph was linearized around, or the inverse ordering.
|
||||
* If the inverse ordering is present, it will be faster.
|
||||
* consisting of LinearContainerFactors.
|
||||
*/
|
||||
static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph,
|
||||
const Ordering& ordering, const Values& linearizationPoint = Values());
|
||||
|
||||
static NonlinearFactorGraph convertLinearGraph(const GaussianFactorGraph& linear_graph,
|
||||
const InvertedOrdering& invOrdering, const Values& linearizationPoint = Values());
|
||||
|
||||
protected:
|
||||
void rekeyFactor(const Ordering& ordering);
|
||||
void rekeyFactor(const Ordering::InvertedMap& invOrdering);
|
||||
void initializeLinearizationPoint(const Values& linearizationPoint);
|
||||
|
||||
}; // \class LinearContainerFactor
|
||||
|
|
|
|||
|
|
@ -223,7 +223,7 @@ Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
|
|||
variableIndex));
|
||||
|
||||
// Permute the Ordering with the COLAMD ordering
|
||||
ordering->permuteWithInverse(*colamdPerm->inverse());
|
||||
ordering->permuteInPlace(*colamdPerm);
|
||||
return ordering;
|
||||
}
|
||||
|
||||
|
|
@ -254,7 +254,7 @@ Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMDConstrained(const Value
|
|||
variableIndex, index_constraints));
|
||||
|
||||
// Permute the Ordering with the COLAMD ordering
|
||||
ordering->permuteWithInverse(*colamdPerm->inverse());
|
||||
ordering->permuteInPlace(*colamdPerm);
|
||||
return ordering;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -55,10 +55,8 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors,
|
|||
|
||||
// Augment ordering
|
||||
// TODO: allow for ordering constraints within the new variables
|
||||
// FIXME: should just loop over new values
|
||||
BOOST_FOREACH(const NonlinearFactorGraph::sharedFactor& factor, newFactors)
|
||||
BOOST_FOREACH(Key key, factor->keys())
|
||||
ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialValues)
|
||||
ordering_.insert(key_value.key, ordering_.size());
|
||||
|
||||
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors = newFactors.linearize(linPoint_, ordering_);
|
||||
|
||||
|
|
|
|||
|
|
@ -26,24 +26,53 @@ using namespace std;
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering::Ordering(const std::list<Key> & L):nVars_(0) {
|
||||
Ordering::Ordering(const std::list<Key> & L) {
|
||||
int i = 0;
|
||||
BOOST_FOREACH( Key s, L )
|
||||
insert(s, i++) ;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Ordering::permuteWithInverse(const Permutation& inversePermutation) {
|
||||
gttic(Ordering_permuteWithInverse);
|
||||
BOOST_FOREACH(Ordering::value_type& key_order, *this) {
|
||||
key_order.second = inversePermutation[key_order.second];
|
||||
}
|
||||
Ordering::Ordering(const Ordering& other) : order_(other.order_), orderingIndex_(other.size()) {
|
||||
for(iterator item = order_.begin(); item != order_.end(); ++item)
|
||||
orderingIndex_[item->second] = item;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Ordering::reduceWithInverse(const internal::Reduction& inverseReduction) {
|
||||
BOOST_FOREACH(Ordering::value_type& key_order, *this) {
|
||||
key_order.second = inverseReduction[key_order.second];
|
||||
Ordering& Ordering::operator=(const Ordering& rhs) {
|
||||
order_ = rhs.order_;
|
||||
orderingIndex_.resize(rhs.size());
|
||||
for(iterator item = order_.begin(); item != order_.end(); ++item)
|
||||
orderingIndex_[item->second] = item;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Ordering::permuteInPlace(const Permutation& permutation) {
|
||||
gttic(Ordering_permuteInPlace);
|
||||
// Allocate new index and permute in map iterators
|
||||
OrderingIndex newIndex(permutation.size());
|
||||
for(size_t j = 0; j < newIndex.size(); ++j) {
|
||||
newIndex[j] = orderingIndex_[permutation[j]]; // Assign the iterator
|
||||
newIndex[j]->second = j; // Change the stored index to its permuted value
|
||||
}
|
||||
// Swap the new index into this Ordering class
|
||||
orderingIndex_.swap(newIndex);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Ordering::permuteInPlace(const Permutation& selector, const Permutation& permutation) {
|
||||
if(selector.size() != permutation.size())
|
||||
throw invalid_argument("Ordering::permuteInPlace (partial permutation version) called with selector and permutation of different sizes.");
|
||||
// Create new index the size of the permuted entries
|
||||
OrderingIndex newIndex(selector.size());
|
||||
// Permute the affected entries into the new index
|
||||
for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
|
||||
newIndex[dstSlot] = orderingIndex_[selector[permutation[dstSlot]]];
|
||||
// Put the affected entries back in the new order and fix the indices
|
||||
for(size_t slot = 0; slot < selector.size(); ++slot) {
|
||||
orderingIndex_[selector[slot]] = newIndex[slot];
|
||||
orderingIndex_[selector[slot]]->second = selector[slot];
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -51,16 +80,15 @@ void Ordering::reduceWithInverse(const internal::Reduction& inverseReduction) {
|
|||
void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const {
|
||||
cout << str;
|
||||
// Print ordering in index order
|
||||
Ordering::InvertedMap inverted = this->invert();
|
||||
// Print the ordering with varsPerLine ordering entries printed on each line,
|
||||
// for compactness.
|
||||
static const size_t varsPerLine = 10;
|
||||
bool endedOnNewline = false;
|
||||
BOOST_FOREACH(const Ordering::InvertedMap::value_type& index_key, inverted) {
|
||||
if(index_key.first % varsPerLine != 0)
|
||||
BOOST_FOREACH(const Map::iterator& index_key, orderingIndex_) {
|
||||
if(index_key->second % varsPerLine != 0)
|
||||
cout << ", ";
|
||||
cout << index_key.first << ":" << keyFormatter(index_key.second);
|
||||
if(index_key.first % varsPerLine == varsPerLine - 1) {
|
||||
cout << index_key->second<< ":" << keyFormatter(index_key->first);
|
||||
if(index_key->second % varsPerLine == varsPerLine - 1) {
|
||||
cout << "\n";
|
||||
endedOnNewline = true;
|
||||
} else {
|
||||
|
|
@ -79,40 +107,11 @@ bool Ordering::equals(const Ordering& rhs, double tol) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Ordering::value_type Ordering::pop_back() {
|
||||
// FIXME: is there a way of doing this without searching over the entire structure?
|
||||
for (iterator it=begin(); it!=end(); ++it) {
|
||||
if (it->second == nVars_ - 1) {
|
||||
value_type result = *it;
|
||||
order_.erase(it);
|
||||
--nVars_;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
return value_type();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Index Ordering::pop_back(Key key) {
|
||||
Map::iterator item = order_.find(key);
|
||||
if(item == order_.end()) {
|
||||
throw invalid_argument("Attempting to remove a key from an ordering that does not contain that key");
|
||||
} else {
|
||||
if(item->second != nVars_ - 1) {
|
||||
throw invalid_argument("Attempting to remove a key from an ordering in which that key is not last");
|
||||
} else {
|
||||
order_.erase(item);
|
||||
-- nVars_;
|
||||
return nVars_;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Ordering::InvertedMap Ordering::invert() const {
|
||||
InvertedMap result;
|
||||
BOOST_FOREACH(const value_type& p, *this)
|
||||
result.insert(make_pair(p.second, p.first));
|
||||
return result;
|
||||
iterator lastItem = orderingIndex_.back(); // Get the map iterator to the highest-index entry
|
||||
value_type removed = *lastItem; // Save the key-index pair to return
|
||||
order_.erase(lastItem); // Erase the entry from the map
|
||||
orderingIndex_.pop_back(); // Erase the entry from the index
|
||||
return removed; // Return the removed item
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@
|
|||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/list_inserter.hpp>
|
||||
#include <boost/pool/pool_alloc.hpp>
|
||||
#include <set>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -35,8 +35,9 @@ namespace gtsam {
|
|||
class Ordering {
|
||||
protected:
|
||||
typedef FastMap<Key, Index> Map;
|
||||
typedef std::vector<Map::iterator> OrderingIndex;
|
||||
Map order_;
|
||||
Index nVars_;
|
||||
OrderingIndex orderingIndex_;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -51,25 +52,33 @@ public:
|
|||
/// @{
|
||||
|
||||
/// Default constructor for empty ordering
|
||||
Ordering() : nVars_(0) {}
|
||||
Ordering() {}
|
||||
|
||||
/// Copy constructor
|
||||
Ordering(const Ordering& other);
|
||||
|
||||
/// Construct from list, assigns order indices sequentially to list items.
|
||||
Ordering(const std::list<Key> & L) ;
|
||||
Ordering(const std::list<Key> & L);
|
||||
|
||||
/// Assignment operator
|
||||
Ordering& operator=(const Ordering& rhs);
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** One greater than the maximum ordering index, i.e. including missing indices in the count. See also size(). */
|
||||
Index nVars() const { return nVars_; }
|
||||
|
||||
/** The actual number of variables in this ordering, i.e. not including missing indices in the count. See also nVars(). */
|
||||
Index size() const { return order_.size(); }
|
||||
Index size() const { return orderingIndex_.size(); }
|
||||
|
||||
const_iterator begin() const { return order_.begin(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */
|
||||
const_iterator end() const { return order_.end(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */
|
||||
|
||||
Index at(Key key) const { return operator[](key); } ///< Synonym for operator[](Key) const
|
||||
Key key(Index index) const {
|
||||
if(index >= orderingIndex_.size())
|
||||
throw std::out_of_range("Attempting to access out-of-range index in Ordering");
|
||||
else
|
||||
return orderingIndex_[index]->first; }
|
||||
|
||||
/** Assigns the ordering index of the requested \c key into \c index if the symbol
|
||||
* is present in the ordering, otherwise does not modify \c index. The
|
||||
|
|
@ -85,8 +94,7 @@ public:
|
|||
index = i->second;
|
||||
return true;
|
||||
} else
|
||||
return false;
|
||||
}
|
||||
return false; }
|
||||
|
||||
/// Access the index for the requested key, throws std::out_of_range if the
|
||||
/// key is not present in the ordering (note that this differs from the
|
||||
|
|
@ -122,23 +130,25 @@ public:
|
|||
*/
|
||||
const_iterator find(Key key) const { return order_.find(key); }
|
||||
|
||||
/**
|
||||
* Attempts to insert a symbol/order pair with same semantics as stl::Map::insert(),
|
||||
* i.e., returns a pair of iterator and success (false if already present)
|
||||
*/
|
||||
std::pair<iterator,bool> tryInsert(const value_type& key_order) {
|
||||
std::pair<iterator,bool> it_ok(order_.insert(key_order));
|
||||
if(it_ok.second == true && key_order.second+1 > nVars_)
|
||||
nVars_ = key_order.second+1;
|
||||
return it_ok;
|
||||
}
|
||||
|
||||
/** Try insert, but will fail if the key is already present */
|
||||
/** Insert a key-index pair, but will fail if the key is already present */
|
||||
iterator insert(const value_type& key_order) {
|
||||
std::pair<iterator,bool> it_ok(tryInsert(key_order));
|
||||
if(!it_ok.second) throw std::invalid_argument(std::string("Attempting to insert a key into an ordering that already contains that key"));
|
||||
else return it_ok.first;
|
||||
}
|
||||
std::pair<iterator,bool> it_ok(order_.insert(key_order));
|
||||
if(it_ok.second) {
|
||||
if(key_order.second >= orderingIndex_.size())
|
||||
orderingIndex_.resize(key_order.second + 1);
|
||||
orderingIndex_[key_order.second] = it_ok.first;
|
||||
return it_ok.first;
|
||||
} else
|
||||
throw std::invalid_argument(std::string("Attempting to insert a key into an ordering that already contains that key")); }
|
||||
|
||||
/// Test if the key exists in the ordering.
|
||||
bool exists(Key key) const { return order_.count(key) > 0; }
|
||||
|
||||
/** Insert a key-index pair, but will fail if the key is already present */
|
||||
iterator insert(Key key, Index order) { return insert(std::make_pair(key,order)); }
|
||||
|
||||
/// Adds a new key to the ordering with an index of one greater than the current highest index.
|
||||
Index push_back(Key key) { return insert(std::make_pair(key, orderingIndex_.size()))->second; }
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
|
|
@ -154,18 +164,6 @@ public:
|
|||
*/
|
||||
iterator end() { return order_.end(); }
|
||||
|
||||
/// Test if the key exists in the ordering.
|
||||
bool exists(Key key) const { return order_.count(key) > 0; }
|
||||
|
||||
///TODO: comment
|
||||
std::pair<iterator,bool> tryInsert(Key key, Index order) { return tryInsert(std::make_pair(key,order)); }
|
||||
|
||||
///TODO: comment
|
||||
iterator insert(Key key, Index order) { return insert(std::make_pair(key,order)); }
|
||||
|
||||
/// Adds a new key to the ordering with an index of one greater than the current highest index.
|
||||
Index push_back(Key key) { return insert(std::make_pair(key, nVars_))->second; }
|
||||
|
||||
/** Remove the last (last-ordered, not highest-sorting key) symbol/index pair
|
||||
* from the ordering (this version is \f$ O(n) \f$, use it when you do not
|
||||
* know the last-ordered key).
|
||||
|
|
@ -177,16 +175,6 @@ public:
|
|||
*/
|
||||
value_type pop_back();
|
||||
|
||||
/** Remove the last-ordered symbol from the ordering (this version is
|
||||
* \f$ O(\log n) \f$, use it if you already know the last-ordered key).
|
||||
*
|
||||
* Throws std::invalid_argument if the requested key is not actually the
|
||||
* last-ordered.
|
||||
*
|
||||
* @return The index of the symbol that was removed.
|
||||
*/
|
||||
Index pop_back(Key key);
|
||||
|
||||
/**
|
||||
* += operator allows statements like 'ordering += x0,x1,x2,x3;', which are
|
||||
* very useful for unit tests. This functionality is courtesy of
|
||||
|
|
@ -201,24 +189,13 @@ public:
|
|||
* internally, permuting an initial key-sorted ordering into a fill-reducing
|
||||
* ordering.
|
||||
*/
|
||||
void permuteWithInverse(const Permutation& inversePermutation);
|
||||
void permuteInPlace(const Permutation& permutation);
|
||||
|
||||
/**
|
||||
* 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);
|
||||
void permuteInPlace(const Permutation& selector, const Permutation& permutation);
|
||||
|
||||
/// Synonym for operator[](Key)
|
||||
Index& at(Key key) { return operator[](key); }
|
||||
|
||||
/**
|
||||
* Create an inverse mapping from Index->Key, useful for decoding linear systems
|
||||
* @return inverse mapping structure
|
||||
*/
|
||||
InvertedMap invert() const;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
|
@ -235,16 +212,26 @@ private:
|
|||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
template<class ARCHIVE>
|
||||
void save(ARCHIVE & ar, const unsigned int version) const
|
||||
{
|
||||
ar & BOOST_SERIALIZATION_NVP(order_);
|
||||
ar & BOOST_SERIALIZATION_NVP(nVars_);
|
||||
}
|
||||
size_t size_ = orderingIndex_.size(); // Save only the size but not the iterators
|
||||
ar & BOOST_SERIALIZATION_NVP(size_);
|
||||
}
|
||||
template<class ARCHIVE>
|
||||
void load(ARCHIVE & ar, const unsigned int version)
|
||||
{
|
||||
ar & BOOST_SERIALIZATION_NVP(order_);
|
||||
size_t size_;
|
||||
ar & BOOST_SERIALIZATION_NVP(size_);
|
||||
orderingIndex_.resize(size_);
|
||||
for(iterator item = order_.begin(); item != order_.end(); ++item)
|
||||
orderingIndex_[item->second] = item; // Assign the iterators
|
||||
}
|
||||
BOOST_SERIALIZATION_SPLIT_MEMBER()
|
||||
}; // \class Ordering
|
||||
|
||||
// typedef for use with matlab
|
||||
typedef Ordering::InvertedMap InvertedOrdering;
|
||||
|
||||
/**
|
||||
* @class Unordered
|
||||
* @brief a set of unordered indices
|
||||
|
|
@ -270,14 +257,15 @@ public:
|
|||
|
||||
// Create an index formatter that looks up the Key in an inverse ordering, then
|
||||
// formats the key using the provided key formatter, used in saveGraph.
|
||||
struct OrderingIndexFormatter {
|
||||
Ordering::InvertedMap inverseOrdering;
|
||||
const KeyFormatter& keyFormatter;
|
||||
class OrderingIndexFormatter {
|
||||
private:
|
||||
Ordering ordering_;
|
||||
KeyFormatter keyFormatter_;
|
||||
public:
|
||||
OrderingIndexFormatter(const Ordering& ordering, const KeyFormatter& keyFormatter) :
|
||||
inverseOrdering(ordering.invert()), keyFormatter(keyFormatter) {}
|
||||
ordering_(ordering), keyFormatter_(keyFormatter) {}
|
||||
std::string operator()(Index index) {
|
||||
return keyFormatter(inverseOrdering.at(index));
|
||||
}
|
||||
return keyFormatter_(ordering_.key(index)); }
|
||||
};
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
|
|
@ -23,26 +23,31 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testOrdering, simple_modifications ) {
|
||||
TEST( Ordering, simple_modifications ) {
|
||||
Ordering ordering;
|
||||
|
||||
// create an ordering
|
||||
Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4);
|
||||
ordering += x1, x2, x3, x4;
|
||||
|
||||
EXPECT_LONGS_EQUAL(0, ordering[x1]);
|
||||
EXPECT_LONGS_EQUAL(1, ordering[x2]);
|
||||
EXPECT_LONGS_EQUAL(2, ordering[x3]);
|
||||
EXPECT_LONGS_EQUAL(3, ordering[x4]);
|
||||
EXPECT_LONGS_EQUAL(Key(x1), ordering.key(0));
|
||||
EXPECT_LONGS_EQUAL(Key(x2), ordering.key(1));
|
||||
EXPECT_LONGS_EQUAL(Key(x3), ordering.key(2));
|
||||
EXPECT_LONGS_EQUAL(Key(x4), ordering.key(3));
|
||||
|
||||
// pop the last two elements
|
||||
Ordering::value_type x4p = ordering.pop_back();
|
||||
EXPECT_LONGS_EQUAL(3, ordering.size());
|
||||
EXPECT(assert_equal(x4, x4p.first));
|
||||
|
||||
Index x3p = ordering.pop_back(x3);
|
||||
Index x3p = ordering.pop_back().second;
|
||||
EXPECT_LONGS_EQUAL(2, ordering.size());
|
||||
EXPECT_LONGS_EQUAL(2, (int)x3p);
|
||||
|
||||
// try to pop an element that doesn't exist and isn't last
|
||||
CHECK_EXCEPTION(ordering.pop_back(x4), std::invalid_argument);
|
||||
CHECK_EXCEPTION(ordering.pop_back(x1), std::invalid_argument);
|
||||
|
||||
// reassemble back make the ordering 1, 2, 4, 3
|
||||
EXPECT_LONGS_EQUAL(2, ordering.push_back(x4));
|
||||
EXPECT_LONGS_EQUAL(3, ordering.push_back(x3));
|
||||
|
|
@ -50,6 +55,9 @@ TEST( testOrdering, simple_modifications ) {
|
|||
EXPECT_LONGS_EQUAL(2, ordering[x4]);
|
||||
EXPECT_LONGS_EQUAL(3, ordering[x3]);
|
||||
|
||||
EXPECT_LONGS_EQUAL(Key(x4), ordering.key(2));
|
||||
EXPECT_LONGS_EQUAL(Key(x3), ordering.key(3));
|
||||
|
||||
// verify
|
||||
Ordering expectedFinal;
|
||||
expectedFinal += x1, x2, x4, x3;
|
||||
|
|
@ -57,7 +65,36 @@ TEST( testOrdering, simple_modifications ) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testOrdering, invert ) {
|
||||
TEST(Ordering, permute) {
|
||||
Ordering ordering;
|
||||
ordering += 2, 4, 6, 8;
|
||||
|
||||
Ordering expected;
|
||||
expected += 2, 8, 6, 4;
|
||||
|
||||
Permutation permutation(4);
|
||||
permutation[0] = 0;
|
||||
permutation[1] = 3;
|
||||
permutation[2] = 2;
|
||||
permutation[3] = 1;
|
||||
|
||||
Ordering actual = ordering;
|
||||
actual.permuteInPlace(permutation);
|
||||
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
|
||||
EXPECT_LONGS_EQUAL(0, actual[2]);
|
||||
EXPECT_LONGS_EQUAL(1, actual[8]);
|
||||
EXPECT_LONGS_EQUAL(2, actual[6]);
|
||||
EXPECT_LONGS_EQUAL(3, actual[4]);
|
||||
EXPECT_LONGS_EQUAL(2, actual.key(0));
|
||||
EXPECT_LONGS_EQUAL(8, actual.key(1));
|
||||
EXPECT_LONGS_EQUAL(6, actual.key(2));
|
||||
EXPECT_LONGS_EQUAL(4, actual.key(3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Ordering, invert ) {
|
||||
// creates a map with the opposite mapping: Index->Key
|
||||
Ordering ordering;
|
||||
|
||||
|
|
@ -65,14 +102,10 @@ TEST( testOrdering, invert ) {
|
|||
Symbol x1('x', 1), x2('x', 2), x3('x', 3), x4('x', 4);
|
||||
ordering += x1, x2, x3, x4;
|
||||
|
||||
Ordering::InvertedMap actual = ordering.invert();
|
||||
Ordering::InvertedMap expected;
|
||||
expected.insert(make_pair(0, x1));
|
||||
expected.insert(make_pair(1, x2));
|
||||
expected.insert(make_pair(2, x3));
|
||||
expected.insert(make_pair(3, x4));
|
||||
|
||||
EXPECT(assert_container_equality(expected, actual));
|
||||
EXPECT_LONGS_EQUAL(Key(x1), ordering.key(0));
|
||||
EXPECT_LONGS_EQUAL(Key(x2), ordering.key(1));
|
||||
EXPECT_LONGS_EQUAL(Key(x3), ordering.key(2));
|
||||
EXPECT_LONGS_EQUAL(Key(x4), ordering.key(3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -252,7 +252,7 @@ TEST( GaussianFactorGraph, getOrdering)
|
|||
Ordering original; original += L(1),X(1),X(2);
|
||||
FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original));
|
||||
Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic)));
|
||||
Ordering actual = original; actual.permuteWithInverse((*perm.inverse()));
|
||||
Ordering actual = original; actual.permuteInPlace(perm);
|
||||
Ordering expected; expected += L(1),X(2),X(1);
|
||||
EXPECT(assert_equal(expected,actual));
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue