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

remove VALUES in other classes. New testNonlinearFactor passed.

release/4.3a0
Duy-Nguyen Ta 2012-01-27 23:00:29 +00:00
parent 79a3855316
commit ee0b7f8a74
10 changed files with 352 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/nonlinear/DynamicValues.h
View File

@ -213,6 +213,23 @@ namespace gtsam {
/** Create an array of variable dimensions using the given ordering */ /** Create an array of variable dimensions using the given ordering */
std::vector<size_t> dims(const Ordering& ordering) const; std::vector<size_t> dims(const Ordering& ordering) const;
/**
* Apply a class with an application operator() to a const_iterator over
* every <key,value> pair. The operator must be able to handle such an
* iterator for every type in the Values, (i.e. through templating).
*/
template<typename A>
void apply(A& operation) {
for(iterator it = begin(); it != end(); ++it)
operation(it);
}
template<typename A>
void apply(A& operation) const {
for(const_iterator it = begin(); it != end(); ++it)
operation(it);
}
/** /**
* Generate a default ordering, simply in key sort order. To instead * Generate a default ordering, simply in key sort order. To instead
* create a fill-reducing ordering, use * create a fill-reducing ordering, use

10
gtsam/nonlinear/GaussianISAM2.h
View File

@ -36,15 +36,15 @@ namespace gtsam {
* variables. * variables.
* @tparam GRAPH The NonlinearFactorGraph structure to store factors. Defaults to standard NonlinearFactorGraph<VALUES> * @tparam GRAPH The NonlinearFactorGraph structure to store factors. Defaults to standard NonlinearFactorGraph<VALUES>
*/ */
template <class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> > template <class GRAPH = NonlinearFactorGraph>
class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES, GRAPH> { class GaussianISAM2 : public ISAM2<GaussianConditional, GRAPH> {
typedef ISAM2<GaussianConditional, VALUES, GRAPH> Base; typedef ISAM2<GaussianConditional, GRAPH> Base;
public: public:
/** Create an empty ISAM2 instance */ /** Create an empty ISAM2 instance */
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES, GRAPH>(params) {} GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, GRAPH>(params) {}
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
GaussianISAM2() : ISAM2<GaussianConditional, VALUES, GRAPH>() {} GaussianISAM2() : ISAM2<GaussianConditional, GRAPH>() {}
void cloneTo(boost::shared_ptr<GaussianISAM2>& newGaussianISAM2) const { void cloneTo(boost::shared_ptr<GaussianISAM2>& newGaussianISAM2) const {
boost::shared_ptr<Base> isam2 = boost::static_pointer_cast<Base>(newGaussianISAM2); boost::shared_ptr<Base> isam2 = boost::static_pointer_cast<Base>(newGaussianISAM2);

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

@ -19,10 +19,10 @@ namespace gtsam {
using namespace std; using namespace std;
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl { struct ISAM2<CONDITIONAL, GRAPH>::Impl {
typedef ISAM2<CONDITIONAL, VALUES, GRAPH> ISAM2Type; typedef ISAM2<CONDITIONAL, GRAPH> ISAM2Type;
struct PartialSolveResult { struct PartialSolveResult {
typename ISAM2Type::sharedClique bayesTree; typename ISAM2Type::sharedClique bayesTree;
@ -45,7 +45,7 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
* @param ordering Current ordering to be augmented with new variables * @param ordering Current ordering to be augmented with new variables
* @param nodes Current BayesTree::Nodes index to be augmented with slots for 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); static void AddVariables(const DynamicValues& newTheta, DynamicValues& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
/** /**
* Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol
@ -95,7 +95,7 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
* where we might expmap something twice, or expmap it but then not * where we might expmap something twice, or expmap it but then not
* recalculate its delta. * recalculate its delta.
*/ */
static void ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta, static void ExpmapMasked(DynamicValues& values, const Permuted<VectorValues>& delta,
const Ordering& ordering, const std::vector<bool>& mask, const Ordering& ordering, const std::vector<bool>& mask,
boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>()); boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>());
@ -134,9 +134,9 @@ struct _VariableAdder {
}; };
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables( void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) { const DynamicValues& newTheta, DynamicValues& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
const bool debug = ISDEBUG("ISAM2 AddVariables"); const bool debug = ISDEBUG("ISAM2 AddVariables");
theta.insert(newTheta); theta.insert(newTheta);
@ -162,10 +162,10 @@ void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
FastSet<Index> indices; FastSet<Index> indices;
BOOST_FOREACH(const typename NonlinearFactor<VALUES>::shared_ptr& factor, factors) { BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) {
BOOST_FOREACH(const Symbol& key, factor->keys()) { BOOST_FOREACH(const Symbol& key, factor->keys()) {
indices.insert(ordering[key]); indices.insert(ordering[key]);
} }
@ -174,8 +174,8 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const O
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
FastSet<Index> relinKeys; FastSet<Index> relinKeys;
if(relinearizeThreshold.type() == typeid(double)) { if(relinearizeThreshold.type() == typeid(double)) {
@ -202,8 +202,8 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(const
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) { void ISAM2<CONDITIONAL,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
static const bool debug = false; static const bool debug = false;
// does the separator contain any of the variables? // does the separator contain any of the variables?
bool found = false; bool found = false;
@ -256,8 +256,8 @@ struct _SelectiveExpmapAndClear {
}; };
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta, void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(DynamicValues& values, const Permuted<VectorValues>& delta,
const Ordering& ordering, const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug) { const Ordering& ordering, const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug) {
// If debugging, invalidate if requested, otherwise do not invalidate. // If debugging, invalidate if requested, otherwise do not invalidate.
// Invalidating means setting expmapped entries to Inf, to trigger assertions // Invalidating means setting expmapped entries to Inf, to trigger assertions
@ -271,9 +271,9 @@ void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::ExpmapMasked(VALUES& values, const
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
typename ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolveResult typename ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolveResult
ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors, ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
const FastSet<Index>& keys, const ReorderingMode& reorderingMode) { const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
static const bool debug = ISDEBUG("ISAM2 recalculate"); static const bool debug = ISDEBUG("ISAM2 recalculate");

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

@ -39,8 +39,8 @@ static const bool disableReordering = false;
static const double batchThreshold = 0.65; static const double batchThreshold = 0.65;
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(const ISAM2Params& params): ISAM2<CONDITIONAL, GRAPH>::ISAM2(const ISAM2Params& params):
delta_(Permutation(), deltaUnpermuted_), params_(params) { delta_(Permutation(), deltaUnpermuted_), params_(params) {
// See note in gtsam/base/boost_variant_with_workaround.h // See note in gtsam/base/boost_variant_with_workaround.h
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
@ -48,8 +48,8 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(const ISAM2Params& params):
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(): ISAM2<CONDITIONAL, GRAPH>::ISAM2():
delta_(Permutation(), deltaUnpermuted_) { delta_(Permutation(), deltaUnpermuted_) {
// See note in gtsam/base/boost_variant_with_workaround.h // See note in gtsam/base/boost_variant_with_workaround.h
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
@ -57,14 +57,14 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2():
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
FastList<size_t> ISAM2<CONDITIONAL, VALUES, GRAPH>::getAffectedFactors(const FastList<Index>& keys) const { FastList<size_t> ISAM2<CONDITIONAL, GRAPH>::getAffectedFactors(const FastList<Index>& keys) const {
static const bool debug = false; static const bool debug = false;
if(debug) cout << "Getting affected factors for "; if(debug) cout << "Getting affected factors for ";
if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } } if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } }
if(debug) cout << endl; if(debug) cout << endl;
FactorGraph<NonlinearFactor<VALUES> > allAffected; FactorGraph<NonlinearFactor > allAffected;
FastList<size_t> indices; FastList<size_t> indices;
BOOST_FOREACH(const Index key, keys) { BOOST_FOREACH(const Index key, keys) {
// const list<size_t> l = nonlinearFactors_.factors(key); // const list<size_t> l = nonlinearFactors_.factors(key);
@ -86,9 +86,9 @@ FastList<size_t> ISAM2<CONDITIONAL, VALUES, GRAPH>::getAffectedFactors(const Fas
/* ************************************************************************* */ /* ************************************************************************* */
// retrieve all factors that ONLY contain the affected variables // retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors) // (note that the remaining stuff is summarized in the cached factors)
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
FactorGraph<GaussianFactor>::shared_ptr FactorGraph<GaussianFactor>::shared_ptr
ISAM2<CONDITIONAL, VALUES, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const { ISAM2<CONDITIONAL, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
tic(1,"getAffectedFactors"); tic(1,"getAffectedFactors");
FastList<size_t> candidates = getAffectedFactors(affectedKeys); FastList<size_t> candidates = getAffectedFactors(affectedKeys);
@ -125,9 +125,9 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::relinearizeAffectedFactors(const FastList<Ind
/* ************************************************************************* */ /* ************************************************************************* */
// find intermediate (linearized) factors from cache that are passed into the affected area // find intermediate (linearized) factors from cache that are passed into the affected area
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
FactorGraph<typename ISAM2<CONDITIONAL, VALUES, GRAPH>::CacheFactor> FactorGraph<typename ISAM2<CONDITIONAL, GRAPH>::CacheFactor>
ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) { ISAM2<CONDITIONAL, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
static const bool debug = false; static const bool debug = false;
@ -151,8 +151,8 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
return cachedBoundary; return cachedBoundary;
} }
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculate( boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors, const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
const boost::optional<FastSet<Index> >& constrainKeys, ISAM2Result& result) { const boost::optional<FastSet<Index> >& constrainKeys, ISAM2Result& result) {
@ -395,8 +395,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update( ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices, const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
const boost::optional<FastSet<Symbol> >& constrainedKeys, bool force_relinearize) { const boost::optional<FastSet<Symbol> >& constrainedKeys, bool force_relinearize) {
@ -579,36 +579,36 @@ ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate() const { DynamicValues ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
// We use ExpmapMasked here instead of regular expmap because the former // We use ExpmapMasked here instead of regular expmap because the former
// handles Permuted<VectorValues> // handles Permuted<VectorValues>
VALUES ret(theta_); DynamicValues ret(theta_);
vector<bool> mask(ordering_.nVars(), true); vector<bool> mask(ordering_.nVars(), true);
Impl::ExpmapMasked(ret, delta_, ordering_, mask); Impl::ExpmapMasked(ret, delta_, ordering_, mask);
return ret; return ret;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
template<class KEY> template<class KEY>
typename KEY::Value ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate(const KEY& key) const { typename KEY::Value ISAM2<CONDITIONAL, GRAPH>::calculateEstimate(const KEY& key) const {
const Index index = getOrdering()[key]; const Index index = getOrdering()[key];
const SubVector delta = getDelta()[index]; const SubVector delta = getDelta()[index];
return getLinearizationPoint()[key].retract(delta); return getLinearizationPoint()[key].retract(delta);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateBestEstimate() const { DynamicValues ISAM2<CONDITIONAL, GRAPH>::calculateBestEstimate() const {
VectorValues delta(theta_.dims(ordering_)); VectorValues delta(theta_.dims(ordering_));
optimize2(this->root(), delta); optimize2(this->root(), delta);
return theta_.retract(delta, ordering_); return theta_.retract(delta, ordering_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam) { VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam) {
VectorValues delta = *allocateVectorValues(isam); VectorValues delta = *allocateVectorValues(isam);
optimize2(isam.root(), delta); optimize2(isam.root(), delta);
return delta; return delta;

20
gtsam/nonlinear/ISAM2.h
View File

@ -265,13 +265,13 @@ private:
* estimate of all variables. * estimate of all variables.
* *
*/ */
template<class CONDITIONAL, class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> > template<class CONDITIONAL, class GRAPH = NonlinearFactorGraph>
class ISAM2: public BayesTree<CONDITIONAL, ISAM2Clique<CONDITIONAL> > { class ISAM2: public BayesTree<CONDITIONAL, ISAM2Clique<CONDITIONAL> > {
protected: protected:
/** The current linearization point */ /** The current linearization point */
VALUES theta_; DynamicValues theta_;
/** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */ /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */
VariableIndex variableIndex_; VariableIndex variableIndex_;
@ -313,8 +313,8 @@ private:
public: public:
typedef BayesTree<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class typedef BayesTree<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
typedef ISAM2<CONDITIONAL, VALUES> This; ///< This class typedef ISAM2<CONDITIONAL> This; ///< This class
typedef VALUES Values; typedef DynamicValues Values;
typedef GRAPH Graph; typedef GRAPH Graph;
/** Create an empty ISAM2 instance */ /** Create an empty ISAM2 instance */
@ -367,19 +367,19 @@ public:
* (Params::relinearizeSkip). * (Params::relinearizeSkip).
* @return An ISAM2Result struct containing information about the update * @return An ISAM2Result struct containing information about the update
*/ */
ISAM2Result update(const GRAPH& newFactors = GRAPH(), const VALUES& newTheta = VALUES(), ISAM2Result update(const GRAPH& newFactors = GRAPH(), const DynamicValues& newTheta = DynamicValues(),
const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(), const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
const boost::optional<FastSet<Symbol> >& constrainedKeys = boost::none, const boost::optional<FastSet<Symbol> >& constrainedKeys = boost::none,
bool force_relinearize = false); bool force_relinearize = false);
/** Access the current linearization point */ /** Access the current linearization point */
const VALUES& getLinearizationPoint() const {return theta_;} const DynamicValues& getLinearizationPoint() const {return theta_;}
/** Compute an estimate from the incomplete linear delta computed during the last update. /** Compute an estimate from the incomplete linear delta computed during the last update.
* This delta is incomplete because it was not updated below wildfire_threshold. If only * This delta is incomplete because it was not updated below wildfire_threshold. If only
* a single variable is needed, it is faster to call calculateEstimate(const KEY&). * a single variable is needed, it is faster to call calculateEstimate(const KEY&).
*/ */
VALUES calculateEstimate() const; DynamicValues calculateEstimate() const;
/** Compute an estimate for a single variable using its incomplete linear delta computed /** Compute an estimate for a single variable using its incomplete linear delta computed
* during the last update. This is faster than calling the no-argument version of * during the last update. This is faster than calling the no-argument version of
@ -398,7 +398,7 @@ public:
/** Compute an estimate using a complete delta computed by a full back-substitution. /** Compute an estimate using a complete delta computed by a full back-substitution.
*/ */
VALUES calculateBestEstimate() const; DynamicValues calculateBestEstimate() const;
/** Access the current delta, computed during the last call to update */ /** Access the current delta, computed during the last call to update */
const Permuted<VectorValues>& getDelta() const { return delta_; } const Permuted<VectorValues>& getDelta() const { return delta_; }
@ -434,8 +434,8 @@ private:
}; // ISAM2 }; // ISAM2
/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ /** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
template<class CONDITIONAL, class VALUES, class GRAPH> template<class CONDITIONAL, class GRAPH>
VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam); VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam);
} /// namespace gtsam } /// namespace gtsam

2
gtsam/nonlinear/Makefile.am
View File

@ -18,7 +18,7 @@ check_PROGRAMS =
# Lie Groups # Lie Groups
headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h DynamicValues-inl.h headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h DynamicValues-inl.h
sources += DynamicValues.cpp sources += DynamicValues.cpp
check_PROGRAMS += tests/testValues tests/testDynamicValues tests/testKey tests/testOrdering check_PROGRAMS += tests/testValues tests/testDynamicValues tests/testKey tests/testOrdering tests/testNonlinearFactor
# Nonlinear nonlinear # Nonlinear nonlinear
headers += Key.h headers += Key.h

91
gtsam/nonlinear/NonlinearFactor.h
View File

@ -31,6 +31,7 @@
#include <gtsam/linear/SharedNoiseModel.h> #include <gtsam/linear/SharedNoiseModel.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/DynamicValues.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
namespace gtsam { namespace gtsam {
@ -56,18 +57,17 @@ inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vec
* more general than just vectors, e.g., Rot3 or Pose3, * more general than just vectors, e.g., Rot3 or Pose3,
* which are objects in non-linear manifolds (Lie groups). * which are objects in non-linear manifolds (Lie groups).
*/ */
template<class VALUES>
class NonlinearFactor: public Factor<Symbol> { class NonlinearFactor: public Factor<Symbol> {
protected: protected:
// Some handy typedefs // Some handy typedefs
typedef Factor<Symbol> Base; typedef Factor<Symbol> Base;
typedef NonlinearFactor<VALUES> This; typedef NonlinearFactor This;
public: public:
typedef boost::shared_ptr<NonlinearFactor<VALUES> > shared_ptr; typedef boost::shared_ptr<NonlinearFactor > shared_ptr;
/** Default constructor for I/O only */ /** Default constructor for I/O only */
NonlinearFactor() { NonlinearFactor() {
@ -107,7 +107,7 @@ public:
* This is typically equal to log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian. * This is typically equal to log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian.
* You can override this for systems with unusual noise models. * You can override this for systems with unusual noise models.
*/ */
virtual double error(const VALUES& c) const = 0; virtual double error(const DynamicValues& c) const = 0;
/** get the dimension of the factor (number of rows on linearization) */ /** get the dimension of the factor (number of rows on linearization) */
virtual size_t dim() const = 0; virtual size_t dim() const = 0;
@ -122,11 +122,11 @@ public:
* when the constraint is *NOT* fulfilled. * when the constraint is *NOT* fulfilled.
* @return true if the constraint is active * @return true if the constraint is active
*/ */
virtual bool active(const VALUES& c) const { return true; } virtual bool active(const DynamicValues& c) const { return true; }
/** linearize to a GaussianFactor */ /** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactor> virtual boost::shared_ptr<GaussianFactor>
linearize(const VALUES& c, const Ordering& ordering) const = 0; linearize(const DynamicValues& c, const Ordering& ordering) const = 0;
/** /**
* Create a symbolic factor using the given ordering to determine the * Create a symbolic factor using the given ordering to determine the
@ -152,20 +152,19 @@ public:
* The noise model is typically Gaussian, but robust and constrained error models are also supported. * The noise model is typically Gaussian, but robust and constrained error models are also supported.
*/ */
template<class VALUES> class NoiseModelFactor: public NonlinearFactor {
class NoiseModelFactor: public NonlinearFactor<VALUES> {
protected: protected:
// handy typedefs // handy typedefs
typedef NonlinearFactor<VALUES> Base; typedef NonlinearFactor Base;
typedef NoiseModelFactor<VALUES> This; typedef NoiseModelFactor This;
SharedNoiseModel noiseModel_; /** Noise model */ SharedNoiseModel noiseModel_; /** Noise model */
public: public:
typedef boost::shared_ptr<NoiseModelFactor<VALUES> > shared_ptr; typedef boost::shared_ptr<NoiseModelFactor > shared_ptr;
/** Default constructor for I/O only */ /** Default constructor for I/O only */
NoiseModelFactor() { NoiseModelFactor() {
@ -212,7 +211,7 @@ public:
} }
/** Check if two factors are equal */ /** Check if two factors are equal */
virtual bool equals(const NoiseModelFactor<VALUES>& f, double tol = 1e-9) const { virtual bool equals(const NoiseModelFactor& f, double tol = 1e-9) const {
return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol); return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol);
} }
@ -232,13 +231,13 @@ public:
* If any of the optional Matrix reference arguments are specified, it should compute * If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3...). * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
*/ */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0; virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
/** /**
* Vector of errors, whitened * Vector of errors, whitened
* This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian * This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian
*/ */
Vector whitenedError(const VALUES& c) const { Vector whitenedError(const DynamicValues& c) const {
return noiseModel_->whiten(unwhitenedError(c)); return noiseModel_->whiten(unwhitenedError(c));
} }
@ -248,7 +247,7 @@ public:
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/ */
virtual double error(const VALUES& c) const { virtual double error(const DynamicValues& c) const {
if (this->active(c)) if (this->active(c))
return 0.5 * noiseModel_->distance(unwhitenedError(c)); return 0.5 * noiseModel_->distance(unwhitenedError(c));
else else
@ -260,7 +259,7 @@ public:
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/ */
boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const { boost::shared_ptr<GaussianFactor> linearize(const DynamicValues& x, const Ordering& ordering) const {
// Only linearize if the factor is active // Only linearize if the factor is active
if (!this->active(x)) if (!this->active(x))
return boost::shared_ptr<JacobianFactor>(); return boost::shared_ptr<JacobianFactor>();
@ -308,8 +307,8 @@ private:
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 1 /** A convenient base class for creating your own NoiseModelFactor with 1
* variable. To derive from this class, implement evaluateError(). */ * variable. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY> template<class KEY>
class NonlinearFactor1: public NoiseModelFactor<VALUES> { class NonlinearFactor1: public NoiseModelFactor {
public: public:
@ -321,8 +320,8 @@ protected:
// The value of the key. Not const to allow serialization // The value of the key. Not const to allow serialization
KEY key_; KEY key_;
typedef NoiseModelFactor<VALUES> Base; typedef NoiseModelFactor Base;
typedef NonlinearFactor1<VALUES, KEY> This; typedef NonlinearFactor1<KEY> This;
public: public:
@ -344,7 +343,7 @@ public:
/** Calls the 1-key specific version of evaluateError, which is pure virtual /** Calls the 1-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
const X& x1 = x[key_]; const X& x1 = x[key_];
if(H) { if(H) {
@ -387,8 +386,8 @@ private:
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 2 /** A convenient base class for creating your own NoiseModelFactor with 2
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2> template<class KEY1, class KEY2>
class NonlinearFactor2: public NoiseModelFactor<VALUES> { class NonlinearFactor2: public NoiseModelFactor {
public: public:
@ -402,8 +401,8 @@ protected:
KEY1 key1_; KEY1 key1_;
KEY2 key2_; KEY2 key2_;
typedef NoiseModelFactor<VALUES> Base; typedef NoiseModelFactor Base;
typedef NonlinearFactor2<VALUES, KEY1, KEY2> This; typedef NonlinearFactor2<KEY1, KEY2> This;
public: public:
@ -428,7 +427,7 @@ public:
/** Calls the 2-key specific version of evaluateError, which is pure virtual /** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
const X1& x1 = x[key1_]; const X1& x1 = x[key1_];
const X2& x2 = x[key2_]; const X2& x2 = x[key2_];
@ -475,8 +474,8 @@ private:
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 3 /** A convenient base class for creating your own NoiseModelFactor with 3
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3> template<class KEY1, class KEY2, class KEY3>
class NonlinearFactor3: public NoiseModelFactor<VALUES> { class NonlinearFactor3: public NoiseModelFactor {
public: public:
@ -492,8 +491,8 @@ protected:
KEY2 key2_; KEY2 key2_;
KEY3 key3_; KEY3 key3_;
typedef NoiseModelFactor<VALUES> Base; typedef NoiseModelFactor Base;
typedef NonlinearFactor3<VALUES, KEY1, KEY2, KEY3> This; typedef NonlinearFactor3<KEY1, KEY2, KEY3> This;
public: public:
@ -520,7 +519,7 @@ public:
/** Calls the 3-key specific version of evaluateError, which is pure virtual /** Calls the 3-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]); return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]);
@ -569,8 +568,8 @@ private:
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 4 /** A convenient base class for creating your own NoiseModelFactor with 4
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4> template<class KEY1, class KEY2, class KEY3, class KEY4>
class NonlinearFactor4: public NoiseModelFactor<VALUES> { class NonlinearFactor4: public NoiseModelFactor {
public: public:
@ -588,8 +587,8 @@ protected:
KEY3 key3_; KEY3 key3_;
KEY4 key4_; KEY4 key4_;
typedef NoiseModelFactor<VALUES> Base; typedef NoiseModelFactor Base;
typedef NonlinearFactor4<VALUES, KEY1, KEY2, KEY3, KEY4> This; typedef NonlinearFactor4<KEY1, KEY2, KEY3, KEY4> This;
public: public:
@ -618,7 +617,7 @@ public:
/** Calls the 4-key specific version of evaluateError, which is pure virtual /** Calls the 4-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]); return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
@ -669,8 +668,8 @@ private:
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 5 /** A convenient base class for creating your own NoiseModelFactor with 5
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4, class KEY5> template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5>
class NonlinearFactor5: public NoiseModelFactor<VALUES> { class NonlinearFactor5: public NoiseModelFactor {
public: public:
@ -690,8 +689,8 @@ protected:
KEY4 key4_; KEY4 key4_;
KEY5 key5_; KEY5 key5_;
typedef NoiseModelFactor<VALUES> Base; typedef NoiseModelFactor Base;
typedef NonlinearFactor5<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5> This; typedef NonlinearFactor5<KEY1, KEY2, KEY3, KEY4, KEY5> This;
public: public:
@ -722,7 +721,7 @@ public:
/** Calls the 5-key specific version of evaluateError, which is pure virtual /** Calls the 5-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
@ -776,8 +775,8 @@ private:
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 6 /** A convenient base class for creating your own NoiseModelFactor with 6
* variables. To derive from this class, implement evaluateError(). */ * variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6> template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6>
class NonlinearFactor6: public NoiseModelFactor<VALUES> { class NonlinearFactor6: public NoiseModelFactor {
public: public:
@ -799,8 +798,8 @@ protected:
KEY5 key5_; KEY5 key5_;
KEY6 key6_; KEY6 key6_;
typedef NoiseModelFactor<VALUES> Base; typedef NoiseModelFactor Base;
typedef NonlinearFactor6<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This; typedef NonlinearFactor6<KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This;
public: public:
@ -833,7 +832,7 @@ public:
/** Calls the 6-key specific version of evaluateError, which is pure virtual /** Calls the 6-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */ * so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const { virtual Vector unwhitenedError(const DynamicValues& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) { if(this->active(x)) {
if(H) if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);

32
gtsam/nonlinear/NonlinearFactorGraph-inl.h
View File

@ -29,20 +29,17 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES> double NonlinearFactorGraph::probPrime(const DynamicValues& c) const {
double NonlinearFactorGraph<VALUES>::probPrime(const VALUES& c) const {
return exp(-0.5 * error(c)); return exp(-0.5 * error(c));
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES> void NonlinearFactorGraph::print(const std::string& str) const {
void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
Base::print(str); Base::print(str);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES> double NonlinearFactorGraph::error(const DynamicValues& c) const {
double NonlinearFactorGraph<VALUES>::error(const VALUES& c) const {
double total_error = 0.; double total_error = 0.;
// iterate over all the factors_ to accumulate the log probabilities // iterate over all the factors_ to accumulate the log probabilities
BOOST_FOREACH(const sharedFactor& factor, this->factors_) { BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
@ -53,8 +50,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES> std::set<Symbol> NonlinearFactorGraph::keys() const {
std::set<Symbol> NonlinearFactorGraph<VALUES>::keys() const {
std::set<Symbol> keys; std::set<Symbol> keys;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) { BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor) if(factor)
@ -64,9 +60,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES> Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
Ordering::shared_ptr NonlinearFactorGraph<VALUES>::orderingCOLAMD( const DynamicValues& config) const {
const VALUES& config) const {
// Create symbolic graph and initial (iterator) ordering // Create symbolic graph and initial (iterator) ordering
SymbolicFactorGraph::shared_ptr symbolic; SymbolicFactorGraph::shared_ptr symbolic;
@ -91,8 +86,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES> SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::symbolic(const Ordering& ordering) const {
// Generate the symbolic factor graph // Generate the symbolic factor graph
SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph); SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
symbolicfg->reserve(this->size()); symbolicfg->reserve(this->size());
@ -108,21 +102,19 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES> pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph< const DynamicValues& config) const {
VALUES>::symbolic(const VALUES& config) const {
// Generate an initial key ordering in iterator order // Generate an initial key ordering in iterator order
Ordering::shared_ptr ordering(config.orderingArbitrary()); Ordering::shared_ptr ordering(config.orderingArbitrary());
return make_pair(symbolic(*ordering), ordering); return make_pair(symbolic(*ordering), ordering);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES> GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
typename GaussianFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::linearize( const DynamicValues& config, const Ordering& ordering) const {
const VALUES& config, const Ordering& ordering) const {
// create an empty linear FG // create an empty linear FG
typename GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph); GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
linearFG->reserve(this->size()); linearFG->reserve(this->size());
// linearize all factors // linearize all factors

19
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -35,14 +35,13 @@ namespace gtsam {
* tangent vector space at the linearization point. Because the tangent space is a true * tangent vector space at the linearization point. Because the tangent space is a true
* vector space, the config type will be an VectorValues in that linearized factor graph. * vector space, the config type will be an VectorValues in that linearized factor graph.
*/ */
template<class VALUES> class NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<VALUES> > {
public: public:
typedef FactorGraph<NonlinearFactor<VALUES> > Base; typedef FactorGraph<NonlinearFactor> Base;
typedef boost::shared_ptr<NonlinearFactorGraph<VALUES> > shared_ptr; typedef boost::shared_ptr<NonlinearFactorGraph> shared_ptr;
typedef boost::shared_ptr<NonlinearFactor<VALUES> > sharedFactor; typedef boost::shared_ptr<NonlinearFactor> sharedFactor;
/** print just calls base class */ /** print just calls base class */
void print(const std::string& str = "NonlinearFactorGraph: ") const; void print(const std::string& str = "NonlinearFactorGraph: ") const;
@ -51,10 +50,10 @@ namespace gtsam {
std::set<Symbol> keys() const; std::set<Symbol> keys() const;
/** unnormalized error */ /** unnormalized error */
double error(const VALUES& c) const; double error(const DynamicValues& c) const;
/** Unnormalized probability. O(n) */ /** Unnormalized probability. O(n) */
double probPrime(const VALUES& c) const; double probPrime(const DynamicValues& c) const;
template<class F> template<class F>
void add(const F& factor) { void add(const F& factor) {
@ -73,20 +72,20 @@ namespace gtsam {
* ordering is found. * ordering is found.
*/ */
std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
symbolic(const VALUES& config) const; symbolic(const DynamicValues& config) const;
/** /**
* Compute a fill-reducing ordering using COLAMD. This returns the * Compute a fill-reducing ordering using COLAMD. This returns the
* ordering and a VariableIndex, which can later be re-used to save * ordering and a VariableIndex, which can later be re-used to save
* computation. * computation.
*/ */
Ordering::shared_ptr orderingCOLAMD(const VALUES& config) const; Ordering::shared_ptr orderingCOLAMD(const DynamicValues& config) const;
/** /**
* linearize a nonlinear factor graph * linearize a nonlinear factor graph
*/ */
boost::shared_ptr<GaussianFactorGraph > boost::shared_ptr<GaussianFactorGraph >
linearize(const VALUES& config, const Ordering& ordering) const; linearize(const DynamicValues& config, const Ordering& ordering) const;
private: private:

208
gtsam/nonlinear/tests/testNonlinearFactor.cpp Normal file
View File

@ -0,0 +1,208 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testDynamicValues.cpp
* @author Duy-Nguyen Ta
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
typedef TypedSymbol<Pose2, 'p'> PoseKey;
/* ************************************************* */
class TestFactor1: public NonlinearFactor1<PoseKey> {
typedef NonlinearFactor1<PoseKey> Base;
public:
TestFactor1() : Base(sharedSigmas(Vector_(1, 1.0)), 1) {}
virtual ~TestFactor1() {}
virtual Vector evaluateError(const Pose2& pose, boost::optional<Matrix&> H = boost::none) const {
if (H) *H = zeros(1,3);
return zero(1);
}
};
/* ************************************************************************* */
TEST(NonlinearFactor, TestFactor1) {
TestFactor1 factor1;
Vector actualError = factor1.evaluateError(Pose2());
Vector expectedError = zero(1);
CHECK(assert_equal(expectedError, actualError));
}
/* ************************************************************************* */
typedef TypedSymbol<LieVector, 'x'> TestKey;
/* ************************************************************************* */
class TestFactor4 : public NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> {
public:
typedef NonlinearFactor4<TestKey, TestKey, TestKey, TestKey> Base;
TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4) {}
virtual ~TestFactor4() {}
virtual Vector
evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none) const {
if(H1) {
*H1 = Matrix_(1,1, 1.0);
*H2 = Matrix_(1,1, 2.0);
*H3 = Matrix_(1,1, 3.0);
*H4 = Matrix_(1,1, 4.0);
}
return (Vector(1) << x1 + x2 + x3 + x4).finished();
}
};
/* ************************************ */
TEST(NonlinearFactor, NonlinearFactor4) {
TestFactor4 tf;
DynamicValues tv;
tv.insert(TestKey(1), LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0));
EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);
LONGS_EQUAL(jf.keys()[2], 2);
LONGS_EQUAL(jf.keys()[3], 3);
EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
EXPECT(assert_equal(Vector_(1, -5.0), jf.getb()));
}
/* ************************************************************************* */
class TestFactor5 : public NonlinearFactor5<TestKey, TestKey, TestKey, TestKey, TestKey> {
public:
typedef NonlinearFactor5<TestKey, TestKey, TestKey, TestKey, TestKey> Base;
TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {}
virtual ~TestFactor5() {}
virtual Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const {
if(H1) {
*H1 = Matrix_(1,1, 1.0);
*H2 = Matrix_(1,1, 2.0);
*H3 = Matrix_(1,1, 3.0);
*H4 = Matrix_(1,1, 4.0);
*H5 = Matrix_(1,1, 5.0);
}
return (Vector(1) << x1 + x2 + x3 + x4 + x5).finished();
}
};
/* ************************************ */
TEST(NonlinearFactor, NonlinearFactor5) {
TestFactor5 tf;
DynamicValues tv;
tv.insert(TestKey(1), LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0));
tv.insert(TestKey(5), LieVector(1, 5.0));
EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);
LONGS_EQUAL(jf.keys()[2], 2);
LONGS_EQUAL(jf.keys()[3], 3);
LONGS_EQUAL(jf.keys()[4], 4);
EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
EXPECT(assert_equal(Matrix_(1,1, 2.5), jf.getA(jf.begin()+4)));
EXPECT(assert_equal(Vector_(1, -7.5), jf.getb()));
}
/* ************************************************************************* */
class TestFactor6 : public NonlinearFactor6<TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> {
public:
typedef NonlinearFactor6<TestKey, TestKey, TestKey, TestKey, TestKey, TestKey> Base;
TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {}
virtual ~TestFactor6() {}
virtual Vector
evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none,
boost::optional<Matrix&> H6 = boost::none) const {
if(H1) {
*H1 = Matrix_(1,1, 1.0);
*H2 = Matrix_(1,1, 2.0);
*H3 = Matrix_(1,1, 3.0);
*H4 = Matrix_(1,1, 4.0);
*H5 = Matrix_(1,1, 5.0);
*H6 = Matrix_(1,1, 6.0);
}
return (Vector(1) << x1 + x2 + x3 + x4 + x5 + x6).finished();
}
};
/* ************************************ */
TEST(NonlinearFactor, NonlinearFactor6) {
TestFactor6 tf;
DynamicValues tv;
tv.insert(TestKey(1), LieVector(1, 1.0));
tv.insert(TestKey(2), LieVector(1, 2.0));
tv.insert(TestKey(3), LieVector(1, 3.0));
tv.insert(TestKey(4), LieVector(1, 4.0));
tv.insert(TestKey(5), LieVector(1, 5.0));
tv.insert(TestKey(6), LieVector(1, 6.0));
EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5), TestKey(6);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);
LONGS_EQUAL(jf.keys()[2], 2);
LONGS_EQUAL(jf.keys()[3], 3);
LONGS_EQUAL(jf.keys()[4], 4);
LONGS_EQUAL(jf.keys()[5], 5);
EXPECT(assert_equal(Matrix_(1,1, 0.5), jf.getA(jf.begin())));
EXPECT(assert_equal(Matrix_(1,1, 1.0), jf.getA(jf.begin()+1)));
EXPECT(assert_equal(Matrix_(1,1, 1.5), jf.getA(jf.begin()+2)));
EXPECT(assert_equal(Matrix_(1,1, 2.0), jf.getA(jf.begin()+3)));
EXPECT(assert_equal(Matrix_(1,1, 2.5), jf.getA(jf.begin()+4)));
EXPECT(assert_equal(Matrix_(1,1, 3.0), jf.getA(jf.begin()+5)));
EXPECT(assert_equal(Vector_(1, -10.5), jf.getb()));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */