remove VALUES in other classes. New testNonlinearFactor passed.
Duy-Nguyen Ta
parent
79a3855316
commit
ee0b7f8a74
|
|
@ -213,6 +213,23 @@ namespace gtsam {
|
|||
/** Create an array of variable dimensions using the given ordering */
|
||||
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
|
||||
* create a fill-reducing ordering, use
|
||||
|
|
|
|||
|
|
@ -36,15 +36,15 @@ namespace gtsam {
|
|||
* variables.
|
||||
* @tparam GRAPH The NonlinearFactorGraph structure to store factors. Defaults to standard NonlinearFactorGraph<VALUES>
|
||||
*/
|
||||
template <class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
|
||||
class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES, GRAPH> {
|
||||
typedef ISAM2<GaussianConditional, VALUES, GRAPH> Base;
|
||||
template <class GRAPH = NonlinearFactorGraph>
|
||||
class GaussianISAM2 : public ISAM2<GaussianConditional, GRAPH> {
|
||||
typedef ISAM2<GaussianConditional, GRAPH> Base;
|
||||
public:
|
||||
/** 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) */
|
||||
GaussianISAM2() : ISAM2<GaussianConditional, VALUES, GRAPH>() {}
|
||||
GaussianISAM2() : ISAM2<GaussianConditional, GRAPH>() {}
|
||||
|
||||
void cloneTo(boost::shared_ptr<GaussianISAM2>& newGaussianISAM2) const {
|
||||
boost::shared_ptr<Base> isam2 = boost::static_pointer_cast<Base>(newGaussianISAM2);
|
||||
|
|
|
|||
|
|
@ -19,10 +19,10 @@ namespace gtsam {
|
|||
|
||||
using namespace std;
|
||||
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
struct ISAM2<CONDITIONAL, GRAPH>::Impl {
|
||||
|
||||
typedef ISAM2<CONDITIONAL, VALUES, GRAPH> ISAM2Type;
|
||||
typedef ISAM2<CONDITIONAL, GRAPH> ISAM2Type;
|
||||
|
||||
struct PartialSolveResult {
|
||||
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 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
|
||||
|
|
@ -95,7 +95,7 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
|
|||
* where we might expmap something twice, or expmap it but then not
|
||||
* 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,
|
||||
boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>());
|
||||
|
||||
|
|
@ -134,9 +134,9 @@ struct _VariableAdder {
|
|||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
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) {
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
|
||||
const DynamicValues& newTheta, DynamicValues& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
|
||||
const bool debug = ISDEBUG("ISAM2 AddVariables");
|
||||
|
||||
theta.insert(newTheta);
|
||||
|
|
@ -162,10 +162,10 @@ void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
FastSet<Index> ISAM2<CONDITIONAL,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 typename NonlinearFactor::shared_ptr& factor, factors) {
|
||||
BOOST_FOREACH(const Symbol& key, factor->keys()) {
|
||||
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>
|
||||
FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
|
||||
FastSet<Index> relinKeys;
|
||||
|
||||
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>
|
||||
void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
void ISAM2<CONDITIONAL,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
|
||||
static const bool debug = false;
|
||||
// does the separator contain any of the variables?
|
||||
bool found = false;
|
||||
|
|
@ -256,8 +256,8 @@ struct _SelectiveExpmapAndClear {
|
|||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta,
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(DynamicValues& 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
|
||||
|
|
@ -271,9 +271,9 @@ void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::ExpmapMasked(VALUES& values, const
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
typename ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolveResult
|
||||
ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
typename ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolveResult
|
||||
ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
|
||||
const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
|
||||
|
||||
static const bool debug = ISDEBUG("ISAM2 recalculate");
|
||||
|
|
|
|||
|
|
@ -39,8 +39,8 @@ static const bool disableReordering = false;
|
|||
static const double batchThreshold = 0.65;
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(const ISAM2Params& params):
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
ISAM2<CONDITIONAL, GRAPH>::ISAM2(const ISAM2Params& params):
|
||||
delta_(Permutation(), deltaUnpermuted_), params_(params) {
|
||||
// See note in gtsam/base/boost_variant_with_workaround.h
|
||||
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>
|
||||
ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2():
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
ISAM2<CONDITIONAL, GRAPH>::ISAM2():
|
||||
delta_(Permutation(), deltaUnpermuted_) {
|
||||
// See note in gtsam/base/boost_variant_with_workaround.h
|
||||
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
|
||||
|
|
@ -57,14 +57,14 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2():
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
FastList<size_t> ISAM2<CONDITIONAL, VALUES, GRAPH>::getAffectedFactors(const FastList<Index>& keys) const {
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
FastList<size_t> ISAM2<CONDITIONAL, 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 > allAffected;
|
||||
FastList<size_t> indices;
|
||||
BOOST_FOREACH(const Index key, keys) {
|
||||
// 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
|
||||
// (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
|
||||
ISAM2<CONDITIONAL, VALUES, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
|
||||
ISAM2<CONDITIONAL, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
|
||||
|
||||
tic(1,"getAffectedFactors");
|
||||
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
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
FactorGraph<typename ISAM2<CONDITIONAL, VALUES, GRAPH>::CacheFactor>
|
||||
ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
FactorGraph<typename ISAM2<CONDITIONAL, GRAPH>::CacheFactor>
|
||||
ISAM2<CONDITIONAL, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
|
||||
|
||||
static const bool debug = false;
|
||||
|
||||
|
|
@ -151,8 +151,8 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
|
|||
return cachedBoundary;
|
||||
}
|
||||
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculate(
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
|
||||
const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
|
||||
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>
|
||||
ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
|
||||
const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
|
||||
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>
|
||||
VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate() const {
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
DynamicValues ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
|
||||
// We use ExpmapMasked here instead of regular expmap because the former
|
||||
// handles Permuted<VectorValues>
|
||||
VALUES ret(theta_);
|
||||
DynamicValues ret(theta_);
|
||||
vector<bool> mask(ordering_.nVars(), true);
|
||||
Impl::ExpmapMasked(ret, delta_, ordering_, mask);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
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 SubVector delta = getDelta()[index];
|
||||
return getLinearizationPoint()[key].retract(delta);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateBestEstimate() const {
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
DynamicValues ISAM2<CONDITIONAL, GRAPH>::calculateBestEstimate() const {
|
||||
VectorValues delta(theta_.dims(ordering_));
|
||||
optimize2(this->root(), delta);
|
||||
return theta_.retract(delta, ordering_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam) {
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam) {
|
||||
VectorValues delta = *allocateVectorValues(isam);
|
||||
optimize2(isam.root(), delta);
|
||||
return delta;
|
||||
|
|
|
|||
|
|
@ -265,13 +265,13 @@ private:
|
|||
* 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> > {
|
||||
|
||||
protected:
|
||||
|
||||
/** The current linearization point */
|
||||
VALUES theta_;
|
||||
DynamicValues theta_;
|
||||
|
||||
/** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */
|
||||
VariableIndex variableIndex_;
|
||||
|
|
@ -313,8 +313,8 @@ private:
|
|||
public:
|
||||
|
||||
typedef BayesTree<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
|
||||
typedef ISAM2<CONDITIONAL, VALUES> This; ///< This class
|
||||
typedef VALUES Values;
|
||||
typedef ISAM2<CONDITIONAL> This; ///< This class
|
||||
typedef DynamicValues Values;
|
||||
typedef GRAPH Graph;
|
||||
|
||||
/** Create an empty ISAM2 instance */
|
||||
|
|
@ -367,19 +367,19 @@ public:
|
|||
* (Params::relinearizeSkip).
|
||||
* @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 boost::optional<FastSet<Symbol> >& constrainedKeys = boost::none,
|
||||
bool force_relinearize = false);
|
||||
|
||||
/** 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.
|
||||
* 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&).
|
||||
*/
|
||||
VALUES calculateEstimate() const;
|
||||
DynamicValues calculateEstimate() const;
|
||||
|
||||
/** 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
|
||||
|
|
@ -398,7 +398,7 @@ public:
|
|||
|
||||
/** 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 */
|
||||
const Permuted<VectorValues>& getDelta() const { return delta_; }
|
||||
|
|
@ -434,8 +434,8 @@ private:
|
|||
}; // ISAM2
|
||||
|
||||
/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
|
||||
template<class CONDITIONAL, class VALUES, class GRAPH>
|
||||
VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam);
|
||||
template<class CONDITIONAL, class GRAPH>
|
||||
VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam);
|
||||
|
||||
} /// namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@ check_PROGRAMS =
|
|||
# Lie Groups
|
||||
headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h DynamicValues-inl.h
|
||||
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
|
||||
headers += Key.h
|
||||
|
|
|
|||
|
|
@ -31,6 +31,7 @@
|
|||
#include <gtsam/linear/SharedNoiseModel.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
|
||||
#include <gtsam/nonlinear/DynamicValues.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
|
||||
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,
|
||||
* which are objects in non-linear manifolds (Lie groups).
|
||||
*/
|
||||
template<class VALUES>
|
||||
class NonlinearFactor: public Factor<Symbol> {
|
||||
|
||||
protected:
|
||||
|
||||
// Some handy typedefs
|
||||
typedef Factor<Symbol> Base;
|
||||
typedef NonlinearFactor<VALUES> This;
|
||||
typedef NonlinearFactor This;
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<NonlinearFactor<VALUES> > shared_ptr;
|
||||
typedef boost::shared_ptr<NonlinearFactor > shared_ptr;
|
||||
|
||||
/** Default constructor for I/O only */
|
||||
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.
|
||||
* 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) */
|
||||
virtual size_t dim() const = 0;
|
||||
|
|
@ -122,11 +122,11 @@ public:
|
|||
* when the constraint is *NOT* fulfilled.
|
||||
* @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 */
|
||||
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
|
||||
|
|
@ -152,20 +152,19 @@ public:
|
|||
|
||||
* The noise model is typically Gaussian, but robust and constrained error models are also supported.
|
||||
*/
|
||||
template<class VALUES>
|
||||
class NoiseModelFactor: public NonlinearFactor<VALUES> {
|
||||
class NoiseModelFactor: public NonlinearFactor {
|
||||
|
||||
protected:
|
||||
|
||||
// handy typedefs
|
||||
typedef NonlinearFactor<VALUES> Base;
|
||||
typedef NoiseModelFactor<VALUES> This;
|
||||
typedef NonlinearFactor Base;
|
||||
typedef NoiseModelFactor This;
|
||||
|
||||
SharedNoiseModel noiseModel_; /** Noise model */
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<NoiseModelFactor<VALUES> > shared_ptr;
|
||||
typedef boost::shared_ptr<NoiseModelFactor > shared_ptr;
|
||||
|
||||
/** Default constructor for I/O only */
|
||||
NoiseModelFactor() {
|
||||
|
|
@ -212,7 +211,7 @@ public:
|
|||
}
|
||||
|
||||
/** 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);
|
||||
}
|
||||
|
||||
|
|
@ -232,13 +231,13 @@ public:
|
|||
* 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...).
|
||||
*/
|
||||
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
|
||||
* 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));
|
||||
}
|
||||
|
||||
|
|
@ -248,7 +247,7 @@ public:
|
|||
* 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.
|
||||
*/
|
||||
virtual double error(const VALUES& c) const {
|
||||
virtual double error(const DynamicValues& c) const {
|
||||
if (this->active(c))
|
||||
return 0.5 * noiseModel_->distance(unwhitenedError(c));
|
||||
else
|
||||
|
|
@ -260,7 +259,7 @@ public:
|
|||
* \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$
|
||||
*/
|
||||
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
|
||||
if (!this->active(x))
|
||||
return boost::shared_ptr<JacobianFactor>();
|
||||
|
|
@ -308,8 +307,8 @@ private:
|
|||
/* ************************************************************************* */
|
||||
/** A convenient base class for creating your own NoiseModelFactor with 1
|
||||
* variable. To derive from this class, implement evaluateError(). */
|
||||
template<class VALUES, class KEY>
|
||||
class NonlinearFactor1: public NoiseModelFactor<VALUES> {
|
||||
template<class KEY>
|
||||
class NonlinearFactor1: public NoiseModelFactor {
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -321,8 +320,8 @@ protected:
|
|||
// The value of the key. Not const to allow serialization
|
||||
KEY key_;
|
||||
|
||||
typedef NoiseModelFactor<VALUES> Base;
|
||||
typedef NonlinearFactor1<VALUES, KEY> This;
|
||||
typedef NoiseModelFactor Base;
|
||||
typedef NonlinearFactor1<KEY> This;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -344,7 +343,7 @@ public:
|
|||
|
||||
/** Calls the 1-key specific version of evaluateError, which is pure virtual
|
||||
* 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)) {
|
||||
const X& x1 = x[key_];
|
||||
if(H) {
|
||||
|
|
@ -387,8 +386,8 @@ private:
|
|||
/* ************************************************************************* */
|
||||
/** A convenient base class for creating your own NoiseModelFactor with 2
|
||||
* variables. To derive from this class, implement evaluateError(). */
|
||||
template<class VALUES, class KEY1, class KEY2>
|
||||
class NonlinearFactor2: public NoiseModelFactor<VALUES> {
|
||||
template<class KEY1, class KEY2>
|
||||
class NonlinearFactor2: public NoiseModelFactor {
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -402,8 +401,8 @@ protected:
|
|||
KEY1 key1_;
|
||||
KEY2 key2_;
|
||||
|
||||
typedef NoiseModelFactor<VALUES> Base;
|
||||
typedef NonlinearFactor2<VALUES, KEY1, KEY2> This;
|
||||
typedef NoiseModelFactor Base;
|
||||
typedef NonlinearFactor2<KEY1, KEY2> This;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -428,7 +427,7 @@ public:
|
|||
|
||||
/** Calls the 2-key specific version of evaluateError, which is pure virtual
|
||||
* 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)) {
|
||||
const X1& x1 = x[key1_];
|
||||
const X2& x2 = x[key2_];
|
||||
|
|
@ -475,8 +474,8 @@ private:
|
|||
/* ************************************************************************* */
|
||||
/** A convenient base class for creating your own NoiseModelFactor with 3
|
||||
* variables. To derive from this class, implement evaluateError(). */
|
||||
template<class VALUES, class KEY1, class KEY2, class KEY3>
|
||||
class NonlinearFactor3: public NoiseModelFactor<VALUES> {
|
||||
template<class KEY1, class KEY2, class KEY3>
|
||||
class NonlinearFactor3: public NoiseModelFactor {
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -492,8 +491,8 @@ protected:
|
|||
KEY2 key2_;
|
||||
KEY3 key3_;
|
||||
|
||||
typedef NoiseModelFactor<VALUES> Base;
|
||||
typedef NonlinearFactor3<VALUES, KEY1, KEY2, KEY3> This;
|
||||
typedef NoiseModelFactor Base;
|
||||
typedef NonlinearFactor3<KEY1, KEY2, KEY3> This;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -520,7 +519,7 @@ public:
|
|||
|
||||
/** Calls the 3-key specific version of evaluateError, which is pure virtual
|
||||
* 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(H)
|
||||
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
|
||||
* variables. To derive from this class, implement evaluateError(). */
|
||||
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4>
|
||||
class NonlinearFactor4: public NoiseModelFactor<VALUES> {
|
||||
template<class KEY1, class KEY2, class KEY3, class KEY4>
|
||||
class NonlinearFactor4: public NoiseModelFactor {
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -588,8 +587,8 @@ protected:
|
|||
KEY3 key3_;
|
||||
KEY4 key4_;
|
||||
|
||||
typedef NoiseModelFactor<VALUES> Base;
|
||||
typedef NonlinearFactor4<VALUES, KEY1, KEY2, KEY3, KEY4> This;
|
||||
typedef NoiseModelFactor Base;
|
||||
typedef NonlinearFactor4<KEY1, KEY2, KEY3, KEY4> This;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -618,7 +617,7 @@ public:
|
|||
|
||||
/** Calls the 4-key specific version of evaluateError, which is pure virtual
|
||||
* 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(H)
|
||||
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
|
||||
* variables. To derive from this class, implement evaluateError(). */
|
||||
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4, class KEY5>
|
||||
class NonlinearFactor5: public NoiseModelFactor<VALUES> {
|
||||
template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5>
|
||||
class NonlinearFactor5: public NoiseModelFactor {
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -690,8 +689,8 @@ protected:
|
|||
KEY4 key4_;
|
||||
KEY5 key5_;
|
||||
|
||||
typedef NoiseModelFactor<VALUES> Base;
|
||||
typedef NonlinearFactor5<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5> This;
|
||||
typedef NoiseModelFactor Base;
|
||||
typedef NonlinearFactor5<KEY1, KEY2, KEY3, KEY4, KEY5> This;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -722,7 +721,7 @@ public:
|
|||
|
||||
/** Calls the 5-key specific version of evaluateError, which is pure virtual
|
||||
* 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(H)
|
||||
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
|
||||
* variables. To derive from this class, implement evaluateError(). */
|
||||
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6>
|
||||
class NonlinearFactor6: public NoiseModelFactor<VALUES> {
|
||||
template<class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6>
|
||||
class NonlinearFactor6: public NoiseModelFactor {
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -799,8 +798,8 @@ protected:
|
|||
KEY5 key5_;
|
||||
KEY6 key6_;
|
||||
|
||||
typedef NoiseModelFactor<VALUES> Base;
|
||||
typedef NonlinearFactor6<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This;
|
||||
typedef NoiseModelFactor Base;
|
||||
typedef NonlinearFactor6<KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -833,7 +832,7 @@ public:
|
|||
|
||||
/** Calls the 6-key specific version of evaluateError, which is pure virtual
|
||||
* 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(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]);
|
||||
|
|
|
|||
|
|
@ -29,20 +29,17 @@ using namespace std;
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES>
|
||||
double NonlinearFactorGraph<VALUES>::probPrime(const VALUES& c) const {
|
||||
double NonlinearFactorGraph::probPrime(const DynamicValues& c) const {
|
||||
return exp(-0.5 * error(c));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES>
|
||||
void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
|
||||
void NonlinearFactorGraph::print(const std::string& str) const {
|
||||
Base::print(str);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES>
|
||||
double NonlinearFactorGraph<VALUES>::error(const VALUES& c) const {
|
||||
double NonlinearFactorGraph::error(const DynamicValues& c) const {
|
||||
double total_error = 0.;
|
||||
// iterate over all the factors_ to accumulate the log probabilities
|
||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
||||
|
|
@ -53,8 +50,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES>
|
||||
std::set<Symbol> NonlinearFactorGraph<VALUES>::keys() const {
|
||||
std::set<Symbol> NonlinearFactorGraph::keys() const {
|
||||
std::set<Symbol> keys;
|
||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
|
||||
if(factor)
|
||||
|
|
@ -64,9 +60,8 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES>
|
||||
Ordering::shared_ptr NonlinearFactorGraph<VALUES>::orderingCOLAMD(
|
||||
const VALUES& config) const {
|
||||
Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
|
||||
const DynamicValues& config) const {
|
||||
|
||||
// Create symbolic graph and initial (iterator) ordering
|
||||
SymbolicFactorGraph::shared_ptr symbolic;
|
||||
|
|
@ -91,8 +86,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES>
|
||||
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::symbolic(const Ordering& ordering) const {
|
||||
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
|
||||
// Generate the symbolic factor graph
|
||||
SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
|
||||
symbolicfg->reserve(this->size());
|
||||
|
|
@ -108,21 +102,19 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES>
|
||||
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph<
|
||||
VALUES>::symbolic(const VALUES& config) const {
|
||||
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
|
||||
const DynamicValues& config) const {
|
||||
// Generate an initial key ordering in iterator order
|
||||
Ordering::shared_ptr ordering(config.orderingArbitrary());
|
||||
return make_pair(symbolic(*ordering), ordering);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES>
|
||||
typename GaussianFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::linearize(
|
||||
const VALUES& config, const Ordering& ordering) const {
|
||||
GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
|
||||
const DynamicValues& config, const Ordering& ordering) const {
|
||||
|
||||
// create an empty linear FG
|
||||
typename GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
|
||||
GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
|
||||
linearFG->reserve(this->size());
|
||||
|
||||
// linearize all factors
|
||||
|
|
|
|||
|
|
@ -35,14 +35,13 @@ namespace gtsam {
|
|||
* 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.
|
||||
*/
|
||||
template<class VALUES>
|
||||
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<VALUES> > {
|
||||
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
|
||||
|
||||
public:
|
||||
|
||||
typedef FactorGraph<NonlinearFactor<VALUES> > Base;
|
||||
typedef boost::shared_ptr<NonlinearFactorGraph<VALUES> > shared_ptr;
|
||||
typedef boost::shared_ptr<NonlinearFactor<VALUES> > sharedFactor;
|
||||
typedef FactorGraph<NonlinearFactor> Base;
|
||||
typedef boost::shared_ptr<NonlinearFactorGraph> shared_ptr;
|
||||
typedef boost::shared_ptr<NonlinearFactor> sharedFactor;
|
||||
|
||||
/** print just calls base class */
|
||||
void print(const std::string& str = "NonlinearFactorGraph: ") const;
|
||||
|
|
@ -51,10 +50,10 @@ namespace gtsam {
|
|||
std::set<Symbol> keys() const;
|
||||
|
||||
/** unnormalized error */
|
||||
double error(const VALUES& c) const;
|
||||
double error(const DynamicValues& c) const;
|
||||
|
||||
/** Unnormalized probability. O(n) */
|
||||
double probPrime(const VALUES& c) const;
|
||||
double probPrime(const DynamicValues& c) const;
|
||||
|
||||
template<class F>
|
||||
void add(const F& factor) {
|
||||
|
|
@ -73,20 +72,20 @@ namespace gtsam {
|
|||
* ordering is found.
|
||||
*/
|
||||
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
|
||||
* ordering and a VariableIndex, which can later be re-used to save
|
||||
* computation.
|
||||
*/
|
||||
Ordering::shared_ptr orderingCOLAMD(const VALUES& config) const;
|
||||
Ordering::shared_ptr orderingCOLAMD(const DynamicValues& config) const;
|
||||
|
||||
/**
|
||||
* linearize a nonlinear factor graph
|
||||
*/
|
||||
boost::shared_ptr<GaussianFactorGraph >
|
||||
linearize(const VALUES& config, const Ordering& ordering) const;
|
||||
linearize(const DynamicValues& config, const Ordering& ordering) const;
|
||||
|
||||
private:
|
||||
|
||||
|
|
|
|||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue