gtsam  4.1.0
gtsam
Values.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
25 #pragma once
26 
27 #include <gtsam/base/GenericValue.h>
28 #include <gtsam/base/VectorSpace.h>
29 #include <gtsam/inference/Key.h>
30 #include <boost/iterator/transform_iterator.hpp>
31 #include <boost/iterator/filter_iterator.hpp>
32 #ifdef __GNUC__
33 #pragma GCC diagnostic push
34 #pragma GCC diagnostic ignored "-Wunused-variable"
35 #pragma GCC diagnostic ignored "-Wunused-local-typedefs"
36 #endif
37 #include <boost/bind.hpp>
38 #ifdef __GNUC__
39 #pragma GCC diagnostic pop
40 #endif
41 #include <boost/ptr_container/serialize_ptr_map.hpp>
42 #include <boost/shared_ptr.hpp>
43 
44 #include <string>
45 #include <utility>
46 
47 namespace gtsam {
48 
49  // Forward declarations / utilities
50  class VectorValues;
51  class ValueAutomaticCasting;
52  template<typename T> static bool _truePredicate(const T&) { return true; }
53 
54  /* ************************************************************************* */
55  class GTSAM_EXPORT ValueCloneAllocator {
56  public:
57  static Value* allocate_clone(const Value& a) { return a.clone_(); }
58  static void deallocate_clone(const Value* a) { a->deallocate_(); }
60  };
61 
71  class GTSAM_EXPORT Values {
72 
73  private:
74 
75  // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined
76  // below) to clone and deallocate the Value objects, and a boost
77  // fast_pool_allocator to allocate map nodes. In this way, all memory is
78  // allocated in a boost memory pool.
79  typedef boost::ptr_map<
80  Key,
81  Value,
82  std::less<Key>,
84  boost::fast_pool_allocator<std::pair<const Key, void*> > > KeyValueMap;
85 
86  // The member to store the values, see just above
87  KeyValueMap values_;
88 
89  // Types obtained by iterating
90  typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair;
91  typedef KeyValueMap::iterator::value_type KeyValuePtrPair;
92 
93  public:
94 
96  typedef boost::shared_ptr<Values> shared_ptr;
97 
99  typedef boost::shared_ptr<const Values> const_shared_ptr;
100 
102  struct GTSAM_EXPORT KeyValuePair {
103  const Key key;
105 
106  KeyValuePair(Key _key, Value& _value) : key(_key), value(_value) {}
107  };
108 
110  struct GTSAM_EXPORT ConstKeyValuePair {
111  const Key key;
112  const Value& value;
113 
114  ConstKeyValuePair(Key _key, const Value& _value) : key(_key), value(_value) {}
115  ConstKeyValuePair(const KeyValuePair& kv) : key(kv.key), value(kv.value) {}
116  };
117 
119  typedef boost::transform_iterator<
120  boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::iterator> iterator;
121 
123  typedef boost::transform_iterator<
124  boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_iterator> const_iterator;
125 
127  typedef boost::transform_iterator<
128  boost::function1<KeyValuePair, const KeyValuePtrPair&>, KeyValueMap::reverse_iterator> reverse_iterator;
129 
131  typedef boost::transform_iterator<
132  boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
133 
134  typedef KeyValuePair value_type;
135 
137  template<class ValueType = Value>
138  class Filtered;
139 
141  template<class ValueType = Value>
142  class ConstFiltered;
143 
145  Values() {}
146 
148  Values(const Values& other);
149 
151  Values(Values&& other);
152 
158  Values(std::initializer_list<ConstKeyValuePair> init);
159 
161  Values(const Values& other, const VectorValues& delta);
162 
164  template<class ValueType>
165  Values(const Filtered<ValueType>& view);
166 
168  template<class ValueType>
169  Values(const ConstFiltered<ValueType>& view);
170 
173 
175  void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
176 
178  bool equals(const Values& other, double tol=1e-9) const;
179 
181 
190  template<typename ValueType>
191  ValueType at(Key j) const;
192 
194  double atDouble(size_t key) const { return at<double>(key);}
195 
201  const Value& at(Key j) const;
202 
206  bool exists(Key j) const;
207 
212  template<typename ValueType>
213  boost::optional<const ValueType&> exists(Key j) const;
214 
217  iterator find(Key j) { return boost::make_transform_iterator(values_.find(j), &make_deref_pair); }
218 
221  const_iterator find(Key j) const { return boost::make_transform_iterator(values_.find(j), &make_const_deref_pair); }
222 
224  iterator lower_bound(Key j) { return boost::make_transform_iterator(values_.lower_bound(j), &make_deref_pair); }
225 
227  const_iterator lower_bound(Key j) const { return boost::make_transform_iterator(values_.lower_bound(j), &make_const_deref_pair); }
228 
230  iterator upper_bound(Key j) { return boost::make_transform_iterator(values_.upper_bound(j), &make_deref_pair); }
231 
233  const_iterator upper_bound(Key j) const { return boost::make_transform_iterator(values_.upper_bound(j), &make_const_deref_pair); }
234 
236  size_t size() const { return values_.size(); }
237 
239  bool empty() const { return values_.empty(); }
240 
241  const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); }
242  const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); }
243  iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); }
244  iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); }
245  const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); }
246  const_reverse_iterator rend() const { return boost::make_transform_iterator(values_.rend(), &make_const_deref_pair); }
247  reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); }
248  reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); }
249 
252 
254  Values retract(const VectorValues& delta) const;
255 
257  VectorValues localCoordinates(const Values& cp) const;
258 
260 
262  void insert(Key j, const Value& val);
263 
265  void insert(const Values& values);
266 
270  template <typename ValueType>
271  void insert(Key j, const ValueType& val);
272 
274  void insertDouble(Key j, double c) { insert<double>(j,c); }
275 
280  std::pair<iterator, bool> tryInsert(Key j, const Value& value);
281 
283  void update(Key j, const Value& val);
284 
289  template <typename T>
290  void update(Key j, const T& val);
291 
293  void update(const Values& values);
294 
296  void erase(Key j);
297 
302  KeyVector keys() const;
303 
305  Values& operator=(const Values& rhs);
306 
308  void swap(Values& other) { values_.swap(other.values_); }
309 
311  void clear() { values_.clear(); }
312 
314  size_t dim() const;
315 
317  VectorValues zeroVectors() const;
318 
332  Filtered<Value>
333  filter(const boost::function<bool(Key)>& filterFcn);
334 
354  template<class ValueType>
355  Filtered<ValueType>
356  filter(const boost::function<bool(Key)>& filterFcn = &_truePredicate<Key>);
357 
371  ConstFiltered<Value>
372  filter(const boost::function<bool(Key)>& filterFcn) const;
373 
392  template<class ValueType>
393  ConstFiltered<ValueType>
394  filter(const boost::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
395 
396  // Count values of given type \c ValueType
397  template<class ValueType>
398  size_t count() const {
399  size_t i = 0;
400  for (const auto key_value : *this) {
401  if (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value))
402  ++i;
403  }
404  return i;
405  }
406 
407  private:
408  // Filters based on ValueType (if not Value) and also based on the user-
409  // supplied \c filter function.
410  template<class ValueType>
411  static bool filterHelper(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
412  BOOST_STATIC_ASSERT((!boost::is_same<ValueType, Value>::value));
413  // Filter and check the type
414  return filter(key_value.key) && (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value));
415  }
416 
418  friend class boost::serialization::access;
419  template<class ARCHIVE>
420  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
421  ar & BOOST_SERIALIZATION_NVP(values_);
422  }
423 
424  static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) {
425  return ConstKeyValuePair(key_value.first, *key_value.second); }
426 
427  static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) {
428  return KeyValuePair(key_value.first, *key_value.second); }
429 
430  };
431 
432  /* ************************************************************************* */
433  class ValuesKeyAlreadyExists : public std::exception {
434  protected:
435  const Key key_;
436 
437  private:
438  mutable std::string message_;
439 
440  public:
443  key_(key) {}
444 
445  virtual ~ValuesKeyAlreadyExists() noexcept {}
446 
448  Key key() const noexcept { return key_; }
449 
451  GTSAM_EXPORT const char* what() const noexcept override;
452  };
453 
454  /* ************************************************************************* */
455  class ValuesKeyDoesNotExist : public std::exception {
456  protected:
457  const char* operation_;
458  const Key key_;
459 
460  private:
461  mutable std::string message_;
462 
463  public:
465  ValuesKeyDoesNotExist(const char* operation, Key key) noexcept :
466  operation_(operation), key_(key) {}
467 
468  virtual ~ValuesKeyDoesNotExist() noexcept {}
469 
471  Key key() const noexcept { return key_; }
472 
474  GTSAM_EXPORT const char* what() const noexcept override;
475  };
476 
477  /* ************************************************************************* */
478  class ValuesIncorrectType : public std::exception {
479  protected:
480  const Key key_;
481  const std::type_info& storedTypeId_;
482  const std::type_info& requestedTypeId_;
483 
484  private:
485  mutable std::string message_;
486 
487  public:
490  const std::type_info& storedTypeId, const std::type_info& requestedTypeId) noexcept :
491  key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
492 
493  virtual ~ValuesIncorrectType() noexcept {}
494 
496  Key key() const noexcept { return key_; }
497 
499  const std::type_info& storedTypeId() const { return storedTypeId_; }
500 
502  const std::type_info& requestedTypeId() const { return requestedTypeId_; }
503 
505  GTSAM_EXPORT const char* what() const noexcept override;
506  };
507 
508  /* ************************************************************************* */
509  class DynamicValuesMismatched : public std::exception {
510 
511  public:
512  DynamicValuesMismatched() noexcept {}
513 
514  virtual ~DynamicValuesMismatched() noexcept {}
515 
516  const char* what() const noexcept override {
517  return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values";
518  }
519  };
520 
521  /* ************************************************************************* */
522  class NoMatchFoundForFixed: public std::exception {
523 
524  protected:
525  const size_t M1_, N1_;
526  const size_t M2_, N2_;
527 
528  private:
529  mutable std::string message_;
530 
531  public:
532  NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) noexcept :
533  M1_(M1), N1_(N1), M2_(M2), N2_(N2) {
534  }
535 
536  virtual ~NoMatchFoundForFixed() noexcept {
537  }
538 
539  GTSAM_EXPORT const char* what() const noexcept override;
540  };
541 
542  /* ************************************************************************* */
544  template<>
545  struct traits<Values> : public Testable<Values> {
546  };
547 
548 } //\ namespace gtsam
549 
550 
551 #include <gtsam/nonlinear/Values-inl.h>
gtsam::NoMatchFoundForFixed
Definition: Values.h:522
gtsam::Values::lower_bound
const_iterator lower_bound(Key j) const
Find the element greater than or equal to the specified key.
Definition: Values.h:227
gtsam::Values::atDouble
double atDouble(size_t key) const
version for double
Definition: Values.h:194
gtsam::Values::upper_bound
iterator upper_bound(Key j)
Find the lowest-ordered element greater than the specified key.
Definition: Values.h:230
gtsam::ValuesKeyDoesNotExist::operation_
const char * operation_
The operation that attempted to access the key.
Definition: Values.h:457
gtsam::ValuesKeyDoesNotExist
Definition: Values.h:455
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::Values::clear
void clear()
Remove all variables from the config.
Definition: Values.h:311
gtsam::Value
This is the base class for any type to be stored in Values.
Definition: Value.h:36
gtsam::Values::ConstFiltered
A filtered view of a const Values, returned from Values::filter.
Definition: Values-inl.h:167
gtsam::Values::ConstKeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition: Values.h:110
gtsam::Values::find
const_iterator find(Key j) const
Find an element by key, returning an iterator, or end() if the key was not found.
Definition: Values.h:221
gtsam::ValuesIncorrectType::storedTypeId
const std::type_info & storedTypeId() const
The typeid of the value stores in the Values.
Definition: Values.h:499
gtsam::ValueCloneAllocator
Definition: Values.h:55
gtsam::Values::KeyValuePair::key
const Key key
The key.
Definition: Values.h:103
gtsam::DynamicValuesMismatched
Definition: Values.h:509
gtsam::GenericValue
Wraps any type T so it can play as a Value.
Definition: GenericValue.h:47
gtsam::Values::size
size_t size() const
The number of variables in this config.
Definition: Values.h:236
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Values::empty
bool empty() const
whether the config is empty
Definition: Values.h:239
gtsam::Value::clone_
virtual Value * clone_() const =0
Clone this value in a special memory pool, must be deleted with Value::deallocate_,...
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::ValuesKeyAlreadyExists::ValuesKeyAlreadyExists
ValuesKeyAlreadyExists(Key key) noexcept
Construct with the key-value pair attempted to be added.
Definition: Values.h:442
gtsam::Values::KeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition: Values.h:102
gtsam::ValuesKeyDoesNotExist::key
Key key() const noexcept
The key that was attempted to be accessed that does not exist.
Definition: Values.h:471
gtsam::Values::shared_ptr
boost::shared_ptr< Values > shared_ptr
A shared_ptr to this class.
Definition: Values.h:96
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::ValuesKeyAlreadyExists::key
Key key() const noexcept
The duplicate key that was attempted to be added.
Definition: Values.h:448
gtsam::Values::ConstKeyValuePair::key
const Key key
The key.
Definition: Values.h:111
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Values::const_shared_ptr
boost::shared_ptr< const Values > const_shared_ptr
A const shared_ptr to this class.
Definition: Values.h:99
gtsam::Values::const_iterator
boost::transform_iterator< boost::function1< ConstKeyValuePair, const ConstKeyValuePtrPair & >, KeyValueMap::const_iterator > const_iterator
Const forward iterator, with value type ConstKeyValuePair.
Definition: Values.h:124
gtsam::Values::upper_bound
const_iterator upper_bound(Key j) const
Find the lowest-ordered element greater than the specified key.
Definition: Values.h:233
gtsam::Values::insertDouble
void insertDouble(Key j, double c)
version for double
Definition: Values.h:274
gtsam::Values::swap
void swap(Values &other)
Swap the contents of two Values without copying data.
Definition: Values.h:308
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::Values::find
iterator find(Key j)
Find an element by key, returning an iterator, or end() if the key was not found.
Definition: Values.h:217
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::Values::reverse_iterator
boost::transform_iterator< boost::function1< KeyValuePair, const KeyValuePtrPair & >, KeyValueMap::reverse_iterator > reverse_iterator
Mutable reverse iterator, with value type KeyValuePair.
Definition: Values.h:128
gtsam::ValuesIncorrectType::key
Key key() const noexcept
The key that was attempted to be accessed that does not exist.
Definition: Values.h:496
gtsam::Values::update
void update(Key j, const T &val)
Templated version to update a variable with the given j, throws KeyDoesNotExist<J> if j is not presen...
gtsam::ValuesKeyAlreadyExists::key_
const Key key_
The key that already existed.
Definition: Values.h:435
gtsam::Values::Filtered
A filtered view of a Values, returned from Values::filter.
Definition: Values-inl.h:93
gtsam::ValuesKeyAlreadyExists
Definition: Values.h:433
Key.h
gtsam::ValuesKeyAlreadyExists::what
GTSAM_EXPORT const char * what() const noexcept override
The message to be displayed to the user.
Definition: Values.cpp:224
gtsam::Values::KeyValuePair::value
Value & value
The value.
Definition: Values.h:104
gtsam::KeyFormatter
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::ValuesKeyDoesNotExist::key_
const Key key_
The key that does not exist.
Definition: Values.h:458
gtsam::ValuesIncorrectType
Definition: Values.h:478
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::Values::Values
Values()
Default constructor creates an empty Values class.
Definition: Values.h:145
gtsam::ValuesIncorrectType::key_
const Key key_
The key requested.
Definition: Values.h:480
gtsam::Values::const_reverse_iterator
boost::transform_iterator< boost::function1< ConstKeyValuePair, const ConstKeyValuePtrPair & >, KeyValueMap::const_reverse_iterator > const_reverse_iterator
Const reverse iterator, with value type ConstKeyValuePair.
Definition: Values.h:132
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::ValuesKeyDoesNotExist::ValuesKeyDoesNotExist
ValuesKeyDoesNotExist(const char *operation, Key key) noexcept
Construct with the key that does not exist in the values.
Definition: Values.h:465
gtsam::Values::lower_bound
iterator lower_bound(Key j)
Find the element greater than or equal to the specified key.
Definition: Values.h:224
gtsam::ValuesIncorrectType::requestedTypeId
const std::type_info & requestedTypeId() const
The requested typeid.
Definition: Values.h:502
gtsam::Values::iterator
boost::transform_iterator< boost::function1< KeyValuePair, const KeyValuePtrPair & >, KeyValueMap::iterator > iterator
Mutable forward iterator, with value type KeyValuePair.
Definition: Values.h:120
gtsam::ValuesIncorrectType::ValuesIncorrectType
ValuesIncorrectType(Key key, const std::type_info &storedTypeId, const std::type_info &requestedTypeId) noexcept
Construct with the key that does not exist in the values.
Definition: Values.h:489
gtsam::Values::ConstKeyValuePair::value
const Value & value
The value.
Definition: Values.h:112
gtsam::Value::deallocate_
virtual void deallocate_() const =0
Deallocate a raw pointer of this value.