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

Detailed results optionally returned by ISAM2::update, with the status of each variable

release/4.3a0
Richard Roberts 2012-04-12 03:04:32 +00:00
parent fbef6ce63f
commit 792c8ee55a
2 changed files with 94 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
gtsam/nonlinear/ISAM2.cpp
View File

@ -206,7 +206,7 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
}
boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys, const FastVector<Index>& newKeys,
const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys, const FastVector<Index>& observedKeys,
const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result) {
// TODO: new factors are linearized twice, the newFactors passed in are not used.
@ -237,8 +237,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
cout << "markedKeys: ";
BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; }
cout << endl;
cout << "newKeys: ";
BOOST_FOREACH(const Index key, newKeys) { cout << key << " "; }
cout << "observedKeys: ";
BOOST_FOREACH(const Index key, observedKeys) { cout << key << " "; }
cout << endl;
}
@ -270,12 +270,13 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
FastList<Index> affectedKeys = affectedBayesNet.ordering();
toc(2,"affectedKeys");
boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Index>()); // Will return this result
if(affectedKeys.size() >= theta_.size() * batchThreshold) {
tic(3,"batch");
tic(0,"add keys");
boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Index>());
BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); }
toc(0,"add keys");
@ -296,8 +297,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
}
}
} else {
if(theta_.size() > newKeys.size()) { // Only if some variables are unconstrained
BOOST_FOREACH(Index var, newKeys) { cmember[var] = 1; }
if(theta_.size() > observedKeys.size()) { // Only if some variables are unconstrained
BOOST_FOREACH(Index var, observedKeys) { cmember[var] = 1; }
}
}
Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember));
@ -338,15 +339,17 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
this->insert(newRoot);
toc(6,"insert");
toc(3,"batch");
result.variablesReeliminated = affectedKeysSet->size();
lastAffectedMarkedCount = markedKeys.size();
lastAffectedVariableCount = affectedKeysSet->size();
lastAffectedFactorCount = linearFactors_.size();
return affectedKeysSet;
// Reeliminated keys for detailed results
if(params_.enableDetailedResults)
BOOST_FOREACH(Key key, theta_.keys()) { result.detail->variableStatus[key].isReeliminated = true; }
toc(3,"batch");
} else {
@ -355,7 +358,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
// 2. Add the new factors \Factors' into the resulting factor graph
FastList<Index> affectedAndNewKeys;
affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end());
affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end());
affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), observedKeys.end());
tic(1,"relinearizeAffected");
GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys));
if(debug) factors.print("Relinearized factors: ");
@ -363,6 +366,10 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
if(debug) { cout << "Affected keys: "; BOOST_FOREACH(const Index key, affectedKeys) { cout << key << " "; } cout << endl; }
// Reeliminated keys for detailed results
if(params_.enableDetailedResults)
BOOST_FOREACH(Index index, affectedAndNewKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isReeliminated = true; }
result.variablesReeliminated = affectedAndNewKeys.size();
lastAffectedMarkedCount = markedKeys.size();
lastAffectedVariableCount = affectedKeys.size();
@ -391,7 +398,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
tic(1,"list to set");
// create a partial reordering for the new and contaminated factors
// markedKeys are passed in: those variables will be forced to the end in the ordering
boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Index>(markedKeys));
affectedKeysSet->insert(markedKeys.begin(), markedKeys.end());
affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end());
toc(1,"list to set");
@ -404,7 +411,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
reorderingMode.constrainedKeys = *constrainKeys;
} else {
reorderingMode.constrainedKeys = FastMap<Index,int>();
BOOST_FOREACH(Index var, newKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); }
BOOST_FOREACH(Index var, observedKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); }
}
Impl::PartialSolveResult partialSolveResult =
Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode, (params_.factorization == ISAM2Params::QR));
@ -467,9 +474,13 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
toc(7,"orphans");
toc(4,"incremental");
return affectedKeysSet;
}
// 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; }
return affectedKeysSet;
}
/* ************************************************************************* */
@ -490,6 +501,8 @@ ISAM2Result ISAM2::update(
lastBacksubVariableCount = 0;
lastNnzTop = 0;
ISAM2Result result;
if(params_.enableDetailedResults)
result.detail = ISAM2Result::DetailedResults();
const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0);
if(verbose) {
@ -528,6 +541,10 @@ ISAM2Result ISAM2::update(
tic(2,"add new variables");
// 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}.
Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_, Base::nodes_);
// New keys for detailed results
if(params_.enableDetailedResults) {
inverseOrdering_ = ordering_.invert();
BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } }
toc(2,"add new variables");
tic(3,"evaluate error before");
@ -543,10 +560,13 @@ ISAM2Result ISAM2::update(
FastSet<Index> markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors
markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys
}
// Observed keys for detailed results
if(params_.enableDetailedResults)
BOOST_FOREACH(Index index, markedKeys) { result.detail->variableStatus[inverseOrdering_->at(index)].isObserved = true; }
// NOTE: we use assign instead of the iterator constructor here because this
// is a vector of size_t, so the constructor unintentionally resolves to
// vector(size_t count, Index value) instead of the iterator constructor.
FastVector<Index> newKeys; newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them
FastVector<Index> observedKeys; observedKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them
toc(4,"gather involved keys");
// Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
@ -558,6 +578,12 @@ ISAM2Result ISAM2::update(
relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold);
if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging
// 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; } }
// Add the variables being relinearized to the marked keys
BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; }
markedKeys.insert(relinKeys.begin(), relinKeys.end());
@ -565,8 +591,20 @@ ISAM2Result ISAM2::update(
tic(6,"fluid find_all");
// 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors.
if (!relinKeys.empty() && this->root())
Impl::FindAll(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator
if (!relinKeys.empty() && this->root()) {
// add other cliques that have the marked ones in the separator
Impl::FindAll(this->root(), markedKeys, markedRelinMask);
// Relin involved keys for detailed results
if(params_.enableDetailedResults) {
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; } }
}
}
toc(6,"fluid find_all");
tic(7,"expmap");
@ -617,8 +655,8 @@ ISAM2Result ISAM2::update(
}
}
boost::shared_ptr<FastSet<Index> > replacedKeys;
if(!markedKeys.empty() || !newKeys.empty())
replacedKeys = recalculate(markedKeys, relinKeys, newKeys, constrainedIndices, result);
if(!markedKeys.empty() || !observedKeys.empty())
replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, constrainedIndices, result);
// Update replaced keys mask (accumulates until back-substitution takes place)
if(replacedKeys) {

39
gtsam/nonlinear/ISAM2.h
View File

@ -127,6 +127,8 @@ struct ISAM2Params {
KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
bool enableDetailedResults; ///< Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: false)
/** Specify parameters as constructor arguments */
ISAM2Params(
OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams
@ -140,7 +142,8 @@ struct ISAM2Params {
) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold),
relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization),
cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter) {}
cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter),
enableDetailedResults(false) {}
};
/**
@ -201,6 +204,35 @@ struct ISAM2Result {
* used later to refer to the factors in order to remove them.
*/
FastVector<size_t> newFactorsIndices;
/** A struct holding detailed results, which must be enabled with
* ISAM2Params::enableDetailedResults.
*/
struct DetailedResults {
/** The status of a single variable, this struct is stored in
* DetailedResults::variableStatus */
struct VariableStatus {
/** Whether the variable was just reeliminated, due to being relinearized,
* observed, new, or on the path up to the root clique from another
* reeliminated variable. */
bool isReeliminated;
bool isAboveRelinThreshold; ///< Whether the variable was just relinearized due to being above the relinearization threshold
bool isRelinearizeInvolved; ///< Whether the variable was below the relinearization threshold but was relinearized by being involved in a factor with a variable above the relinearization threshold
bool isRelinearized; /// Whether the variable was relinearized, either by being above the relinearization threshold or by involvement.
bool isObserved; ///< Whether the variable was just involved in new factors
bool isNew; ///< Whether the variable itself was just added
bool inRootClique; ///< Whether the variable is in the root clique
VariableStatus(): isReeliminated(false), isRelinearized(false), isObserved(false), isNew(false), inRootClique(false) {}
};
/** The status of each variable during this update, see VariableStatus.
*/
FastMap<Key, VariableStatus> variableStatus;
};
/** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See
* Detail for information about the results data stored here. */
boost::optional<DetailedResults> detail;
};
struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianConditional> {
@ -364,6 +396,9 @@ 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_;
private:
#ifndef NDEBUG
std::vector<bool> lastRelinVariables_;
@ -468,7 +503,7 @@ private:
GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys,
const FastVector<Index>& newKeys,
const FastVector<Index>& observedKeys,
const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
// void linear_update(const GaussianFactorGraph& newFactors);
void updateDelta(bool forceFullSolve = false) const;