diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 3d140fc77..8f88979a1 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -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 @@ -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 diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index 4da1d3053..26049ec69 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -40,16 +40,27 @@ template > class GaussianISAM2 : public ISAM2 { typedef ISAM2 Base; public: + + /// @name Standard Constructors + /// @{ + /** Create an empty ISAM2 instance */ GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ GaussianISAM2() : ISAM2() {} + /// @} + /// @name Advanced Interface + /// @{ + void cloneTo(boost::shared_ptr& newGaussianISAM2) const { boost::shared_ptr isam2 = boost::static_pointer_cast(newGaussianISAM2); Base::cloneTo(isam2); } + + /// @} + }; /** optimize the BayesTree, starting from the root */ diff --git a/gtsam/nonlinear/Key.h b/gtsam/nonlinear/Key.h index de609d413..1e28f51e6 100644 --- a/gtsam/nonlinear/Key.h +++ b/gtsam/nonlinear/Key.h @@ -60,7 +60,7 @@ public: virtual ~TypedSymbol() {} // Get stuff: - + ///TODO: comment size_t index() const { return j_; } diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 4b47e133e..7537ea192 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -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 NonlinearEquality: public NonlinearFactor1 { @@ -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 > 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 > 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() {} diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index c65b4ac08..ad137a5d9 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -55,6 +55,7 @@ inline void __fill_from_tuple(std::vector& 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 NonlinearFactor: public Factor { @@ -69,6 +70,9 @@ public: typedef boost::shared_ptr > 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. diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index dd0ae2ae5..b572c7d84 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -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 NonlinearFactorGraph: public FactorGraph > { @@ -45,9 +46,16 @@ namespace gtsam { typedef boost::shared_ptr > shared_ptr; typedef boost::shared_ptr > 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 keys() const; @@ -57,11 +65,6 @@ namespace gtsam { /** Unnormalized probability. O(n) */ double probPrime(const VALUES& c) const; - template - void add(const F& factor) { - this->push_back(boost::shared_ptr(new F(factor))); - } - /** * Create a symbolic factor graph using an existing ordering */ @@ -89,6 +92,17 @@ namespace gtsam { boost::shared_ptr linearize(const VALUES& config, const Ordering& ordering) const; + /// @} + /// @name Advanced Interface + /// @{ + + template + void add(const F& factor) { + this->push_back(boost::shared_ptr(new F(factor))); + } + + /// @} + private: /** Serialization function */ diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 8c6d2499d..9f082330c 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -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_; } ///()); @@ -142,6 +154,8 @@ namespace gtsam { return ptr; } + /// @} + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 4073df424..a0fab4006 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -56,6 +56,7 @@ public: * one optimizes the linearized system using various methods. * * To use the optimizer in code, include in your cpp file + * \nosubgrouping */ template 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)); } + ///TODO: comment static shared_values optimizeLM(const G& graph, const T& values, Parameters::verbosityLevel verbosity) { @@ -367,6 +392,7 @@ public: boost::make_shared(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 ¶meters, double currentError, double newError); } // gtsam +/// @} + #include diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index faa06bbc7..4d10b15b5 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -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 & 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 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 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 */ diff --git a/gtsam/nonlinear/TupleValues.h b/gtsam/nonlinear/TupleValues.h index 9f1cbebae..df3eab51b 100644 --- a/gtsam/nonlinear/TupleValues.h +++ b/gtsam/nonlinear/TupleValues.h @@ -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 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 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);} /// void insert(const TupleValues& values) { second_.insert(values); } - void insert(const TupleValues& values) { + void insert(const TupleValues& values) { /// void update(const TupleValues& values) { second_.update(values); } - void update(const TupleValues& values) { + void update(const TupleValues& values) { /// 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); } /// void insertSub(const CFG& values) { second_.insertSub(values); } - void insertSub(const VALUES1& values) { first_.insert(values); } + void insertSub(const VALUES1& values) { first_.insert(values); } /// void erase(const KEY& j) { second_.erase(j); } - void erase(const Key1& j) { first_.erase(j); } + void erase(const Key1& j) { first_.erase(j); } /// 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); } /// boost::optional exists_(const KEY& j) const { return second_.exists_(j); } - boost::optional exists_(const Key1& j) const { return first_.exists_(j); } + boost::optional exists_(const Key1& j) const { return first_.exists_(j); } /// 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]; } /// 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); } /// 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 pair. The operator must be able to handle such an + * iterator for every type in the Values, (i.e. through templating). + */ + template + void apply(A& operation) { + first_.apply(operation); + second_.apply(operation); + } + + /** + * TODO: comment + */ + template + 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 dims(const Ordering& ordering) const { + _ValuesDimensionCollector dimCollector(ordering); + this->apply(dimCollector); + return dimCollector.dimensions; + } + /** Expmap */ TupleValues 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 pair. The operator must be able to handle such an - * iterator for every type in the Values, (i.e. through templating). - */ - template - void apply(A& operation) { - first_.apply(operation); - second_.apply(operation); - } - template - void apply(A& operation) const { - first_.apply(operation); - second_.apply(operation); - } + /// @} private: /** Serialization function */ diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index a4bc57c0c..4c98e8a58 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -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 Values(const Values& 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 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(); } /// 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(); } /// 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 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 */ diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index 6f2c5b0b7..f5fb7fae1 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -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 WhiteNoiseFactor: public NonlinearFactor { @@ -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 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