Added an optional template parameter to ISAM2 for a customized nonlinear factor graph
Alex Cunningham
parent
6d961844c4
commit
b5e46e9b7c
|
|
@ -26,15 +26,16 @@ namespace gtsam {
|
|||
*
|
||||
* @tparam VALUES The Values or TupleValues\Emph{N} that contains the
|
||||
* variables.
|
||||
* @tparam GRAPH The NonlinearFactorGraph structure to store factors. Defaults to standard NonlinearFactorGraph<VALUES>
|
||||
*/
|
||||
template <class VALUES>
|
||||
class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES> {
|
||||
template <class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
|
||||
class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES, GRAPH> {
|
||||
public:
|
||||
/** Create an empty ISAM2 instance */
|
||||
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES>(params) {}
|
||||
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES, GRAPH>(params) {}
|
||||
|
||||
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
|
||||
GaussianISAM2() : ISAM2<GaussianConditional, VALUES>() {}
|
||||
GaussianISAM2() : ISAM2<GaussianConditional, VALUES, GRAPH>() {}
|
||||
};
|
||||
|
||||
// optimize the BayesTree, starting from the root
|
||||
|
|
|
|||
|
|
@ -23,10 +23,10 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
template<class CONDITIONAL, class VALUES>
|
||||
struct ISAM2<CONDITIONAL, VALUES>::Impl {
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
|
||||
|
||||
typedef ISAM2<CONDITIONAL, VALUES> ISAM2Type;
|
||||
typedef ISAM2<CONDITIONAL, VALUES, GRAPH> ISAM2Type;
|
||||
|
||||
struct PartialSolveResult {
|
||||
typename ISAM2Type::sharedClique bayesTree;
|
||||
|
|
@ -58,7 +58,7 @@ struct ISAM2<CONDITIONAL, VALUES>::Impl {
|
|||
* @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);
|
||||
static FastSet<Index> IndicesFromFactors(const Ordering& ordering, const GRAPH& factors);
|
||||
|
||||
/**
|
||||
* Find the set of variables to be relinearized according to relinearizeThreshold.
|
||||
|
|
@ -138,8 +138,8 @@ struct _VariableAdder {
|
|||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES>
|
||||
void ISAM2<CONDITIONAL,VALUES>::Impl::AddVariables(
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
|
||||
const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
|
||||
const bool debug = ISDEBUG("ISAM2 AddVariables");
|
||||
|
||||
|
|
@ -166,8 +166,8 @@ void ISAM2<CONDITIONAL,VALUES>::Impl::AddVariables(
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES>
|
||||
FastSet<Index> ISAM2<CONDITIONAL,VALUES>::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph<VALUES>& factors) {
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
|
||||
FastSet<Index> indices;
|
||||
BOOST_FOREACH(const typename NonlinearFactor<VALUES>::shared_ptr& factor, factors) {
|
||||
BOOST_FOREACH(const Symbol& key, factor->keys()) {
|
||||
|
|
@ -178,8 +178,8 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES>::Impl::IndicesFromFactors(const Orderin
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES>
|
||||
FastSet<Index> ISAM2<CONDITIONAL,VALUES>::Impl::CheckRelinearization(Permuted<VectorValues>& delta, double relinearizeThreshold) {
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::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>();
|
||||
|
|
@ -191,8 +191,8 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES>::Impl::CheckRelinearization(Permuted<Ve
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Values>
|
||||
void ISAM2<Conditional, Values>::Impl::FindAll(ISAM2Type::sharedClique clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::FindAll(ISAM2Type::sharedClique clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
|
||||
static const bool debug = false;
|
||||
// does the separator contain any of the variables?
|
||||
bool found = false;
|
||||
|
|
@ -245,8 +245,8 @@ struct _SelectiveExpmapAndClear {
|
|||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES>
|
||||
void ISAM2<CONDITIONAL, VALUES>::Impl::ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta,
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta,
|
||||
const Ordering& ordering, const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug) {
|
||||
// If debugging, invalidate if requested, otherwise do not invalidate.
|
||||
// Invalidating means setting expmapped entries to Inf, to trigger assertions
|
||||
|
|
@ -260,9 +260,9 @@ void ISAM2<CONDITIONAL, VALUES>::Impl::ExpmapMasked(VALUES& values, const Permut
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES>
|
||||
typename ISAM2<CONDITIONAL, VALUES>::Impl::PartialSolveResult
|
||||
ISAM2<CONDITIONAL, VALUES>::Impl::PartialSolve(GaussianFactorGraph& factors,
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
typename ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolveResult
|
||||
ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
|
||||
const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
|
||||
|
||||
static const bool debug = ISDEBUG("ISAM2 recalculate");
|
||||
|
|
|
|||
|
|
@ -50,24 +50,24 @@ static const bool latestLast = true;
|
|||
static const bool structuralLast = false;
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Values>
|
||||
ISAM2<Conditional, Values>::ISAM2(const ISAM2Params& params):
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(const ISAM2Params& params):
|
||||
delta_(Permutation(), deltaUnpermuted_), params_(params) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Values>
|
||||
ISAM2<Conditional, Values>::ISAM2():
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2():
|
||||
delta_(Permutation(), deltaUnpermuted_) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Values>
|
||||
FastList<size_t> ISAM2<Conditional, Values>::getAffectedFactors(const FastList<Index>& keys) const {
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
FastList<size_t> ISAM2<CONDITIONAL, VALUES, GRAPH>::getAffectedFactors(const FastList<Index>& keys) const {
|
||||
static const bool debug = false;
|
||||
if(debug) cout << "Getting affected factors for ";
|
||||
if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } }
|
||||
if(debug) cout << endl;
|
||||
|
||||
FactorGraph<NonlinearFactor<Values> > allAffected;
|
||||
FactorGraph<NonlinearFactor<VALUES> > allAffected;
|
||||
FastList<size_t> indices;
|
||||
BOOST_FOREACH(const Index key, keys) {
|
||||
// const list<size_t> l = nonlinearFactors_.factors(key);
|
||||
|
|
@ -89,15 +89,15 @@ FastList<size_t> ISAM2<Conditional, Values>::getAffectedFactors(const FastList<I
|
|||
/* ************************************************************************* */
|
||||
// retrieve all factors that ONLY contain the affected variables
|
||||
// (note that the remaining stuff is summarized in the cached factors)
|
||||
template<class Conditional, class Values>
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
FactorGraph<GaussianFactor>::shared_ptr
|
||||
ISAM2<Conditional, Values>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
|
||||
ISAM2<CONDITIONAL, VALUES, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
|
||||
|
||||
tic(1,"getAffectedFactors");
|
||||
FastList<size_t> candidates = getAffectedFactors(affectedKeys);
|
||||
toc(1,"getAffectedFactors");
|
||||
|
||||
NonlinearFactorGraph<Values> nonlinearAffectedFactors;
|
||||
GRAPH nonlinearAffectedFactors;
|
||||
|
||||
tic(2,"affectedKeysSet");
|
||||
// for fast lookup below
|
||||
|
|
@ -128,9 +128,9 @@ ISAM2<Conditional, Values>::relinearizeAffectedFactors(const FastList<Index>& af
|
|||
|
||||
/* ************************************************************************* */
|
||||
// find intermediate (linearized) factors from cache that are passed into the affected area
|
||||
template<class Conditional, class Values>
|
||||
FactorGraph<typename ISAM2<Conditional, Values>::CacheFactor>
|
||||
ISAM2<Conditional, Values>::getCachedBoundaryFactors(Cliques& orphans) {
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
FactorGraph<typename ISAM2<CONDITIONAL, VALUES, GRAPH>::CacheFactor>
|
||||
ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
|
||||
|
||||
static const bool debug = false;
|
||||
|
||||
|
|
@ -140,9 +140,9 @@ ISAM2<Conditional, Values>::getCachedBoundaryFactors(Cliques& orphans) {
|
|||
// find the last variable that was eliminated
|
||||
Index key = (*orphan)->frontals().back();
|
||||
#ifndef NDEBUG
|
||||
// typename BayesNet<Conditional>::const_iterator it = orphan->end();
|
||||
// const Conditional& lastConditional = **(--it);
|
||||
// typename Conditional::const_iterator keyit = lastConditional.endParents();
|
||||
// typename BayesNet<CONDITIONAL>::const_iterator it = orphan->end();
|
||||
// const CONDITIONAL& lastCONDITIONAL = **(--it);
|
||||
// typename CONDITIONAL::const_iterator keyit = lastCONDITIONAL.endParents();
|
||||
// const Index lastKey = *(--keyit);
|
||||
// assert(key == lastKey);
|
||||
#endif
|
||||
|
|
@ -154,8 +154,8 @@ ISAM2<Conditional, Values>::getCachedBoundaryFactors(Cliques& orphans) {
|
|||
return cachedBoundary;
|
||||
}
|
||||
|
||||
template<class Conditional, class Values>
|
||||
boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculate(
|
||||
const FastSet<Index>& markedKeys, const FastSet<Index>& structuralKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors, ISAM2Result& result) {
|
||||
|
||||
// TODO: new factors are linearized twice, the newFactors passed in are not used.
|
||||
|
|
@ -174,8 +174,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
|
|||
if (counter>0) { // cannot call on empty tree
|
||||
GaussianISAM2_P::CliqueData cdata = this->getCliqueData();
|
||||
GaussianISAM2_P::CliqueStats cstats = cdata.getStats();
|
||||
maxClique = cstats.maxConditionalSize;
|
||||
avgClique = cstats.avgConditionalSize;
|
||||
maxClique = cstats.maxCONDITIONALSize;
|
||||
avgClique = cstats.avgCONDITIONALSize;
|
||||
numCliques = cdata.conditionalSizes.size();
|
||||
nnzR = calculate_nnz(this->root());
|
||||
}
|
||||
|
|
@ -292,7 +292,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
|
|||
toc(5,"eliminate");
|
||||
|
||||
tic(6,"insert");
|
||||
BayesTree<Conditional>::clear();
|
||||
BayesTree<CONDITIONAL>::clear();
|
||||
this->insert(newRoot);
|
||||
toc(6,"insert");
|
||||
|
||||
|
|
@ -418,9 +418,9 @@ boost::shared_ptr<FastSet<Index> > ISAM2<Conditional, Values>::recalculate(
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Values>
|
||||
ISAM2Result ISAM2<Conditional, Values>::update(
|
||||
const NonlinearFactorGraph<Values>& newFactors, const Values& newTheta, bool force_relinearize) {
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
|
||||
const GRAPH& newFactors, const Values& newTheta, bool force_relinearize) {
|
||||
|
||||
static const bool debug = ISDEBUG("ISAM2 update");
|
||||
static const bool verbose = ISDEBUG("ISAM2 update verbose");
|
||||
|
|
@ -554,28 +554,28 @@ ISAM2Result ISAM2<Conditional, Values>::update(
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Values>
|
||||
Values ISAM2<Conditional, Values>::calculateEstimate() const {
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate() const {
|
||||
// We use ExpmapMasked here instead of regular expmap because the former
|
||||
// handles Permuted<VectorValues>
|
||||
Values ret(theta_);
|
||||
VALUES ret(theta_);
|
||||
vector<bool> mask(ordering_.nVars(), true);
|
||||
Impl::ExpmapMasked(ret, delta_, ordering_, mask);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES>
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
template<class KEY>
|
||||
typename KEY::Value ISAM2<CONDITIONAL, VALUES>::calculateEstimate(const KEY& key) const {
|
||||
typename KEY::Value ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate(const KEY& key) const {
|
||||
const Index index = getOrdering()[key];
|
||||
const SubVector delta = getDelta()[index];
|
||||
return getLinearizationPoint()[key].retract(delta);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Values>
|
||||
Values ISAM2<Conditional, Values>::calculateBestEstimate() const {
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateBestEstimate() const {
|
||||
VectorValues delta(theta_.dims(ordering_));
|
||||
optimize2(this->root(), delta);
|
||||
return theta_.retract(delta, ordering_);
|
||||
|
|
|
|||
|
|
@ -123,8 +123,9 @@ struct ISAM2Result {
|
|||
* 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.
|
||||
*
|
||||
*/
|
||||
template<class CONDITIONAL, class VALUES>
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
|
||||
class ISAM2: public BayesTree<CONDITIONAL> {
|
||||
|
||||
protected:
|
||||
|
|
@ -148,7 +149,7 @@ protected:
|
|||
Permuted<VectorValues> delta_;
|
||||
|
||||
/** All original nonlinear factors are stored here to use during relinearization */
|
||||
NonlinearFactorGraph<VALUES> nonlinearFactors_;
|
||||
GRAPH nonlinearFactors_;
|
||||
|
||||
/** @brief The current elimination ordering Symbols to Index (integer) keys.
|
||||
*
|
||||
|
|
@ -170,6 +171,8 @@ public:
|
|||
|
||||
typedef BayesTree<CONDITIONAL> Base; ///< The BayesTree base class
|
||||
typedef ISAM2<CONDITIONAL, VALUES> This; ///< This class
|
||||
typedef VALUES Values;
|
||||
typedef GRAPH Graph;
|
||||
|
||||
/** Create an empty ISAM2 instance */
|
||||
ISAM2(const ISAM2Params& params);
|
||||
|
|
@ -198,7 +201,7 @@ public:
|
|||
* (Params::relinearizeSkip).
|
||||
* @return An ISAM2Result struct containing information about the update
|
||||
*/
|
||||
ISAM2Result update(const NonlinearFactorGraph<VALUES>& newFactors, const VALUES& newTheta,
|
||||
ISAM2Result update(const GRAPH& newFactors, const VALUES& newTheta,
|
||||
bool force_relinearize = false);
|
||||
|
||||
/** Access the current linearization point */
|
||||
|
|
@ -233,7 +236,7 @@ public:
|
|||
const Permuted<VectorValues>& getDelta() const { return delta_; }
|
||||
|
||||
/** Access the set of nonlinear factors */
|
||||
const NonlinearFactorGraph<VALUES>& getFactorsUnsafe() const { return nonlinearFactors_; }
|
||||
const GRAPH& getFactorsUnsafe() const { return nonlinearFactors_; }
|
||||
|
||||
/** Access the current ordering */
|
||||
const Ordering& getOrdering() const { return ordering_; }
|
||||
|
|
|
|||
Loading…
Reference in New Issue