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

added comment groups to some nonlinear classes

release/4.3a0
Nick Barrash 2012-01-23 20:16:59 +00:00
parent f7865c80b1
commit aa940ec8d0
13 changed files with 314 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -37,6 +37,7 @@ namespace gtsam {
*
* The class provides a "predict" and "update" function to perform these steps independently.
* TODO: a "predictAndUpdate" that combines both steps for some computational savings.
* \nosubgrouping
*/
template<class VALUES, class KEY>
@ -57,9 +58,17 @@ namespace gtsam {
const KEY& x, JacobianFactor::shared_ptr& newPrior) const;
public:
/// @name Standard Constructors
/// @{
ExtendedKalmanFilter(T x_initial,
noiseModel::Gaussian::shared_ptr P_initial);
/// @}
/// @name Testable
/// @{
/// print
void print(const std::string& s="") const {
std::cout << s << "\n";
@ -67,8 +76,17 @@ namespace gtsam {
priorFactor_->print(s+"density");
}
/// @}
/// @name Advanced Interface
/// @{
///TODO: comment
T predict(const MotionFactor& motionFactor);
///TODO: comment
T update(const MeasurementFactor& measurementFactor);
/// @}
};
} // namespace

11
gtsam/nonlinear/GaussianISAM2.h
View File

@ -40,16 +40,27 @@ template <class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES, GRAPH> {
typedef ISAM2<GaussianConditional, VALUES, GRAPH> Base;
public:
/// @name Standard Constructors
/// @{
/** Create an empty ISAM2 instance */
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES, GRAPH>(params) {}
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
GaussianISAM2() : ISAM2<GaussianConditional, VALUES, GRAPH>() {}
/// @}
/// @name Advanced Interface
/// @{
void cloneTo(boost::shared_ptr<GaussianISAM2>& newGaussianISAM2) const {
boost::shared_ptr<Base> isam2 = boost::static_pointer_cast<Base>(newGaussianISAM2);
Base::cloneTo(isam2);
}
/// @}
};
/** optimize the BayesTree, starting from the root */

2
gtsam/nonlinear/Key.h
View File

@ -60,7 +60,7 @@ public:
virtual ~TypedSymbol() {}
// Get stuff:
///TODO: comment
size_t index() const {
return j_;
}

17
gtsam/nonlinear/NonlinearEquality.h
View File

@ -44,6 +44,8 @@ namespace gtsam {
* Switchable implementation:
* - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain
* - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error
*
* \nosubgrouping
*/
template<class VALUES, class KEY>
class NonlinearEquality: public NonlinearFactor1<VALUES, KEY> {
@ -76,6 +78,9 @@ namespace gtsam {
virtual ~NonlinearEquality() {}
/// @name Standard Constructors
/// @{
/**
* Constructor - forces exact evaluation
*/
@ -94,6 +99,10 @@ namespace gtsam {
compare_(_compare) {
}
/// @}
/// @name Testable
/// @{
void print(const std::string& s = "") const {
std::cout << "Constraint: " << s << " on [" << (std::string)(this->key_) << "]\n";
gtsam::print(feasible_,"Feasible Point");
@ -107,6 +116,10 @@ namespace gtsam {
fabs(error_gain_ - f.error_gain_) < tol;
}
/// @}
/// @name Standard Interface
/// @{
/** actual error function calculation */
virtual double error(const VALUES& c) const {
const T& xj = c[this->key_];
@ -143,6 +156,8 @@ namespace gtsam {
return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model));
}
/// @}
private:
/** Serialization function */
@ -183,6 +198,7 @@ namespace gtsam {
typedef boost::shared_ptr<NonlinearEquality1<VALUES, KEY> > shared_ptr;
///TODO: comment
NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0)
: Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
@ -237,6 +253,7 @@ namespace gtsam {
typedef boost::shared_ptr<NonlinearEquality2<VALUES, KEY> > shared_ptr;
///TODO: comment
NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0)
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
virtual ~NonlinearEquality2() {}

17
gtsam/nonlinear/NonlinearFactor.h
View File

@ -55,6 +55,7 @@ inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vec
* Templated on a values structure type. The values structures are typically
* more general than just vectors, e.g., Rot3 or Pose3,
* which are objects in non-linear manifolds (Lie groups).
* \nosubgrouping
*/
template<class VALUES>
class NonlinearFactor: public Factor<Symbol> {
@ -69,6 +70,9 @@ public:
typedef boost::shared_ptr<NonlinearFactor<VALUES> > shared_ptr;
/// @name Standard Constructors
/// @{
/** Default constructor for I/O only */
NonlinearFactor() {
}
@ -94,14 +98,23 @@ public:
this->keys_.insert(this->keys_.end(), beginKeys, endKeys);
}
/** Destructor */
virtual ~NonlinearFactor() {}
/// @}
/// @name Testable
/// @{
/** print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor\n";
}
/// @}
/// @name Standard Interface
/// @{
/** Destructor */
virtual ~NonlinearFactor() {}
/**
* Calculate the error of the factor
* This is typically equal to log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian.

24
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -34,6 +34,7 @@ namespace gtsam {
* Linearizing the non-linear factor graph creates a linear factor graph on the
* 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.
* \nosubgrouping
*/
template<class VALUES>
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<VALUES> > {
@ -45,9 +46,16 @@ namespace gtsam {
typedef boost::shared_ptr<NonlinearFactorGraph<VALUES> > shared_ptr;
typedef boost::shared_ptr<NonlinearFactor<VALUES> > sharedFactor;
/// @name Testable
/// @{
/** print just calls base class */
void print(const std::string& str = "NonlinearFactorGraph: ") const;
/// @}
/// @name Standard Interface
/// @{
/** return keys in some random order */
std::set<Symbol> keys() const;
@ -57,11 +65,6 @@ namespace gtsam {
/** Unnormalized probability. O(n) */
double probPrime(const VALUES& c) const;
template<class F>
void add(const F& factor) {
this->push_back(boost::shared_ptr<F>(new F(factor)));
}
/**
* Create a symbolic factor graph using an existing ordering
*/
@ -89,6 +92,17 @@ namespace gtsam {
boost::shared_ptr<GaussianFactorGraph >
linearize(const VALUES& config, const Ordering& ordering) const;
/// @}
/// @name Advanced Interface
/// @{
template<class F>
void add(const F& factor) {
this->push_back(boost::shared_ptr<F>(new F(factor)));
}
/// @}
private:
/** Serialization function */

42
gtsam/nonlinear/NonlinearISAM.h
View File

@ -51,6 +51,9 @@ protected:
public:
/// @name Standard Constructors
/// @{
/**
* Periodically reorder and relinearize
* @param reorderInterval is the number of updates between reorderings,
@ -60,21 +63,13 @@ public:
*/
NonlinearISAM(int reorderInterval = 1) : reorderInterval_(reorderInterval), reorderCounter_(0) {}
/** Add new factors along with their initial linearization points */
void update(const Factors& newFactors, const Values& initialValues);
/// @}
/// @name Standard Interface
/// @{
/** Return the current solution estimate */
Values estimate() const;
/** Relinearization and reordering of variables */
void reorder_relinearize();
/** manually add a key to the end of the ordering */
void addKey(const Symbol& key) { ordering_.push_back(key); }
/** replace the current ordering */
void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; }
/** find the marginal covariance for a single variable */
Matrix marginalCovariance(const Symbol& key) const;
@ -92,12 +87,31 @@ public:
/** get underlying nonlinear graph */
const Factors& getFactorsUnsafe() const { return factors_; }
/** get counters */
int reorderInterval() const { return reorderInterval_; }
int reorderCounter() const { return reorderCounter_; }
/** get counters */
int reorderInterval() const { return reorderInterval_; } ///<TODO: comment
int reorderCounter() const { return reorderCounter_; } ///<TODO: comment
/** prints out all contents of the system */
void print(const std::string& s="") const;
/// @}
/// @name Advanced Interface
/// @{
/** Add new factors along with their initial linearization points */
void update(const Factors& newFactors, const Values& initialValues);
/** Relinearization and reordering of variables */
void reorder_relinearize();
/** manually add a key to the end of the ordering */
void addKey(const Symbol& key) { ordering_.push_back(key); }
/** replace the current ordering */
void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; }
/// @}
};
} // \namespace gtsam

14
gtsam/nonlinear/NonlinearOptimizationParameters.h
View File

@ -26,6 +26,7 @@ namespace gtsam {
/**
* a container for all related parameters
* \nosubgrouping
*/
struct NonlinearOptimizationParameters {
@ -70,6 +71,9 @@ namespace gtsam {
/// if true, solve whole system with QR, otherwise use LDL when possible
bool useQR_;
/// @name Standard Constructors
/// @{
/// Default constructor
NonlinearOptimizationParameters() :
absDecrease_(1e-6), relDecrease_(1e-6), sumError_(0.0), maxIterations_(
@ -97,6 +101,10 @@ namespace gtsam {
parameters.lambdaMode_), useQR_(parameters.useQR_) {
}
/// @}
/// @name Standard Interface
/// @{
/// a copy of old instance except new lambda
sharedThis newLambda_(double lambda) const {
sharedThis ptr(
@ -105,6 +113,10 @@ namespace gtsam {
return ptr;
}
/// @}
/// @name Advanced Interface
/// @{
/// a copy of old instance except new verbosity
static sharedThis newVerbosity(verbosityLevel verbosity) {
sharedThis ptr(boost::make_shared<NonlinearOptimizationParameters>());
@ -142,6 +154,8 @@ namespace gtsam {
return ptr;
}
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;

29
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -56,6 +56,7 @@ public:
* one optimizes the linearized system using various methods.
*
* To use the optimizer in code, include <gtsam/NonlinearOptimizer-inl.h> in your cpp file
* \nosubgrouping
*/
template<class G, class T, class L = GaussianFactorGraph, class GS = GaussianMultifrontalSolver, class W = NullOptimizerWriter>
class NonlinearOptimizer {
@ -108,6 +109,9 @@ private:
// FIXME: remove this!
shared_solver spcg_solver_;
/// @name Advanced Constructors
/// @{
/**
* Constructor that does not do any computation
*/
@ -120,32 +124,43 @@ private:
/** constructors to replace specific components */
///TODO: comment
This newValues_(shared_values newValues) const {
return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues),
ordering_, parameters_, dimensions_, iterations_, structure_); }
///TODO: comment
This newValuesErrorLambda_(shared_values newValues, double newError, double newLambda) const {
return NonlinearOptimizer(graph_, newValues, newError, ordering_,
parameters_->newLambda_(newLambda), dimensions_, iterations_, structure_); }
///TODO: comment
This newIterations_(int iterations) const {
return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters_, dimensions_,
iterations, structure_); }
///TODO: comment
This newLambda_(double newLambda) const {
return NonlinearOptimizer(graph_, values_, error_, ordering_,
parameters_->newLambda_(newLambda), dimensions_, iterations_, structure_); }
///TODO: comment
This newValuesLambda_(shared_values newValues, double newLambda) const {
return NonlinearOptimizer(graph_, newValues, graph_->error(*newValues),
ordering_, parameters_->newLambda_(newLambda), dimensions_, iterations_, structure_); }
///TODO: comment
This newParameters_(shared_parameters parameters) const {
return NonlinearOptimizer(graph_, values_, error_, ordering_, parameters, dimensions_,
iterations_, structure_); }
/// @}
public:
/// @name Standard Constructors
/// @{
/**
* Constructor that evaluates new error
*/
@ -174,6 +189,10 @@ public:
// access to member variables
/// @}
/// @name Standard Interface
/// @{
/** Return current error */
double error() const { return error_; }
@ -244,6 +263,10 @@ public:
// suggested interface
NonlinearOptimizer gaussNewton() const;
/// @}
/// @name Advanced Interface
/// @{
/** Recursively try to do tempered Gauss-Newton steps until we succeed */
NonlinearOptimizer try_lambda(const L& linear);
@ -299,6 +322,7 @@ public:
return result.values();
}
///TODO: comment
static shared_values optimizeLM(shared_graph graph,
shared_values values,
Parameters::verbosityLevel verbosity) {
@ -316,6 +340,7 @@ public:
boost::make_shared<Parameters>(parameters));
}
///TODO: comment
static shared_values optimizeLM(const G& graph,
const T& values,
Parameters::verbosityLevel verbosity) {
@ -367,6 +392,7 @@ public:
boost::make_shared<Parameters>(parameters));
}
///TODO: comment
static shared_values optimizeDogLeg(const G& graph,
const T& values,
Parameters::verbosityLevel verbosity) {
@ -414,10 +440,13 @@ bool check_convergence (
double errorThreshold,
double currentError, double newError, int verbosity);
///TODO: comment
bool check_convergence (
const NonlinearOptimizationParameters &parameters,
double currentError, double newError);
} // gtsam
/// @}
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>

46
gtsam/nonlinear/Ordering.h
View File

@ -30,6 +30,7 @@ namespace gtsam {
/**
* An ordering is a map from symbols (non-typed keys) to integer indices
* \nosubgrouping
*/
class Ordering {
protected:
@ -46,26 +47,28 @@ public:
typedef Map::iterator iterator;
typedef Map::const_iterator const_iterator;
/// @name Standard Constructors
/// @{
/// Default constructor for empty ordering
Ordering() : nVars_(0) {}
/// Construct from list, assigns order indices sequentially to list items.
Ordering(const std::list<Symbol> & L) ;
/// @}
/// @name Standard Interface
/// @{
/** One greater than the maximum ordering index, i.e. including missing indices in the count. See also size(). */
Index nVars() const { return nVars_; }
/** The actual number of variables in this ordering, i.e. not including missing indices in the count. See also nVars(). */
Index size() const { return order_.size(); }
iterator begin() { return order_.begin(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */
const_iterator begin() const { return order_.begin(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */
iterator end() { return order_.end(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */
const_iterator end() const { return order_.end(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */
// access to integer indices
Index& at(const Symbol& key) { return operator[](key); } ///< Synonym for operator[](const Symbol&)
Index at(const Symbol& key) const { return operator[](key); } ///< Synonym for operator[](const Symbol&) const
/** Assigns the ordering index of the requested \c key into \c index if the symbol
@ -117,8 +120,6 @@ public:
*/
const_iterator find(const Symbol& key) const { return order_.find(key); }
// adding symbols
/**
* Attempts to insert a symbol/order pair with same semantics as stl::Map::insert(),
* i.e., returns a pair of iterator and success (false if already present)
@ -129,7 +130,6 @@ public:
nVars_ = key_order.second+1;
return it_ok;
}
std::pair<iterator,bool> tryInsert(const Symbol& key, Index order) { return tryInsert(std::make_pair(key,order)); }
/** Try insert, but will fail if the key is already present */
iterator insert(const value_type& key_order) {
@ -137,11 +137,30 @@ public:
if(!it_ok.second) throw std::invalid_argument(std::string());
else return it_ok.first;
}
iterator insert(const Symbol& key, Index order) { return insert(std::make_pair(key,order)); }
/// @}
/// @name Advanced Interface
/// @{
/**
* Iterator in order of sorted symbols, not in elimination/index order!
*/
iterator begin() { return order_.begin(); }
/**
* Iterator in order of sorted symbols, not in elimination/index order!
*/
iterator end() { return order_.end(); }
/// Test if the key exists in the ordering.
bool exists(const Symbol& key) const { return order_.count(key); }
///TODO: comment
std::pair<iterator,bool> tryInsert(const Symbol& key, Index order) { return tryInsert(std::make_pair(key,order)); }
///TODO: comment
iterator insert(const Symbol& key, Index order) { return insert(std::make_pair(key,order)); }
/// Adds a new key to the ordering with an index of one greater than the current highest index.
Index push_back(const Symbol& key) { return insert(std::make_pair(key, nVars_))->second; }
@ -182,12 +201,21 @@ public:
*/
void permuteWithInverse(const Permutation& inversePermutation);
/// Synonym for operator[](const Symbol&)
Index& at(const Symbol& key) { return operator[](key); }
/// @}
/// @name Testable
/// @{
/** print (from Testable) for testing and debugging */
void print(const std::string& str = "Ordering:") const;
/** equals (from Testable) for testing and debugging */
bool equals(const Ordering& rhs, double tol = 0.0) const;
/// @}
private:
/** Serialization function */

100
gtsam/nonlinear/TupleValues.h
View File

@ -45,6 +45,7 @@ namespace gtsam {
* have two versions: one with templates and one without. The templated one allows
* for the arguments to be passed to the next values, while the specialized one
* operates on the "first" values. TupleValuesEnd contains only the specialized version.
* \nosubgrouping
*/
template<class VALUES1, class VALUES2>
class TupleValues {
@ -63,6 +64,9 @@ namespace gtsam {
typedef typename VALUES1::Key Key1;
typedef typename VALUES1::Value Value1;
/// @name Standard Constructors
/// @{
/** default constructor */
TupleValues() {}
@ -74,6 +78,10 @@ namespace gtsam {
TupleValues(const VALUES1& cfg1, const VALUES2& cfg2) :
first_(cfg1), second_(cfg2) {}
/// @}
/// @name Testable
/// @{
/** Print */
void print(const std::string& s = "") const {
first_.print(s);
@ -85,6 +93,10 @@ namespace gtsam {
return first_.equals(c.first_, tol) && second_.equals(c.second_, tol);
}
/// @}
/// @name Advanced Interface
/// @{
/**
* Insert a key/value pair to the values.
* Note: if the key is already in the values, the values will not be changed.
@ -94,8 +106,8 @@ namespace gtsam {
*/
template<class KEY, class VALUE>
void insert(const KEY& key, const VALUE& value) {second_.insert(key, value);}
void insert(int key, const Value1& value) {first_.insert(Key1(key), value);}
void insert(const Key1& key, const Value1& value) {first_.insert(key, value);}
void insert(int key, const Value1& value) {first_.insert(Key1(key), value);} ///<TODO: comment
void insert(const Key1& key, const Value1& value) {first_.insert(key, value);} ///<TODO: comment
/**
* Insert a complete values at a time.
@ -105,7 +117,7 @@ namespace gtsam {
*/
template<class CFG1, class CFG2>
void insert(const TupleValues<CFG1, CFG2>& values) { second_.insert(values); }
void insert(const TupleValues<VALUES1, VALUES2>& values) {
void insert(const TupleValues<VALUES1, VALUES2>& values) { ///<TODO: comment
first_.insert(values.first_);
second_.insert(values.second_);
}
@ -116,7 +128,7 @@ namespace gtsam {
*/
template<class CFG1, class CFG2>
void update(const TupleValues<CFG1, CFG2>& values) { second_.update(values); }
void update(const TupleValues<VALUES1, VALUES2>& values) {
void update(const TupleValues<VALUES1, VALUES2>& values) { ///<TODO: comment
first_.update(values.first_);
second_.update(values.second_);
}
@ -128,7 +140,7 @@ namespace gtsam {
*/
template<class KEY, class VALUE>
void update(const KEY& key, const VALUE& value) { second_.update(key, value); }
void update(const Key1& key, const Value1& value) { first_.update(key, value); }
void update(const Key1& key, const Value1& value) { first_.update(key, value); } ///<TODO: comment
/**
* Insert a subvalues
@ -136,39 +148,43 @@ namespace gtsam {
*/
template<class CFG>
void insertSub(const CFG& values) { second_.insertSub(values); }
void insertSub(const VALUES1& values) { first_.insert(values); }
void insertSub(const VALUES1& values) { first_.insert(values); } ///<TODO: comment
/** erase an element by key */
template<class KEY>
void erase(const KEY& j) { second_.erase(j); }
void erase(const Key1& j) { first_.erase(j); }
void erase(const Key1& j) { first_.erase(j); } ///<TODO: comment
/** clears the values */
void clear() { first_.clear(); second_.clear(); }
/// @}
/// @name Standard Interface
/// @{
/** determine whether an element exists */
template<class KEY>
bool exists(const KEY& j) const { return second_.exists(j); }
bool exists(const Key1& j) const { return first_.exists(j); }
bool exists(const Key1& j) const { return first_.exists(j); } ///<TODO: comment
/** a variant of exists */
template<class KEY>
boost::optional<typename KEY::Value> exists_(const KEY& j) const { return second_.exists_(j); }
boost::optional<Value1> exists_(const Key1& j) const { return first_.exists_(j); }
boost::optional<Value1> exists_(const Key1& j) const { return first_.exists_(j); } ///<TODO: comment
/** access operator */
template<class KEY>
const typename KEY::Value & operator[](const KEY& j) const { return second_[j]; }
const Value1& operator[](const Key1& j) const { return first_[j]; }
const Value1& operator[](const Key1& j) const { return first_[j]; } ///<TODO: comment
/** at access function */
template<class KEY>
const typename KEY::Value & at(const KEY& j) const { return second_.at(j); }
const Value1& at(const Key1& j) const { return first_.at(j); }
const Value1& at(const Key1& j) const { return first_.at(j); } ///<TODO: comment
/** direct values access */
const VALUES1& values() const { return first_; }
const VALUES2& rest() const { return second_; }
const VALUES2& rest() const { return second_; } ///<TODO: comment
/** zero: create VectorValues of appropriate structure */
VectorValues zero(const Ordering& ordering) const {
@ -181,16 +197,6 @@ namespace gtsam {
/** @return true if values is empty */
bool empty() const { return first_.empty() && second_.empty(); }
/** @return The dimensionality of the tangent space */
size_t dim() const { return first_.dim() + second_.dim(); }
/** Create an array of variable dimensions using the given ordering */
std::vector<size_t> dims(const Ordering& ordering) const {
_ValuesDimensionCollector dimCollector(ordering);
this->apply(dimCollector);
return dimCollector.dimensions;
}
/**
* Generate a default ordering, simply in key sort order. To instead
* create a fill-reducing ordering, use
@ -204,6 +210,40 @@ namespace gtsam {
return keyOrderer.ordering;
}
/**
* 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) {
first_.apply(operation);
second_.apply(operation);
}
/**
* TODO: comment
*/
template<typename A>
void apply(A& operation) const {
first_.apply(operation);
second_.apply(operation);
}
/// @}
/// @name Manifold
/// @{
/** @return The dimensionality of the tangent space */
size_t dim() const { return first_.dim() + second_.dim(); }
/** Create an array of variable dimensions using the given ordering */
std::vector<size_t> dims(const Ordering& ordering) const {
_ValuesDimensionCollector dimCollector(ordering);
this->apply(dimCollector);
return dimCollector.dimensions;
}
/** Expmap */
TupleValues<VALUES1, VALUES2> retract(const VectorValues& delta, const Ordering& ordering) const {
return TupleValues(first_.retract(delta, ordering), second_.retract(delta, ordering));
@ -222,21 +262,7 @@ namespace gtsam {
second_.localCoordinates(cp.second_, ordering, delta);
}
/**
* 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) {
first_.apply(operation);
second_.apply(operation);
}
template<typename A>
void apply(A& operation) const {
first_.apply(operation);
second_.apply(operation);
}
/// @}
private:
/** Serialization function */

46
gtsam/nonlinear/Values.h
View File

@ -20,6 +20,7 @@
* of variables in a factor graph. A Values is a values structure which can hold variables that
* are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
* which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
* \nosubgrouping
*/
#pragma once
@ -77,13 +78,22 @@ namespace gtsam {
public:
/// @name Standard Constructors
/// @{
///TODO comment
Values() {}
///TODO: comment
Values(const Values& config) :
values_(config.values_) {}
///TODO: comment
template<class J_ALT>
Values(const Values<J_ALT>& other) {} // do nothing when initializing with wrong type
virtual ~Values() {}
/// @}
/// @name Testable
/// @{
@ -94,6 +104,8 @@ namespace gtsam {
bool equals(const Values& expected, double tol=1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/** Retrieve a variable by j, throws KeyDoesNotExist<J> if not found */
const Value& at(const J& j) const;
@ -120,11 +132,21 @@ namespace gtsam {
/** Get a zero VectorValues of the correct structure */
VectorValues zero(const Ordering& ordering) const;
const_iterator begin() const { return values_.begin(); }
const_iterator end() const { return values_.end(); }
iterator begin() { return values_.begin(); }
iterator end() { return values_.end(); }
const_iterator begin() const { return values_.begin(); } ///<TODO: comment
const_iterator end() const { return values_.end(); } ///<TODO: comment
/** Create an array of variable dimensions using the given ordering */
std::vector<size_t> dims(const Ordering& ordering) const;
/**
* Generate a default ordering, simply in key sort order. To instead
* create a fill-reducing ordering, use
* NonlinearFactorGraph::orderingCOLAMD(). Alternatively, you may permute
* this ordering yourself (as orderingCOLAMD() does internally).
*/
Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const;
/// @}
/// @name Manifold Operations
/// @{
@ -141,9 +163,14 @@ namespace gtsam {
void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const;
/// @}
/// @name Advanced Interface
/// @{
// imperative methods:
iterator begin() { return values_.begin(); } ///<TODO: comment
iterator end() { return values_.end(); } ///<TODO: comment
/** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
void insert(const J& j, const Value& val);
@ -198,16 +225,7 @@ namespace gtsam {
operation(it);
}
/** Create an array of variable dimensions using the given ordering */
std::vector<size_t> dims(const Ordering& ordering) const;
/**
* Generate a default ordering, simply in key sort order. To instead
* create a fill-reducing ordering, use
* NonlinearFactorGraph::orderingCOLAMD(). Alternatively, you may permute
* this ordering yourself (as orderingCOLAMD() does internally).
*/
Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const;
/// @}
private:
/** Serialization function */

38
gtsam/nonlinear/WhiteNoiseFactor.h
View File

@ -35,6 +35,7 @@ namespace gtsam {
* MKEY: key type to use for mean
* PKEY: key type to use for precision
* VALUES: Values type for optimization
* \nosubgrouping
*/
template<class MKEY, class PKEY, class VALUES>
class WhiteNoiseFactor: public NonlinearFactor<VALUES> {
@ -84,6 +85,9 @@ namespace gtsam {
new HessianFactor(j1, j2, G11, G12, g1, G22, g2, c));
}
/// @name Standard Constructors
/// @{
/** Construct from measurement
* @param z Measurment value
* @param meanKey Key for mean variable
@ -93,16 +97,28 @@ namespace gtsam {
Base(), z_(z), meanKey_(meanKey), precisionKey_(precisionKey) {
}
/// @}
/// @name Advanced Constructors
/// @{
/// Destructor
virtual ~WhiteNoiseFactor() {
}
/// @}
/// @name Testable
/// @{
/// Print
void print(const std::string& p = "WhiteNoiseFactor") const {
Base::print(p);
std::cout << p + ".z: " << z_ << std::endl;
}
/// @}
/// @name Standard Interface
/// @{
/// get the dimension of the factor (number of rows on linearization)
virtual size_t dim() const {
return 2;
@ -124,6 +140,19 @@ namespace gtsam {
return Vector_(1, sqrt(2 * error(x)));
}
/**
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
const Index j1 = ordering[meanKey_], j2 = ordering[precisionKey_];
return IndexFactor::shared_ptr(new IndexFactor(j1, j2));
}
/// @}
/// @name Advanced Interface
/// @{
/// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<GaussianFactor> linearize(const VALUES& x,
const Ordering& ordering) const {
@ -134,14 +163,7 @@ namespace gtsam {
return linearize(z_, u, p, j1, j2);
}
/**
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
const Index j1 = ordering[meanKey_], j2 = ordering[precisionKey_];
return IndexFactor::shared_ptr(new IndexFactor(j1, j2));
}
/// @}
};
// WhiteNoiseFactor