diff --git a/gtsam/nonlinear/DynamicValues-inl.h b/gtsam/nonlinear/DynamicValues-inl.h new file mode 100644 index 000000000..451077d9c --- /dev/null +++ b/gtsam/nonlinear/DynamicValues-inl.h @@ -0,0 +1,38 @@ +/* ---------------------------------------------------------------------------- + + * 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 DynamicValues.h + * @author Richard Roberts + * + * @brief A non-templated config holding any types of Manifold-group elements + * + * Detailed story: + * A values structure is a map from keys to values. It is used to specify the value of a bunch + * 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. + */ + +#include // Only so Eclipse finds class definition + +namespace gtsam { + + /* ************************************************************************* */ + const char* DynamicValuesIncorrectType::what() const throw() { + if(message_.empty()) + message_ = + "Attempting to retrieve value with key \"" + (std::string)key_ + "\", type stored in DynamicValues is " + + std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name()); + return message_.c_str(); + } + +} diff --git a/gtsam/nonlinear/DynamicValues.cpp b/gtsam/nonlinear/DynamicValues.cpp new file mode 100644 index 000000000..79270d060 --- /dev/null +++ b/gtsam/nonlinear/DynamicValues.cpp @@ -0,0 +1,47 @@ +/* ---------------------------------------------------------------------------- + + * 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 DynamicValues.h + * @author Richard Roberts + * + * @brief A non-templated config holding any types of Manifold-group elements + * + * Detailed story: + * A values structure is a map from keys to values. It is used to specify the value of a bunch + * 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. + */ + +#include + +#include + +using namespace std; + +namespace gtsam { + + void DynamicValues::print(const string& str) const { + cout << str << "DynamicValues with " << size() << " values:\n" << endl; + BOOST_FOREACH(const KeyValueMap::value_type& key_value, values_) { + cout << " " << (string)key_value.first << ": "; + key_value.second->print(""); + } + } + + bool DynamicValues::equals(const DynamicValues& other, double tol) const { + if(this->size() != other.size()) + return false; + for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) + } + +} diff --git a/gtsam/nonlinear/DynamicValues.h b/gtsam/nonlinear/DynamicValues.h new file mode 100644 index 000000000..747741bfb --- /dev/null +++ b/gtsam/nonlinear/DynamicValues.h @@ -0,0 +1,305 @@ +/* ---------------------------------------------------------------------------- + + * 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 DynamicValues.h + * @author Richard Roberts + * + * @brief A non-templated config holding any types of Manifold-group elements + * + * Detailed story: + * A values structure is a map from keys to values. It is used to specify the value of a bunch + * 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. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace gtsam { + + class DynamicValues { + + private: + + typedef FastMap > KeyValueMap; + KeyValueMap values_; + + public: + + typedef KeyValueMap::iterator iterator; + typedef KeyValueMap::const_iterator const_iterator; + typedef KeyValueMap::reverse_iterator reverse_iterator; + typedef KeyValueMap::const_reverse_iterator const_reverse_iterator; + + /** Default constructor creates an empty DynamicValues class */ + DynamicValues() {} + + /** Copy constructor duplicates all keys and values */ + DynamicValues(const DynamicValues& other) {} + + /// @name Testable + /// @{ + + /** print method for testing and debugging */ + void print(const std::string& str = "") const; + + /** Test whether the sets of keys and values are identical */ + bool equals(const DynamicValues& other, double tol=1e-9) const; + + /// @} + + /** Retrieve a variable by key \c j. The type of the value associated with + * this key is supplied as a template argument to this function. + * @param j Retrieve the value associated with this key + * @tparam Value The type of the value stored with this key, this method + * throws DynamicValuesIncorrectType if this requested type is not correct. + * @return A const reference to the stored value + */ + template + const Value& at(const Symbol& j) const; + + /** Retrieve a variable using a special key (typically TypedSymbol), which + * contains the type of the value associated with the key, and which must + * be conversion constructible to a Symbol, e.g. + * Symbol(const TypedKey&). Throws DynamicValuesKeyDoesNotExist + * the key is not found, and DynamicValuesIncorrectType if the value type + * associated with the requested key does not match the stored value type. + */ + template + const typename TypedKey::Value& at(const TypedKey& j) const; + + /** operator[] syntax for at(const TypedKey& j) */ + template + const typename TypedKey::Value& operator[](const TypedKey& j) const { + return at(j); } + + /** Check if a value exists with key \c j. See exists<>(const Symbol& j) + * and exists(const TypedKey& j) for versions that return the value if it + * exists. */ + bool exists(const Symbol& i) const; + + /** Check if a value with key \c j exists, returns the value with type + * \c Value if the key does exist, or boost::none if it does not exist. + * Throws DynamicValuesIncorrectType if the value type associated with the + * requested key does not match the stored value type. */ + template + boost::optional exists(const Symbol& j) const; + + /** Check if a value with key \c j exists, returns the value with type + * \c Value if the key does exist, or boost::none if it does not exist. + * Uses a special key (typically TypedSymbol), which contains the type of + * the value associated with the key, and which must be conversion + * constructible to a Symbol, e.g. Symbol(const TypedKey&). Throws + * DynamicValuesIncorrectType if the value type associated with the + * requested key does not match the stored value type. + */ + template + boost::optional exists(const TypedKey& j) const; + + /** The number of variables in this config */ + size_t size() const { return values_.size(); } + + /** whether the config is empty */ + bool empty() const { return values_.empty(); } + + /** 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_reverse_iterator rbegin() const { return values_.rbegin(); } + const_reverse_iterator rend() const { return values_.rend(); } + reverse_iterator rbegin() { return values_.rbegin(); } + reverse_iterator rend() { return values_.rend(); } + + /// @name Manifold Operations + /// @{ + + /** The dimensionality of the tangent space */ + size_t dim() const; + + /** Add a delta config to current config and returns a new config */ + DynamicValues retract(const VectorValues& delta, const Ordering& ordering) const; + + /** Get a delta config about a linearization point c0 (*this) */ + VectorValues localCoordinates(const DynamicValues& cp, const Ordering& ordering) const; + + /** Get a delta config about a linearization point c0 (*this) */ + void localCoordinates(const DynamicValues& cp, const Ordering& ordering, VectorValues& delta) const; + + ///@} + + // imperative methods: + + /** Add a variable with the given j, throws KeyAlreadyExists if j is already present */ + template + void insert(const Symbol& j, const ValueType& val); + + /** Add a set of variables, throws KeyAlreadyExists if a key is already present */ + void insert(const DynamicValues& cfg); + + /** update the current available values without adding new ones */ + void update(const DynamicValues& cfg); + + /** single element change of existing element */ + template + void update(const Symbol& j, const ValueType& val); + + /** Remove a variable from the config, throws KeyDoesNotExist if j is not present */ + void erase(const Symbol& j); + + /** Remove a variable from the config while returning the dimensionality of + * the removed element (normally not needed by user code), throws + * KeyDoesNotExist if j is not present. + */ + //void erase(const J& j, size_t& dim); + + /** + * Returns a set of keys in the config + * Note: by construction, the list is ordered + */ + std::list keys() const; + + /** Replace all keys and variables */ + DynamicValues& operator=(const DynamicValues& rhs) { + values_ = rhs.values_; + return (*this); + } + + /** Remove all variables from the config */ + void clear() { + values_.clear(); + } + + /** + * 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) { + for(iterator it = begin(); it != end(); ++it) + operation(it); + } + template + void apply(A& operation) const { + for(const_iterator it = begin(); it != end(); ++it) + 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; + + }; + + /* ************************************************************************* */ + template + class DynamicValuesKeyAlreadyExists : public std::exception { + protected: + const Symbol key_; ///< The key that already existed + const Value value_; ///< The value attempted to be inserted + + private: + mutable std::string message_; + + public: + /// Construct with the key-value pair attemped to be added + DynamicValuesKeyAlreadyExists(const Symbol& key, const Value& value) throw() : + key_(key), value_(value) {} + + virtual ~DynamicValuesKeyAlreadyExists() throw() {} + + /// The duplicate key that was attemped to be added + const Symbol& key() const throw() { return key_; } + + /// The value that was attempted to be added + const Value& value() const throw() { return value_; } + + /// The message to be displayed to the user + virtual const char* what() const throw(); + }; + + /* ************************************************************************* */ + class DynamicValuesKeyDoesNotExist : public std::exception { + protected: + const char* operation_; ///< The operation that attempted to access the key + const Symbol key_; ///< The key that does not exist + + private: + mutable std::string message_; + + public: + /// Construct with the key that does not exist in the values + DynamicValuesKeyDoesNotExist(const char* operation, const Symbol& key) throw() : + operation_(operation), key_(key) {} + + virtual ~DynamicValuesKeyDoesNotExist() throw() {} + + /// The key that was attempted to be accessed that does not exist + const Symbol& key() const throw() { return key_; } + + /// The message to be displayed to the user + virtual const char* what() const throw(); + }; + + /* ************************************************************************* */ + class DynamicValuesIncorrectType : public std::exception { + protected: + const Symbol key_; ///< The key requested + const std::type_info& storedTypeId_; + const std::type_info& requestedTypeId_; + + private: + mutable std::string message_; + + public: + /// Construct with the key that does not exist in the values + DynamicValuesIncorrectType(const Symbol& key, + const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() : + key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {} + + virtual ~DynamicValuesIncorrectType() throw() {} + + /// The key that was attempted to be accessed that does not exist + const Symbol& key() const throw() { return key_; } + + /// The typeid of the value stores in the DynamicValues + const std::type_info& storedTypeId() const { return storedTypeId_; } + + /// The requested typeid + const std::type_info& requestedTypeId() const { return requestedTypeId_; } + + /// The message to be displayed to the user + virtual const char* what() const throw(); + }; + +} + +#include