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

552 lines
22 KiB
C++
Raw Blame History

/* ----------------------------------------------------------------------------
* 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 Values.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 <gtsam/base/GenericValue.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/inference/Key.h>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/filter_iterator.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/ptr_container/serialize_ptr_map.hpp>
#include <boost/shared_ptr.hpp>
#include <string>
#include <utility>
namespace gtsam {
// Forward declarations / utilities
class VectorValues;
class ValueAutomaticCasting;
template<typename T> static bool _truePredicate(const T&) { return true; }
/* ************************************************************************* */
class GTSAM_EXPORT ValueCloneAllocator {
public:
static Value* allocate_clone(const Value& a) { return a.clone_(); }
static void deallocate_clone(const Value* a) { a->deallocate_(); }
ValueCloneAllocator() {}
};
/**
* A non-templated config holding any types of Manifold-group elements. 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.
*/
class GTSAM_EXPORT Values {
private:
// Internally we store a boost ptr_map, with a ValueCloneAllocator (defined
// below) to clone and deallocate the Value objects, and a boost
// fast_pool_allocator to allocate map nodes. In this way, all memory is
// allocated in a boost memory pool.
typedef boost::ptr_map<
Key,
Value,
std::less<Key>,
ValueCloneAllocator,
boost::fast_pool_allocator<std::pair<const Key, void*> > > KeyValueMap;
// The member to store the values, see just above
KeyValueMap values_;
// Types obtained by iterating
typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair;
typedef KeyValueMap::iterator::value_type KeyValuePtrPair;
public:
/// A shared_ptr to this class
typedef boost::shared_ptr<Values> shared_ptr;
/// A const shared_ptr to this class
typedef boost::shared_ptr<const Values> const_shared_ptr;
/// A key-value pair, which you get by dereferencing iterators
struct GTSAM_EXPORT KeyValuePair {
const Key key; ///< The key
Value& value; ///< The value
KeyValuePair(Key _key, Value& _value) : key(_key), value(_value) {}
};
/// A key-value pair, which you get by dereferencing iterators
struct GTSAM_EXPORT ConstKeyValuePair {
const Key key; ///< The key
const Value& value; ///< The value
ConstKeyValuePair(Key _key, const Value& _value) : key(_key), value(_value) {}
ConstKeyValuePair(const KeyValuePair& kv) : key(kv.key), value(kv.value) {}
};
/// Mutable forward iterator, with value type KeyValuePair
typedef boost::transform_iterator<
boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::iterator> iterator;
/// Const forward iterator, with value type ConstKeyValuePair
typedef boost::transform_iterator<
boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_iterator> const_iterator;
/// Mutable reverse iterator, with value type KeyValuePair
typedef boost::transform_iterator<
boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::reverse_iterator> reverse_iterator;
/// Const reverse iterator, with value type ConstKeyValuePair
typedef boost::transform_iterator<
boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
typedef KeyValuePair value_type;
/** A filtered view of a Values, returned from Values::filter. */
template<class ValueType = Value>
class Filtered;
/** A filtered view of a const Values, returned from Values::filter. */
template<class ValueType = Value>
class ConstFiltered;
/** Default constructor creates an empty Values class */
Values() {}
/** Copy constructor duplicates all keys and values */
Values(const Values& other);
/** Move constructor */
Values(Values&& other);
/** Constructor from initializer list. Example usage:
* \code
* Values v = {{k1, genericValue(pose1)}, {k2, genericValue(point2)}};
* \endcode
*/
Values(std::initializer_list<ConstKeyValuePair> init);
/** Construct from a Values and an update vector: identical to other.retract(delta) */
Values(const Values& other, const VectorValues& delta);
/** Constructor from a Filtered view copies out all values */
template<class ValueType>
Values(const Filtered<ValueType>& view);
/** Constructor from a Filtered or ConstFiltered view */
template<class ValueType>
Values(const ConstFiltered<ValueType>& view);
/// @name Testable
/// @{
/** print method for testing and debugging */
void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** Test whether the sets of keys and values are identical */
bool equals(const Values& 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 ValueType The type of the value stored with this key, this method
* Throws DynamicValuesIncorrectType if this requested type is not correct.
* Dynamic matrices/vectors can be retrieved as fixed-size, but not vice-versa.
* @return The stored value
*/
template<typename ValueType>
ValueType at(Key j) const;
/// version for double
double atDouble(size_t key) const { return at<double>(key);}
/** Retrieve a variable by key \c j. This version returns a reference
* to the base Value class, and needs to be casted before use.
* @param j Retrieve the value associated with this key
* @return A const reference to the stored value
*/
const Value& at(Key j) const;
/** Check if a value exists with key \c j. See exists<>(Key j)
* and exists(const TypedKey& j) for versions that return the value if it
* exists. */
bool exists(Key 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.
* Throws DynamicValuesIncorrectType if the value type associated with the
* requested key does not match the stored value type. */
template<typename ValueType>
boost::optional<const ValueType&> exists(Key j) const;
/** Find an element by key, returning an iterator, or end() if the key was
* not found. */
iterator find(Key j) { return boost::make_transform_iterator(values_.find(j), &make_deref_pair); }
/** Find an element by key, returning an iterator, or end() if the key was
* not found. */
const_iterator find(Key j) const { return boost::make_transform_iterator(values_.find(j), &make_const_deref_pair); }
/** Find the element greater than or equal to the specified key. */
iterator lower_bound(Key j) { return boost::make_transform_iterator(values_.lower_bound(j), &make_deref_pair); }
/** Find the element greater than or equal to the specified key. */
const_iterator lower_bound(Key j) const { return boost::make_transform_iterator(values_.lower_bound(j), &make_const_deref_pair); }
/** Find the lowest-ordered element greater than the specified key. */
iterator upper_bound(Key j) { return boost::make_transform_iterator(values_.upper_bound(j), &make_deref_pair); }
/** Find the lowest-ordered element greater than the specified key. */
const_iterator upper_bound(Key j) const { return boost::make_transform_iterator(values_.upper_bound(j), &make_const_deref_pair); }
/** 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(); }
const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); }
const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); }
iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); }
iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); }
const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); }
const_reverse_iterator rend() const { return boost::make_transform_iterator(values_.rend(), &make_const_deref_pair); }
reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); }
reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); }
/// @name Manifold Operations
/// @{
/** Add a delta config to current config and returns a new config */
Values retract(const VectorValues& delta) const;
/** Get a delta config about a linearization point c0 (*this) */
VectorValues localCoordinates(const Values& cp) const;
///@}
/** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
void insert(Key j, const Value& val);
/** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
void insert(const Values& values);
/** Templated version to add a variable with the given j,
* throws KeyAlreadyExists<J> if j is already present
*/
template <typename ValueType>
void insert(Key j, const ValueType& val);
/// version for double
void insertDouble(Key j, double c) { insert<double>(j,c); }
/** insert that mimics the STL map insert - if the value already exists, the map is not modified
* and an iterator to the existing value is returned, along with 'false'. If the value did not
* exist, it is inserted and an iterator pointing to the new element, along with 'true', is
* returned. */
std::pair<iterator, bool> tryInsert(Key j, const Value& value);
/** single element change of existing element */
void update(Key j, const Value& val);
/** Templated version to update a variable with the given j,
* throws KeyDoesNotExist<J> if j is not present.
* If no chart is specified, the DefaultChart<ValueType> is used.
*/
template <typename T>
void update(Key j, const T& val);
/** update the current available values without adding new ones */
void update(const Values& values);
/** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
void erase(Key j);
/**
* Returns a set of keys in the config
* Note: by construction, the list is ordered
*/
KeyVector keys() const;
/** Replace all keys and variables */
Values& operator=(const Values& rhs);
/** Swap the contents of two Values without copying data */
void swap(Values& other) { values_.swap(other.values_); }
/** Remove all variables from the config */
void clear() { values_.clear(); }
/** Compute the total dimensionality of all values (\f$ O(n) \f$) */
size_t dim() const;
/** Return a VectorValues of zero vectors for each variable in this Values */
VectorValues zeroVectors() const;
/**
* Return a filtered view of this Values class, without copying any data.
* When iterating over the filtered view, only the key-value pairs
* with a key causing \c filterFcn to return \c true are visited. Because
* the object Filtered<Value> returned from filter() is only a
* <em>view</em> the original Values object must not be deallocated or
* go out of scope as long as the view is needed.
* @param filterFcn The function that determines which key-value pairs are
* included in the filtered view, for which this function returns \c true
* on their keys.
* @return A filtered view of the original Values class, which references
* the original Values class.
*/
Filtered<Value>
filter(const boost::function<bool(Key)>& filterFcn);
/**
* Return a filtered view of this Values class, without copying any data.
* In this templated version, only key-value pairs whose value matches the
* template argument \c ValueType and whose key causes the function argument
* \c filterFcn to return true are visited when iterating over the filtered
* view. Because the object Filtered<Value> returned from filter() is only
* a <em>view</em> the original Values object must not be deallocated or
* go out of scope as long as the view is needed.
* @tparam ValueType The type that the value in a key-value pair must match
* to be included in the filtered view. Currently, base classes are not
* resolved so the type must match exactly, except if ValueType = Value, in
* which case no type filtering is done.
* @param filterFcn The function that determines which key-value pairs are
* included in the filtered view, for which this function returns \c true
* on their keys (default: always return true so that filter() only
* filters by type, matching \c ValueType).
* @return A filtered view of the original Values class, which references
* the original Values class.
*/
template<class ValueType>
Filtered<ValueType>
filter(const boost::function<bool(Key)>& filterFcn = &_truePredicate<Key>);
/**
* Return a filtered view of this Values class, without copying any data.
* When iterating over the filtered view, only the key-value pairs
* with a key causing \c filterFcn to return \c true are visited. Because
* the object Filtered<Value> returned from filter() is only a
* <em>view</em> the original Values object must not be deallocated or
* go out of scope as long as the view is needed.
* @param filterFcn The function that determines which key-value pairs are
* included in the filtered view, for which this function returns \c true
* on their keys.
* @return A filtered view of the original Values class, which references
* the original Values class.
*/
ConstFiltered<Value>
filter(const boost::function<bool(Key)>& filterFcn) const;
/**
* Return a filtered view of this Values class, without copying any data.
* In this templated version, only key-value pairs whose value matches the
* template argument \c ValueType and whose key causes the function argument
* \c filterFcn to return true are visited when iterating over the filtered
* view. Because the object Filtered<Value> returned from filter() is only
* a <em>view</em> the original Values object must not be deallocated or
* go out of scope as long as the view is needed.
* @tparam ValueType The type that the value in a key-value pair must match
* to be included in the filtered view. Currently, base classes are not
* resolved so the type must match exactly, except if ValueType = Value, in
* which case no type filtering is done.
* @param filterFcn The function that determines which key-value pairs are
* included in the filtered view, for which this function returns \c true
* on their keys.
* @return A filtered view of the original Values class, which references
* the original Values class.
*/
template<class ValueType>
ConstFiltered<ValueType>
filter(const boost::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
// Count values of given type \c ValueType
template<class ValueType>
size_t count() const {
size_t i = 0;
for (const auto key_value : *this) {
if (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value))
++i;
}
return i;
}
private:
// Filters based on ValueType (if not Value) and also based on the user-
// supplied \c filter function.
template<class ValueType>
static bool filterHelper(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
BOOST_STATIC_ASSERT((!boost::is_same<ValueType, Value>::value));
// Filter and check the type
return filter(key_value.key) && (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value));
}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(values_);
}
static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) {
return ConstKeyValuePair(key_value.first, *key_value.second); }
static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) {
return KeyValuePair(key_value.first, *key_value.second); }
};
/* ************************************************************************* */
class ValuesKeyAlreadyExists : public std::exception {
protected:
const Key key_; ///< The key that already existed
private:
mutable std::string message_;
public:
/// Construct with the key-value pair attempted to be added
ValuesKeyAlreadyExists(Key key) noexcept :
key_(key) {}
virtual ~ValuesKeyAlreadyExists() noexcept {}
/// The duplicate key that was attempted to be added
Key key() const noexcept { return key_; }
/// The message to be displayed to the user
GTSAM_EXPORT const char* what() const noexcept override;
};
/* ************************************************************************* */
class ValuesKeyDoesNotExist : public std::exception {
protected:
const char* operation_; ///< The operation that attempted to access the key
const Key key_; ///< The key that does not exist
private:
mutable std::string message_;
public:
/// Construct with the key that does not exist in the values
ValuesKeyDoesNotExist(const char* operation, Key key) noexcept :
operation_(operation), key_(key) {}
virtual ~ValuesKeyDoesNotExist() noexcept {}
/// The key that was attempted to be accessed that does not exist
Key key() const noexcept { return key_; }
/// The message to be displayed to the user
GTSAM_EXPORT const char* what() const noexcept override;
};
/* ************************************************************************* */
class ValuesIncorrectType : public std::exception {
protected:
const Key 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
ValuesIncorrectType(Key key,
const std::type_info& storedTypeId, const std::type_info& requestedTypeId) noexcept :
key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
virtual ~ValuesIncorrectType() noexcept {}
/// The key that was attempted to be accessed that does not exist
Key key() const noexcept { return key_; }
/// The typeid of the value stores in the Values
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
GTSAM_EXPORT const char* what() const noexcept override;
};
/* ************************************************************************* */
class DynamicValuesMismatched : public std::exception {
public:
DynamicValuesMismatched() noexcept {}
virtual ~DynamicValuesMismatched() noexcept {}
const char* what() const noexcept override {
return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values";
}
};
/* ************************************************************************* */
class NoMatchFoundForFixed: public std::exception {
protected:
const size_t M1_, N1_;
const size_t M2_, N2_;
private:
mutable std::string message_;
public:
NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) noexcept :
M1_(M1), N1_(N1), M2_(M2), N2_(N2) {
}
virtual ~NoMatchFoundForFixed() noexcept {
}
GTSAM_EXPORT const char* what() const noexcept override;
};
/* ************************************************************************* */
/// traits
template<>
struct traits<Values> : public Testable<Values> {
};
} //\ namespace gtsam
#include <gtsam/nonlinear/Values-inl.h>
Reference in New Issue View Git Blame Copy Permalink