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

Caching linearized factors in iSAM2, improves speed when linearization is expensive 

Merge remote-tracking branch 'svn/branches/iSAM2_cache_linearized' into trunk

Conflicts:
	.cproject
release/4.3a0
Richard Roberts 2012-04-11 14:17:59 +00:00
parent 2c62712be6
commit 91e7dc5882
9 changed files with 81 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/linear/GaussianFactor.h
View File

@ -96,7 +96,7 @@ namespace gtsam {
* to already be inverted. This acts just as a change-of-name for each
* variable. The order of the variables within the factor is not changed.
*/
virtual void permuteWithInverse(const Permutation& inversePermutation) = 0;
virtual void permuteWithInverse(const Permutation& inversePermutation) { IndexFactor::permuteWithInverse(inversePermutation); }
private:
/** Serialization function */

1
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -51,6 +51,7 @@ namespace gtsam {
void GaussianFactorGraph::permuteWithInverse(
const Permutation& inversePermutation) {
BOOST_FOREACH(const sharedFactor& factor, factors_) {
if(factor)
factor->permuteWithInverse(inversePermutation);
}
}

8
gtsam/linear/HessianFactor.h
View File

@ -270,14 +270,6 @@ namespace gtsam {
* @return The linear term \f$ g \f$ */
constColumn linearTerm() const;
/**
* Permutes the GaussianFactor, but for efficiency requires the permutation
* to already be inverted. This acts just as a change-of-name for each
* variable. The order of the variables within the factor is not changed.
*/
virtual void permuteWithInverse(const Permutation& inversePermutation) {
IndexFactor::permuteWithInverse(inversePermutation); }
// Friend unit test classes
friend class ::ConversionConstructorHessianFactorTest;
friend class ::Constructor1HessianFactorTest;

37
gtsam/linear/JacobianFactor.cpp
View File

@ -250,43 +250,6 @@ namespace gtsam {
}
}
/* ************************************************************************* */
void JacobianFactor::permuteWithInverse(const Permutation& inversePermutation) {
// Build a map from the new variable indices to the old slot positions.
typedef FastMap<size_t, size_t> SourceSlots;
SourceSlots sourceSlots;
for(size_t j=0; j<size(); ++j)
sourceSlots.insert(make_pair(inversePermutation[keys_[j]], j));
// Build a vector of variable dimensions in the new order
vector<size_t> dimensions(size() + 1);
size_t j = 0;
BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
dimensions[j++] = Ab_(sourceSlot.second).cols();
}
assert(j == size());
dimensions.back() = 1;
// Copy the variables and matrix into the new order
vector<Index> oldKeys(size());
keys_.swap(oldKeys);
AbMatrix oldMatrix;
BlockAb oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.rows());
Ab_.swap(oldAb);
j = 0;
BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) {
keys_[j] = sourceSlot.first;
Ab_(j++).noalias() = oldAb(sourceSlot.second);
}
Ab_(j).noalias() = oldAb(j);
// Since we're permuting the variables, ensure that entire rows from this
// factor are copied when Combine is called
BOOST_FOREACH(size_t& varpos, firstNonzeroBlocks_) { varpos = 0; }
assertInvariants();
}
/* ************************************************************************* */
Vector JacobianFactor::unweighted_error(const VectorValues& c) const {
Vector e = -getb();

7
gtsam/linear/JacobianFactor.h
View File

@ -169,13 +169,6 @@ namespace gtsam {
*/
virtual size_t getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); }
/**
* Permutes the GaussianFactor, but for efficiency requires the permutation
* to already be inverted. This acts just as a change-of-name for each
* variable. The order of the variables within the factor is not changed.
*/
virtual void permuteWithInverse(const Permutation& inversePermutation);
/**
* return the number of rows in the corresponding linear system
*/

2
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -552,7 +552,7 @@ TEST(GaussianFactor, permuteWithInverse)
actual.permuteWithInverse(inversePermutation);
// actualIndex.permute(*inversePermutation.inverse());
JacobianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0));
JacobianFactor expected(4, A1, 2, A2, 0, A3, b, sharedSigma(2, 1.0));
GaussianFactorGraph expectedFG; expectedFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(expected)));
// GaussianVariableIndex expectedIndex(expectedFG);

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

@ -257,6 +257,9 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
cout << "Full var-ordered eliminated BT:\n";
result.bayesTree->printTree("");
}
// Undo permutation on our subset of cached factors, we must later permute *all* of the cached factors
factors.permuteWithInverse(*affectedColamd);
factors.permuteWithInverse(affectedKeysSelector);
toc(8,"permute eliminated");
return result;

75
gtsam/nonlinear/ISAM2.cpp
View File

@ -78,6 +78,12 @@ ISAM2& ISAM2::operator=(const ISAM2& rhs) {
deltaUptodate_ = rhs.deltaUptodate_;
deltaReplacedMask_ = rhs.deltaReplacedMask_;
nonlinearFactors_ = rhs.nonlinearFactors_;
linearFactors_ = GaussianFactorGraph();
linearFactors_.reserve(rhs.linearFactors_.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& linearFactor, rhs.linearFactors_) {
linearFactors_.push_back(linearFactor->clone()); }
ordering_ = rhs.ordering_;
params_ = rhs.params_;
doglegDelta_ = rhs.doglegDelta_;
@ -125,7 +131,7 @@ FastList<size_t> ISAM2::getAffectedFactors(const FastList<Index>& keys) const {
// retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors)
FactorGraph<GaussianFactor>::shared_ptr
ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys, const FastSet<Index>& relinKeys) const {
tic(1,"getAffectedFactors");
FastList<size_t> candidates = getAffectedFactors(affectedKeys);
@ -139,24 +145,37 @@ ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end());
toc(2,"affectedKeysSet");
tic(3,"check candidates");
tic(3,"check candidates and linearize");
FactorGraph<GaussianFactor>::shared_ptr linearized = boost::make_shared<FactorGraph<GaussianFactor> >();
BOOST_FOREACH(size_t idx, candidates) {
bool inside = true;
bool useCachedLinear = params_.cacheLinearizedFactors;
BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) {
Index var = ordering_[key];
if (affectedKeysSet.find(var) == affectedKeysSet.end()) {
if(affectedKeysSet.find(var) == affectedKeysSet.end()) {
inside = false;
break;
}
if(useCachedLinear && relinKeys.find(var) != relinKeys.end())
useCachedLinear = false;
}
if (inside)
nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]);
if(inside) {
if(useCachedLinear) {
assert(linearFactors_[idx]);
linearized->push_back(linearFactors_[idx]);
assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->symbolic(ordering_)->keys());
} else {
GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_, ordering_);
linearized->push_back(linearFactor);
if(params_.cacheLinearizedFactors) {
assert(linearFactors_[idx]->keys() == linearFactor->keys());
linearFactors_[idx] = linearFactor;
}
toc(3,"check candidates");
}
}
}
toc(3,"check candidates and linearize");
tic(4,"linearize");
FactorGraph<GaussianFactor>::shared_ptr linearized(nonlinearAffectedFactors.linearize(theta_, ordering_));
toc(4,"linearize");
return linearized;
}
@ -187,7 +206,7 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
}
boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys, const FastVector<Index>& newKeys,
const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result) {
// TODO: new factors are linearized twice, the newFactors passed in are not used.
@ -300,11 +319,11 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
toc(1,"reorder");
tic(2,"linearize");
GaussianFactorGraph factors(*nonlinearFactors_.linearize(theta_, ordering_));
linearFactors_ = *nonlinearFactors_.linearize(theta_, ordering_);
toc(2,"linearize");
tic(5,"eliminate");
JunctionTree<GaussianFactorGraph, Base::Clique> jt(factors, variableIndex_);
JunctionTree<GaussianFactorGraph, Base::Clique> jt(linearFactors_, variableIndex_);
sharedClique newRoot;
if(params_.factorization == ISAM2Params::LDL)
newRoot = jt.eliminate(EliminatePreferLDL);
@ -325,7 +344,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
lastAffectedMarkedCount = markedKeys.size();
lastAffectedVariableCount = affectedKeysSet->size();
lastAffectedFactorCount = factors.size();
lastAffectedFactorCount = linearFactors_.size();
return affectedKeysSet;
@ -338,7 +357,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end());
affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end());
tic(1,"relinearizeAffected");
GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys));
GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys));
if(debug) factors.print("Relinearized factors: ");
toc(1,"relinearizeAffected");
@ -360,11 +379,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
// add the cached intermediate results from the boundary of the orphans ...
GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans);
if(debug) cachedBoundary.print("Boundary factors: ");
factors.reserve(factors.size() + cachedBoundary.size());
// Copy so that we can later permute factors
BOOST_FOREACH(const GaussianFactor::shared_ptr& cached, cachedBoundary) {
factors.push_back(cached->clone());
}
factors.push_back(cachedBoundary);
toc(2,"cached");
// END OF COPIED CODE
@ -410,6 +425,15 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
tic(5,"permute ordering");
ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
toc(5,"permute ordering");
if(params_.cacheLinearizedFactors) {
tic(6,"permute cached linear");
//linearFactors_.permuteWithInverse(partialSolveResult.fullReorderingInverse);
FastList<size_t> permuteLinearIndices = getAffectedFactors(affectedAndNewKeys);
BOOST_FOREACH(size_t idx, permuteLinearIndices) {
linearFactors_[idx]->permuteWithInverse(partialSolveResult.fullReorderingInverse);
}
toc(6,"permute cached linear");
}
toc(4,"reorder and eliminate");
@ -526,11 +550,12 @@ ISAM2Result ISAM2::update(
toc(4,"gather involved keys");
// Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
FastSet<Index> relinKeys;
if (relinearizeThisStep) {
tic(5,"gather relinearize keys");
vector<bool> markedRelinMask(ordering_.nVars(), false);
// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
FastSet<Index> relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold);
relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold);
if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging
// Add the variables being relinearized to the marked keys
@ -563,15 +588,21 @@ ISAM2Result ISAM2::update(
}
tic(8,"linearize new");
tic(1,"linearize");
// 7. Linearize new factors
if(params_.cacheLinearizedFactors) {
tic(1,"linearize");
FactorGraph<GaussianFactor>::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_);
linearFactors_.push_back(*linearFactors);
assert(nonlinearFactors_.size() == linearFactors_.size());
toc(1,"linearize");
tic(2,"augment VI");
// Augment the variable index with the new factors
variableIndex_.augment(*linearFactors);
toc(2,"augment VI");
} else {
variableIndex_.augment(*newFactors.symbolic(ordering_));
}
toc(8,"linearize new");
tic(9,"recalculate");
@ -587,7 +618,7 @@ ISAM2Result ISAM2::update(
}
boost::shared_ptr<FastSet<Index> > replacedKeys;
if(!markedKeys.empty() || !newKeys.empty())
replacedKeys = recalculate(markedKeys, newKeys, linearFactors, constrainedIndices, result);
replacedKeys = recalculate(markedKeys, relinKeys, newKeys, constrainedIndices, result);
// Update replaced keys mask (accumulates until back-substitution takes place)
if(replacedKeys) {

19
gtsam/nonlinear/ISAM2.h
View File

@ -118,6 +118,13 @@ struct ISAM2Params {
*/
Factorization factorization;
/** Whether to cache linear factors (default: true).
* This can improve performance if linearization is expensive, but can hurt
* performance if linearization is very cleap due to computation to look up
* additional keys.
*/
bool cacheLinearizedFactors;
KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
/** Specify parameters as constructor arguments */
@ -128,11 +135,12 @@ struct ISAM2Params {
bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization
bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError
Factorization _factorization = ISAM2Params::LDL, ///< see ISAM2Params::factorization
bool _cacheLinearizedFactors = true, ///< see ISAM2Params::cacheLinearizedFactors
const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter
) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold),
relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization),
keyFormatter(_keyFormatter) {}
cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter) {}
};
/**
@ -341,6 +349,9 @@ protected:
/** All original nonlinear factors are stored here to use during relinearization */
NonlinearFactorGraph nonlinearFactors_;
/** The current linear factors, which are only updated as needed */
mutable GaussianFactorGraph linearFactors_;
/** The current elimination ordering Symbols to Index (integer) keys.
*
* We keep it up to date as we add and reorder variables.
@ -453,11 +464,11 @@ public:
private:
FastList<size_t> getAffectedFactors(const FastList<Index>& keys) const;
FactorGraph<GaussianFactor>::shared_ptr relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const;
FactorGraph<GaussianFactor>::shared_ptr relinearizeAffectedFactors(const FastList<Index>& affectedKeys, const FastSet<Index>& relinKeys) const;
GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys,
const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys,
const FastVector<Index>& newKeys,
const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
// void linear_update(const GaussianFactorGraph& newFactors);
void updateDelta(bool forceFullSolve = false) const;