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

ISAM2 refactoring and documentation

release/4.3a0
Richard Roberts 2011-09-07 15:42:49 +00:00
parent 660127489d
commit 936ee6d9fd
3 changed files with 116 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
gtsam/nonlinear/ISAM2-impl-inl.h
View File

@ -14,8 +14,34 @@ namespace gtsam {
template<class CONDITIONAL, class VALUES>
struct ISAM2<CONDITIONAL, VALUES>::Impl {
static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
static FastSet<Index> IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph<VALUES>& factors);
/**
* Add new variables to the ISAM2 system.
* @param newTheta Initial values for new variables
* @param theta Current solution to be augmented with new initialization
* @param delta Current linear delta to be augmented with zeros
* @param ordering Current ordering to be augmented with new variables
* @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
*/
static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
/**
* Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol
* in each NonlinearFactor, obtains the index by calling ordering[symbol].
* @param ordering The current ordering from which to obtain the variable indices
* @param factors The factors from which to extract the variables
* @return The set of variables indices from the factors
*/
static FastSet<Index> IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph<VALUES>& factors);
/**
* Find the set of variables to be relinearized according to relinearizeThreshold.
* Any variables in the VectorValues delta whose vector magnitude is greater than
* or equal to relinearizeThreshold are returned.
* @param delta The linear delta to check against the threshold
* @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold
*/
static FastSet<Index> CheckRelinearization(Permuted<VectorValues>& delta, double relinearizeThreshold);
};
/* ************************************************************************* */
@ -70,4 +96,16 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES>::Impl::IndicesFromFactors(const Orderin
return indices;
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES>
FastSet<Index> ISAM2<CONDITIONAL,VALUES>::Impl::CheckRelinearization(Permuted<VectorValues>& delta, double relinearizeThreshold) {
FastSet<Index> relinKeys;
for(Index var=0; var<delta.size(); ++var) {
double maxDelta = delta[var].lpNorm<Eigen::Infinity>();
if(maxDelta >= relinearizeThreshold) {
relinKeys.insert(var);
}
}
}
}

69
gtsam/nonlinear/ISAM2-inl.h
View File

@ -716,21 +716,19 @@ void ISAM2<Conditional, Values>::update(
if(structuralLast) structuralKeys = markedKeys; // If we're using structural-last ordering, make another copy
toc(3,"gather involved keys");
vector<bool> markedRelinMask(ordering_.nVars(), false);
bool relinAny = false;
// Check relinearization if we're at a 10th step, or we are using a looser loop relin threshold
if (force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0)) { // todo: every n steps
tic(4,"gather relinearize keys");
vector<bool> markedRelinMask(ordering_.nVars(), false);
bool relinAny = false;
// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
for(Index var=0; var<delta_.size(); ++var) {
//cout << var << ": " << delta_[var].transpose() << endl;
double maxDelta = delta_[var].lpNorm<Eigen::Infinity>();
if(maxDelta >= params_.relinearizeThreshold || disableReordering) {
markedRelinMask[var] = true;
markedKeys.insert(var);
if(!relinAny) relinAny = true;
}
}
FastSet<Index> relinKeys = Impl::CheckRelinearization(delta_, params_.relinearizeThreshold);
if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, 0.0); // This is used for debugging
BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; }
markedKeys.insert(relinKeys.begin(), relinKeys.end());
if(!relinKeys.empty())
relinAny = true;
toc(4,"gather relinearize keys");
tic(5,"fluid find_all");
@ -739,37 +737,29 @@ void ISAM2<Conditional, Values>::update(
// mark all cliques that involve marked variables
if(this->root())
find_all(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator
// richard commented these out since now using an array to mark keys
//affectedKeys.sort(); // remove duplicates
//affectedKeys.unique();
// merge with markedKeys
}
// richard commented these out since now using an array to mark keys
//markedKeys.splice(markedKeys.begin(), affectedKeys, affectedKeys.begin(), affectedKeys.end());
//markedKeys.sort(); // remove duplicates
//markedKeys.unique();
// BOOST_FOREACH(const Index var, affectedKeys) {
// markedKeys.push_back(var);
// }
toc(5,"fluid find_all");
}
tic(6,"expmap");
// 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}.
if (relinAny) {
tic(6,"expmap");
// 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}.
if (relinAny) {
#ifndef NDEBUG
_SelectiveExpmapAndClear selectiveExpmap(delta_, ordering_, markedRelinMask);
_SelectiveExpmapAndClear selectiveExpmap(delta_, ordering_, markedRelinMask);
#else
_SelectiveExpmap selectiveExpmap(delta_, ordering_, markedRelinMask);
_SelectiveExpmap selectiveExpmap(delta_, ordering_, markedRelinMask);
#endif
theta_.apply(selectiveExpmap);
// theta_ = theta_.expmap(deltaMarked);
}
toc(6,"expmap");
theta_.apply(selectiveExpmap);
}
toc(6,"expmap");
#ifndef NDEBUG
lastRelinVariables_ = markedRelinMask;
lastRelinVariables_ = markedRelinMask;
#endif
} else {
#ifndef NDEBUG
lastRelinVariables_ = vector<bool>(ordering_.nVars(), false);
#endif
}
tic(7,"linearize new");
tic(1,"linearize");
@ -800,19 +790,6 @@ void ISAM2<Conditional, Values>::update(
delta_.permutation() = Permutation::Identity(delta_.size());
delta_.container() = newDelta;
lastBacksubVariableCount = theta_.size();
//#ifndef NDEBUG
// FactorGraph<JacobianFactor> linearfullJ = *nonlinearFactors_.linearize(theta_, ordering_);
// VectorValues deltafullJ = optimize(*GenericSequentialSolver<JacobianFactor>(linearfullJ).eliminate());
// FactorGraph<HessianFactor> linearfullH =
// *nonlinearFactors_.linearize(theta_, ordering_)->template convertCastFactors<FactorGraph<HessianFactor> >();
// VectorValues deltafullH = optimize(*GenericSequentialSolver<HessianFactor>(linearfullH).eliminate());
// if(!assert_equal(deltafullJ, newDelta, 1e-2))
// throw runtime_error("iSAM2 does not agree with full Jacobian solver");
// if(!assert_equal(deltafullH, newDelta, 1e-2))
// throw runtime_error("iSAM2 does not agree with full Hessian solver");
//#endif
} else {
vector<bool> replacedKeysMask(variableIndex_.size(), false);
if(replacedKeys) {

53
tests/testGaussianISAM2.cpp
View File

@ -92,6 +92,59 @@ TEST(ISAM2, AddVariables) {
EXPECT(assert_equal(orderingExpected, ordering));
}
/* ************************************************************************* */
//TEST(ISAM2, IndicesFromFactors) {
//
// using namespace gtsam::planarSLAM;
// typedef GaussianISAM2<planarSLAM::Values>::Impl Impl;
//
// Ordering ordering; ordering += PointKey(0), PoseKey(0), PoseKey(1);
// planarSLAM::Graph graph;
// graph.addPrior(PoseKey(0), Pose2(), sharedUnit(Pose2::dimension));
// graph.addRange(PoseKey(0), PointKey(0), 1.0, sharedUnit(1));
//
// FastSet<Index> expected;
// expected.insert(0);
// expected.insert(1);
//
// FastSet<Index> actual = Impl::IndicesFromFactors(ordering, graph);
//
// EXPECT(assert_equal(expected, actual));
//}
/* ************************************************************************* */
//TEST(ISAM2, CheckRelinearization) {
//
// typedef GaussianISAM2<planarSLAM::Values>::Impl Impl;
//
// // Create values where indices 1 and 3 are above the threshold of 0.1
// VectorValues values;
// values.reserve(4, 10);
// values.push_back_preallocated(Vector_(2, 0.09, 0.09));
// values.push_back_preallocated(Vector_(3, 0.11, 0.11, 0.09));
// values.push_back_preallocated(Vector_(3, 0.09, 0.09, 0.09));
// values.push_back_preallocated(Vector_(2, 0.11, 0.11));
//
// // Create a permutation
// Permutation permutation(4);
// permutation[0] = 2;
// permutation[1] = 0;
// permutation[2] = 1;
// permutation[3] = 3;
//
// Permuted<VectorValues> permuted(permutation, values);
//
// // After permutation, the indices above the threshold are 2 and 2
// FastSet<Index> expected;
// expected.insert(2);
// expected.insert(3);
//
// // Indices checked by CheckRelinearization
// FastSet<Index> actual = Impl::CheckRelinearization(permuted, 0.1);
//
// EXPECT(assert_equal(expected, actual));
//}
/* ************************************************************************* */
TEST(ISAM2, optimize2) {