Values.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * 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
 
 * -------------------------------------------------------------------------- */
 
#pragma once
 
#include <gtsam/base/FastDefaultAllocator.h>
#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>
#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() {}
  };
 
  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 our compile-flag-
    // dependent FastDefaultAllocator to allocate map nodes.  In this way, the
    // user defines the allocation details (i.e. optimize for memory pool/arenas
    // concurrency).
    typedef internal::FastDefaultAllocator<typename std::pair<const Key, void*>>::type KeyValuePtrPairAllocator;
    typedef boost::ptr_map<
        Key,
        Value,
        std::less<Key>,
        ValueCloneAllocator,
        KeyValuePtrPairAllocator > 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:
 
    typedef boost::shared_ptr<Values> shared_ptr;
 
    typedef boost::shared_ptr<const Values> const_shared_ptr;
 
    struct GTSAM_EXPORT KeyValuePair {
      const Key key; 
      Value& value;  
 
      KeyValuePair(Key _key, Value& _value) : key(_key), value(_value) {}
    };
 
    struct GTSAM_EXPORT ConstKeyValuePair {
      const Key key; 
      const Value& value;  
 
      ConstKeyValuePair(Key _key, const Value& _value) : key(_key), value(_value) {}
      ConstKeyValuePair(const KeyValuePair& kv) : key(kv.key), value(kv.value) {}
    };
 
    typedef boost::transform_iterator<
        std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::iterator> iterator;
 
    typedef boost::transform_iterator<
        std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_iterator> const_iterator;
 
    typedef boost::transform_iterator<
        std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::reverse_iterator> reverse_iterator;
 
    typedef boost::transform_iterator<
        std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
 
    typedef KeyValuePair value_type;
 
    template<class ValueType = Value>
    class Filtered;
 
    template<class ValueType = Value>
    class ConstFiltered;
 
    Values() {}
 
    Values(const Values& other);
 
    Values(Values&& other);
    
    Values(std::initializer_list<ConstKeyValuePair> init);
 
    Values(const Values& other, const VectorValues& delta);
 
    template<class ValueType>
    Values(const Filtered<ValueType>& view);
 
    template<class ValueType>
    Values(const ConstFiltered<ValueType>& view);
 
 
    void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
    bool equals(const Values& other, double tol=1e-9) const;
 
 
    template <typename ValueType>
    const ValueType at(Key j) const;
 
    double atDouble(size_t key) const { return at<double>(key);}
 
    const Value& at(Key j) const;
 
    bool exists(Key j) const;
 
    template<typename ValueType>
    boost::optional<const ValueType&> exists(Key j) const;
 
    iterator find(Key j) { return boost::make_transform_iterator(values_.find(j), &make_deref_pair); }
 
    const_iterator find(Key j) const { return boost::make_transform_iterator(values_.find(j), &make_const_deref_pair); }
 
    iterator lower_bound(Key j) { return boost::make_transform_iterator(values_.lower_bound(j), &make_deref_pair); }
 
    const_iterator lower_bound(Key j) const { return boost::make_transform_iterator(values_.lower_bound(j), &make_const_deref_pair); }
 
    iterator upper_bound(Key j) { return boost::make_transform_iterator(values_.upper_bound(j), &make_deref_pair); }
 
    const_iterator upper_bound(Key j) const { return boost::make_transform_iterator(values_.upper_bound(j), &make_const_deref_pair); }
 
    size_t size() const { return values_.size(); }
 
    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); }
 
 
    Values retract(const VectorValues& delta) const;
 
    VectorValues localCoordinates(const Values& cp) const;
 
 
    void insert(Key j, const Value& val);
 
    void insert(const Values& values);
 
    template <typename ValueType>
    void insert(Key j, const ValueType& val);
 
    void insertDouble(Key j, double c) { insert<double>(j,c); }
 
    std::pair<iterator, bool> tryInsert(Key j, const Value& value);
 
    void update(Key j, const Value& val);
 
    template <typename T>
    void update(Key j, const T& val);
 
    void update(const Values& values);
 
    void erase(Key j);
 
    KeyVector keys() const;
 
    Values& operator=(const Values& rhs);
 
    void swap(Values& other) { values_.swap(other.values_); }
 
    void clear() { values_.clear(); }
 
    size_t dim() const;
 
    VectorValues zeroVectors() const;
 
    Filtered<Value>
    filter(const std::function<bool(Key)>& filterFcn);
 
    template<class ValueType>
    Filtered<ValueType>
    filter(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>);
 
    ConstFiltered<Value>
    filter(const std::function<bool(Key)>& filterFcn) const;
 
    template<class ValueType>
    ConstFiltered<ValueType>
    filter(const std::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 std::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));
    }
 
    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_; 
 
  private:
    mutable std::string message_;
 
  public:
    ValuesKeyAlreadyExists(Key key) noexcept :
      key_(key) {}
 
    ~ValuesKeyAlreadyExists() noexcept override {}
 
    Key key() const noexcept { return key_; }
 
    GTSAM_EXPORT const char* what() const noexcept override;
  };
 
  /* ************************************************************************* */
  class ValuesKeyDoesNotExist : public std::exception {
  protected:
    const char* operation_; 
    const Key key_; 
 
  private:
    mutable std::string message_;
 
  public:
    ValuesKeyDoesNotExist(const char* operation, Key key) noexcept :
      operation_(operation), key_(key) {}
 
    ~ValuesKeyDoesNotExist() noexcept override {}
 
    Key key() const noexcept { return key_; }
 
    GTSAM_EXPORT const char* what() const noexcept override;
  };
 
  /* ************************************************************************* */
  class ValuesIncorrectType : public std::exception {
  protected:
    const Key key_; 
    const std::type_info& storedTypeId_;
    const std::type_info& requestedTypeId_;
 
  private:
    mutable std::string message_;
 
  public:
    ValuesIncorrectType(Key key,
        const std::type_info& storedTypeId, const std::type_info& requestedTypeId) noexcept :
      key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
 
    ~ValuesIncorrectType() noexcept override {}
 
    Key key() const noexcept { return key_; }
 
    const std::type_info& storedTypeId() const { return storedTypeId_; }
 
    const std::type_info& requestedTypeId() const { return requestedTypeId_; }
 
    GTSAM_EXPORT const char* what() const noexcept override;
  };
 
  /* ************************************************************************* */
  class DynamicValuesMismatched : public std::exception {
 
  public:
    DynamicValuesMismatched() noexcept {}
 
    ~DynamicValuesMismatched() noexcept override {}
 
    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) {
    }
 
    ~NoMatchFoundForFixed() noexcept override {
    }
 
    GTSAM_EXPORT const char* what() const noexcept override;
  };
 
  /* ************************************************************************* */
  template<>
  struct traits<Values> : public Testable<Values> {
  };
 
} //\ namespace gtsam
 
 
#include <gtsam/nonlinear/Values-inl.h>