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

ISAM2 parameters struct instead of function arguments and hard-coded values, also documentation improvement and refactored IndicesFromFactors

release/4.3a0
Richard Roberts 2011-09-01 21:53:57 +00:00
parent 472c51b9b1
commit 06fd0335cc
5 changed files with 95 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/nonlinear/GaussianISAM2.h
View File

@ -17,6 +17,7 @@
namespace gtsam {
/**
* @ingroup ISAM2
* @brief The main ISAM2 class that is exposed to gtsam users, see ISAM2 for usage.
*
* This is a thin wrapper around an ISAM2 class templated on
@ -27,7 +28,14 @@ namespace gtsam {
* variables.
*/
template <class VALUES>
class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES> {};
class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES> {
public:
/** Create an empty ISAM2 instance */
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES>(params) {}
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
GaussianISAM2() : ISAM2<GaussianConditional, VALUES>() {}
};
// optimize the BayesTree, starting from the root
void optimize2(const BayesTree<GaussianConditional>::sharedClique& root, VectorValues& delta);

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

@ -4,10 +4,21 @@
* @author Michael Kaess, Richard Roberts
*/
#include <gtsam/base/FastSet.h>
#include <boost/foreach.hpp>
#include <vector>
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);
};
/* ************************************************************************* */
template <class UNIQUE>
struct _VariableAdder {
Ordering& ordering;
Permuted<VectorValues>& vconfig;
@ -36,7 +47,7 @@ void ISAM2<CONDITIONAL,VALUES>::Impl::AddVariables(
delta.container().reserve(delta->size() + newTheta.size(), delta->dim() + accumulate(dims.begin(), dims.end(), 0));
delta.permutation().resize(delta->size() + newTheta.size());
{
_VariableAdder<ISAM2<CONDITIONAL,VALUES> > vadder(ordering, delta);
_VariableAdder vadder(ordering, delta);
newTheta.apply(vadder);
assert(delta.permutation().size() == delta.container().size());
assert(delta.container().dim() == delta.container().dimCapacity());
@ -47,4 +58,16 @@ void ISAM2<CONDITIONAL,VALUES>::Impl::AddVariables(
nodes.resize(ordering.nVars());
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES>
FastSet<Index> ISAM2<CONDITIONAL,VALUES>::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph<VALUES>& factors) {
FastSet<Index> indices;
BOOST_FOREACH(const typename NonlinearFactor<VALUES>::shared_ptr& factor, factors) {
BOOST_FOREACH(const Symbol& key, factor->keys()) {
indices.insert(ordering[key]);
}
}
return indices;
}
}

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

@ -36,9 +36,15 @@ static const double batchThreshold = 0.65;
static const bool latestLast = true;
static const bool structuralLast = false;
/** Create an empty Bayes Tree */
/* ************************************************************************* */
template<class Conditional, class Values>
ISAM2<Conditional, Values>::ISAM2() : BayesTree<Conditional>(), delta_(Permutation(), deltaUnpermuted_) {}
ISAM2<Conditional, Values>::ISAM2(const ISAM2Params& params):
delta_(Permutation(), deltaUnpermuted_), params_(params) {}
/* ************************************************************************* */
template<class Conditional, class Values>
ISAM2<Conditional, Values>::ISAM2():
delta_(Permutation(), deltaUnpermuted_) {}
/** Create a Bayes Tree from a nonlinear factor graph */
//template<class Conditional, class Values>
@ -670,12 +676,10 @@ struct _SelectiveExpmapAndClear {
/* ************************************************************************* */
template<class Conditional, class Values>
void ISAM2<Conditional, Values>::update(
const NonlinearFactorGraph<Values>& newFactors, const Values& newTheta,
double wildfire_threshold, double relinearize_threshold, bool relinearize, bool force_relinearize) {
const NonlinearFactorGraph<Values>& newFactors, const Values& newTheta, bool force_relinearize) {
static const bool debug = ISDEBUG("ISAM2 update");
static const bool verbose = ISDEBUG("ISAM2 update verbose");
if(disableReordering) { wildfire_threshold = -1.0; relinearize_threshold = 0.0; }
static int count = 0;
count++;
@ -705,30 +709,23 @@ void ISAM2<Conditional, Values>::update(
tic(3,"gather involved keys");
// 3. Mark linear update
FastSet<Index> markedKeys;
FastSet<Index> markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
vector<Index> newKeys; newKeys.reserve(markedKeys.size());
newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them
FastSet<Index> structuralKeys;
vector<Index> newKeys; newKeys.reserve(newFactors.size() * 6);
BOOST_FOREACH(const typename NonlinearFactor<Values>::shared_ptr& factor, newFactors) {
BOOST_FOREACH(const Symbol& key, factor->keys()) {
Index var = ordering_[key];
markedKeys.insert(var);
if(structuralLast) structuralKeys.insert(var);
newKeys.push_back(var);
}
}
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 || (relinearize && count%1 == 0)) { // todo: every n steps
if (force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0)) { // todo: every n steps
tic(4,"gather relinearize keys");
// 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 >= relinearize_threshold) {
if(maxDelta >= params_.relinearizeThreshold || disableReordering) {
markedRelinMask[var] = true;
markedKeys.insert(var);
if(!relinAny) relinAny = true;
@ -795,7 +792,7 @@ void ISAM2<Conditional, Values>::update(
tic(9,"solve");
// 9. Solve
if (wildfire_threshold<=0.) {
if (params_.wildfireThreshold <= 0.0 || disableReordering) {
VectorValues newDelta(theta_.dims(ordering_));
optimize2(this->root(), newDelta);
if(debug) newDelta.print("newDelta: ");
@ -821,7 +818,7 @@ void ISAM2<Conditional, Values>::update(
if(replacedKeys) {
BOOST_FOREACH(const Index var, *replacedKeys) {
replacedKeysMask[var] = true; } }
lastBacksubVariableCount = optimize2(this->root(), wildfire_threshold, replacedKeysMask, delta_); // modifies delta_
lastBacksubVariableCount = optimize2(this->root(), params_.wildfireThreshold, replacedKeysMask, delta_); // modifies delta_
#ifndef NDEBUG
for(size_t j=0; j<delta_.container().size(); ++j)

50
gtsam/nonlinear/ISAM2.h
View File

@ -30,10 +30,35 @@
namespace gtsam {
/**
* @defgroup ISAM2
*/
/**
* @ingroup ISAM2
* Parameters for the ISAM2 algorithm. Default parameter values are listed below.
*/
struct ISAM2Params {
double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 0.001)
double relinearizeThreshold; ///< Only relinearize variables whose linear delta magnitude is greater than this threshold (default: 0.1)
int relinearizeSkip; ///< Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10)
bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize any variables (default: true)
/** Specify parameters as constructor arguments */
ISAM2Params(
double _wildfireThreshold = 0.001, ///< ISAM2Params::wildfireThreshold
double _relinearizeThreshold = 0.1, ///< ISAM2Params::relinearizeThreshold
int _relinearizeSkip = 10, ///< ISAM2Params::relinearizeSkip
bool _enableRelinearization = true ///< ISAM2Params::enableRelinearization
) : wildfireThreshold(_wildfireThreshold), relinearizeThreshold(_relinearizeThreshold),
relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization) {}
};
/**
* @ingroup ISAM2
* Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
*
* The typical cycle of using this class to create an instance using the default
* constructor, then add measurements and variables as they arrive using the update()
* The typical cycle of using this class to create an instance by providing ISAM2Params
* to the constructor, then add measurements and variables as they arrive using the update()
* method. At any time, calculateEstimate() may be called to obtain the current
* estimate of all variables.
*/
@ -69,6 +94,9 @@ protected:
*/
Ordering ordering_;
/** The current parameters */
ISAM2Params params_;
private:
#ifndef NDEBUG
std::vector<bool> lastRelinVariables_;
@ -82,6 +110,9 @@ public:
typedef ISAM2<CONDITIONAL, VALUES> This; ///< This class
/** Create an empty ISAM2 instance */
ISAM2(const ISAM2Params& params);
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
ISAM2();
typedef typename BayesTree<CONDITIONAL>::sharedClique sharedClique; ///< Shared pointer to a clique
@ -100,17 +131,14 @@ public:
* You must include here all new variables occuring in newFactors (which were not already
* in the system). There must not be any variables here that do not occur in newFactors,
* and additionally, variables that were already in the system must not be included here.
* @param wildfire_threshold The threshold below which the linear solution delta is not
* updated.
* @param relinearize_threshold The threshold on the linear delta below which a variable
* will not be relinearized.
* @param relinearize
* @param force_relinearize Relinearize any variables whose delta magnitude is sufficiently
* large (Params::relinearizeThreshold), regardless of the relinearization interval
* (Params::relinearizeSkip).
*/
void update(const NonlinearFactorGraph<VALUES>& newFactors, const VALUES& newTheta,
double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true,
bool force_relinearize = false);
/** Access the current linearization point
/** Access the current linearization point */
const VALUES& getLinearizationPoint() const {return theta_;}
/** Compute an estimate from the incomplete linear delta computed during the last update.
@ -122,9 +150,7 @@ public:
//@{
/** Internal implementation functions */
struct Impl {
static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
};
struct Impl;
/** Compute an estimate using a complete delta computed by a full back-substitution.
*/

13
tests/testGaussianISAM2.cpp
View File

@ -149,12 +149,11 @@ TEST(ISAM2, slamlike_solution)
typedef planarSLAM::PointKey PointKey;
// Set up parameters
double wildfire = 0.001;
SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0));
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
// These variables will be reused and accumulate factors and values
GaussianISAM2<planarSLAM::Values> isam;
GaussianISAM2<planarSLAM::Values> isam(ISAM2Params(0.001, 0.0, 0, false));
planarSLAM::Values fullinit;
planarSLAM::Graph fullgraph;
@ -171,7 +170,7 @@ TEST(ISAM2, slamlike_solution)
init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01));
isam.update(newfactors, init, wildfire, 0.0, false);
isam.update(newfactors, init);
}
EXPECT(isam_check(fullgraph, fullinit, isam));
@ -186,7 +185,7 @@ TEST(ISAM2, slamlike_solution)
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init, wildfire, 0.0, false);
isam.update(newfactors, init);
}
// Add odometry from time 5 to 6 and landmark measurement at time 5
@ -205,7 +204,7 @@ TEST(ISAM2, slamlike_solution)
fullinit.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init, wildfire, 0.0, false);
isam.update(newfactors, init);
++ i;
}
@ -219,7 +218,7 @@ TEST(ISAM2, slamlike_solution)
init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init, wildfire, 0.0, false);
isam.update(newfactors, init);
}
// Add odometry from time 10 to 11 and landmark measurement at time 10
@ -234,7 +233,7 @@ TEST(ISAM2, slamlike_solution)
init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01));
isam.update(newfactors, init, wildfire, 0.0, false);
isam.update(newfactors, init);
++ i;
}