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

For speed, added partial permutation version of VectorValues::permuteInPlace and removed Permutation::partialPermutation (which generated a full-length permutation with only a few entries rearranged)

release/4.3a0
Richard Roberts 2012-12-18 14:21:58 +00:00
parent c42bccbb3e
commit 4cb66dcdcc
5 changed files with 22 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/inference/Permutation.cpp
View File

@ -123,18 +123,6 @@ Permutation::shared_ptr Permutation::permute(const Permutation& permutation) con
return result;
}
/* ************************************************************************* */
Permutation::shared_ptr Permutation::partialPermutation(
const Permutation& selector, const Permutation& partialPermutation) const {
assert(selector.size() == partialPermutation.size());
Permutation::shared_ptr result(new Permutation(*this));
for(Index subsetPos=0; subsetPos<selector.size(); ++subsetPos)
(*result)[selector[subsetPos]] = (*this)[selector[partialPermutation[subsetPos]]];
return result;
}
/* ************************************************************************* */
Permutation::shared_ptr Permutation::inverse() const {
Permutation::shared_ptr result(new Permutation(this->size()));

8
gtsam/inference/Permutation.h
View File

@ -167,14 +167,6 @@ public:
/// @name Advanced Interface
/// @{
/**
* A partial permutation, reorders the variables selected by selector through
* partialPermutation. selector and partialPermutation should have the same
* size, this is checked if NDEBUG is not defined.
*/
Permutation::shared_ptr partialPermutation(const Permutation& selector, const Permutation& partialPermutation) const;
iterator begin() { return rangeIndices_.begin(); } ///< Iterate through the indices
iterator end() { return rangeIndices_.end(); } ///< Iterate through the indices

14
gtsam/linear/VectorValues.cpp
View File

@ -145,6 +145,20 @@ void VectorValues::permuteInPlace(const Permutation& permutation) {
values_.swap(newValues);
}
/* ************************************************************************* */
void VectorValues::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
Values reorderedEntries(selector.size());
// Permute the affected entries into the new index
for(size_t dstSlot = 0; dstSlot < selector.size(); ++dstSlot)
reorderedEntries[dstSlot].swap(values_[selector[permutation[dstSlot]]]);
// Put the affected entries back in the new order
for(size_t slot = 0; slot < selector.size(); ++slot)
values_[selector[slot]].swap(reorderedEntries[slot]);
}
/* ************************************************************************* */
void VectorValues::swap(VectorValues& other) {
this->values_.swap(other.values_);

5
gtsam/linear/VectorValues.h
View File

@ -253,6 +253,11 @@ namespace gtsam {
*/
bool hasSameStructure(const VectorValues& other) const;
/**
* Permute the variables in the VariableIndex according to the given partial permutation
*/
void VectorValues::permuteInPlace(const Permutation& selector, const Permutation& permutation);
/**
* Permute the entries of this VectorValues in place
*/

8
gtsam/nonlinear/ISAM2.cpp
View File

@ -484,11 +484,9 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
variableIndex_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
gttoc(permute_global_variable_index);
gttic(permute_delta);
const Permutation fullReordering = *Permutation::Identity(delta_.size()).
partialPermutation(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
delta_.permuteInPlace(fullReordering);
deltaNewton_.permuteInPlace(fullReordering);
RgProd_.permuteInPlace(fullReordering);
delta_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
deltaNewton_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
RgProd_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation);
gttoc(permute_delta);
gttic(permute_ordering);
ordering_.reduceWithInverse(partialSolveResult.reorderingInverse);