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/inference/Key.h>
30 #include <gtsam/base/VectorSpace.h>
31 
32 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
33 #include <boost/serialization/unique_ptr.hpp>
34 #endif
35 
36 
37 #include <memory>
38 #include <string>
39 #include <utility>
40 
41 namespace gtsam {
42 
43  // Forward declarations / utilities
44  class VectorValues;
45  class ValueAutomaticCasting;
46  template<typename T> static bool _truePredicate(const T&) { return true; }
47 
48  /* ************************************************************************* */
49  class GTSAM_EXPORT ValueCloneAllocator {
50  public:
51  static Value* allocate_clone(const Value& a) { return a.clone_(); }
52  static void deallocate_clone(const Value* a) { a->deallocate_(); }
54  };
55 
65  class GTSAM_EXPORT Values {
66 
67  private:
68  // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined
69  // below) to clone and deallocate the Value objects, and our compile-flag-
70  // dependent FastDefaultAllocator to allocate map nodes. In this way, the
71  // user defines the allocation details (i.e. optimize for memory pool/arenas
72  // concurrency).
74  using KeyValueMap =
75  std::map<Key, std::unique_ptr<Value>, std::less<Key>,
76  std::allocator<std::pair<const Key, std::unique_ptr<Value>>>>;
77 
78  // The member to store the values, see just above
80 
81  public:
82 
84  typedef std::shared_ptr<Values> shared_ptr;
85 
87  typedef std::shared_ptr<const Values> const_shared_ptr;
88 
90  struct GTSAM_EXPORT KeyValuePair {
91  const Key key;
93 
94  KeyValuePair(Key _key, Value& _value) : key(_key), value(_value) {}
95  };
96 
98  struct GTSAM_EXPORT ConstKeyValuePair {
99  const Key key;
100  const Value& value;
101 
102  ConstKeyValuePair(Key _key, const Value& _value) : key(_key), value(_value) {}
103  ConstKeyValuePair(const KeyValuePair& kv) : key(kv.key), value(kv.value) {}
104  };
105 
107 
110 
112  Values() = default;
113 
115  Values(const Values& other);
116 
118  Values(Values&& other);
119 
125  Values(std::initializer_list<ConstKeyValuePair> init);
126 
128  Values(const Values& other, const VectorValues& delta);
129 
133 
135  void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
136 
138  bool equals(const Values& other, double tol=1e-9) const;
139 
143 
152  template <typename ValueType>
153  const ValueType at(Key j) const;
154 
156  double atDouble(size_t key) const { return at<double>(key);}
157 
163  const Value& at(Key j) const;
164 
168  bool exists(Key j) const;
169 
174  template<typename ValueType>
175  const ValueType * exists(Key j) const;
176 
178  size_t size() const { return values_.size(); }
179 
181  bool empty() const { return values_.empty(); }
182 
186 
187  struct deref_iterator {
188  using const_iterator_type = typename KeyValueMap::const_iterator;
191  ConstKeyValuePair operator*() const { return {it_->first, *(it_->second)}; }
192  std::unique_ptr<ConstKeyValuePair> operator->() {
193  return std::make_unique<ConstKeyValuePair>(it_->first, *(it_->second));
194  }
195  bool operator==(const deref_iterator& other) const {
196  return it_ == other.it_;
197  }
198  bool operator!=(const deref_iterator& other) const { return it_ != other.it_; }
200  ++it_;
201  return *this;
202  }
203  };
204 
205  deref_iterator begin() const { return deref_iterator(values_.begin()); }
206  deref_iterator end() const { return deref_iterator(values_.end()); }
207 
210  deref_iterator find(Key j) const { return deref_iterator(values_.find(j)); }
211 
213  deref_iterator lower_bound(Key j) const { return deref_iterator(values_.lower_bound(j)); }
214 
216  deref_iterator upper_bound(Key j) const { return deref_iterator(values_.upper_bound(j)); }
217 
221 
223  Values retract(const VectorValues& delta) const;
224 
229  void retractMasked(const VectorValues& delta, const KeySet& mask);
230 
232  VectorValues localCoordinates(const Values& cp) const;
233 
235 
237  void insert(Key j, const Value& val);
238 
240  void insert(const Values& values);
241 
245  template <typename ValueType>
246  void insert(Key j, const ValueType& val);
247 
257  template <typename UnaryOp, typename ValueType>
259 
270  template <typename BinaryOp, typename ValueType1, typename ValueType2>
272 
274  void insertDouble(Key j, double c) { insert<double>(j,c); }
275 
277  void update(Key j, const Value& val);
278 
283  template <typename T>
284  void update(Key j, const T& val);
285 
289  template <typename UnaryOp, typename ValueType>
291 
295  template <typename BinaryOp, typename ValueType1, typename ValueType2>
297 
299  void update(const Values& values);
300 
302  void insert_or_assign(Key j, const Value& val);
303 
308  void insert_or_assign(const Values& values);
309 
311  template <typename ValueType>
312  void insert_or_assign(Key j, const ValueType& val);
313 
317  template <typename UnaryOp, typename ValueType>
318  void insert_or_assign(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val);
319 
323  template <typename BinaryOp, typename ValueType1, typename ValueType2>
325 
327  void erase(Key j);
328 
333  KeyVector keys() const;
334 
338  KeySet keySet() const;
339 
341  Values& operator=(const Values& rhs);
342 
344  void swap(Values& other) { values_.swap(other.values_); }
345 
347  void clear() { values_.clear(); }
348 
350  size_t dim() const;
351 
353  std::map<Key,size_t> dims() const;
354 
356  VectorValues zeroVectors() const;
357 
358  // Count values of given type \c ValueType
359  template <class ValueType>
360  size_t count() const;
361 
381  template <class ValueType>
382  std::map<Key, ValueType> // , std::less<Key>, Eigen::aligned_allocator<ValueType>
383  extract(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
384 
385  private:
386  // Filters based on ValueType (if not Value) and also based on the user-
387  // supplied \c filter function.
388  template<class ValueType>
389  static bool filterHelper(const std::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
390  // static_assert if ValueType is type: Value
391  static_assert(!std::is_same<Value, ValueType>::value, "ValueType must not be type: Value to use this filter");
392  // Filter and check the type
393  return filter(key_value.key) && (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value));
394  }
395 
396 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
397 
398  friend class boost::serialization::access;
399  template<class ARCHIVE>
400  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
401  ar & BOOST_SERIALIZATION_NVP(values_);
402  }
403 #endif
404 
405  };
406 
407  /* ************************************************************************* */
408  class ValuesKeyAlreadyExists : public std::exception {
409  protected:
410  const Key key_;
411 
412  private:
413  mutable std::string message_;
414 
415  public:
418  key_(key) {}
419 
420  ~ValuesKeyAlreadyExists() noexcept override {}
421 
423  Key key() const noexcept { return key_; }
424 
426  GTSAM_EXPORT const char* what() const noexcept override;
427  };
428 
429  /* ************************************************************************* */
431  protected:
432  const char* operation_;
433  const Key key_;
434 
435  private:
436  mutable std::string message_;
437 
438  public:
440  ValuesKeyDoesNotExist(const char* operation, Key key) noexcept :
441  operation_(operation), key_(key) {}
442 
443  ~ValuesKeyDoesNotExist() noexcept override {}
444 
446  Key key() const noexcept { return key_; }
447 
449  GTSAM_EXPORT const char* what() const noexcept override;
450  };
451 
452  /* ************************************************************************* */
454  protected:
455  const Key key_;
456  const std::type_info& storedTypeId_;
457  const std::type_info& requestedTypeId_;
458 
459  private:
460  mutable std::string message_;
461 
462  public:
465  const std::type_info& storedTypeId, const std::type_info& requestedTypeId) noexcept :
466  key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
467 
468  ~ValuesIncorrectType() noexcept override {}
469 
471  Key key() const noexcept { return key_; }
472 
474  const std::type_info& storedTypeId() const { return storedTypeId_; }
475 
477  const std::type_info& requestedTypeId() const { return requestedTypeId_; }
478 
480  GTSAM_EXPORT const char* what() const noexcept override;
481  };
482 
483  /* ************************************************************************* */
485 
486  public:
488 
489  ~DynamicValuesMismatched() noexcept override {}
490 
491  const char* what() const noexcept override {
492  return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values";
493  }
494  };
495 
496  /* ************************************************************************* */
497  class NoMatchFoundForFixed: public std::exception {
498 
499  protected:
500  const size_t M1_, N1_;
501  const size_t M2_, N2_;
502 
503  private:
504  mutable std::string message_;
505 
506  public:
507  NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) noexcept :
508  M1_(M1), N1_(N1), M2_(M2), N2_(N2) {
509  }
510 
511  ~NoMatchFoundForFixed() noexcept override {
512  }
513 
514  GTSAM_EXPORT const char* what() const noexcept override;
515  };
516 
517  /* ************************************************************************* */
519  template<>
520  struct traits<Values> : public Testable<Values> {
521  };
522 
523 } //\ namespace gtsam
524 
525 
VectorValues
gtsam::Values::deref_iterator::operator==
bool operator==(const deref_iterator &other) const
Definition: Values.h:195
gtsam::NoMatchFoundForFixed::N2_
const size_t N2_
Definition: Values.h:501
relicense.update
def update(text)
Definition: relicense.py:46
M2
M1<< 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12;Map< MatrixXf > M2(M1.data(), 6, 2)
gtsam::ValuesIncorrectType::storedTypeId
const std::type_info & storedTypeId() const
The typeid of the value stores in the Values.
Definition: Values.h:474
gtsam::DynamicValuesMismatched
Definition: Values.h:484
gtsam::Values::deref_iterator::const_iterator_type
typename KeyValueMap::const_iterator const_iterator_type
Definition: Values.h:188
gtsam::ValuesIncorrectType::requestedTypeId
const std::type_info & requestedTypeId() const
The requested typeid.
Definition: Values.h:477
gtsam::Values::size
size_t size() const
Definition: Values.h:178
Eigen::CwiseBinaryOp
Generic expression where a coefficient-wise binary operator is applied to two expressions.
Definition: CwiseBinaryOp.h:77
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Values::atDouble
double atDouble(size_t key) const
version for double
Definition: Values.h:156
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::ValuesIncorrectType
Definition: Values.h:453
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::ValuesKeyDoesNotExist::operation_
const char * operation_
The operation that attempted to access the key.
Definition: Values.h:432
gtsam::DynamicValuesMismatched::~DynamicValuesMismatched
~DynamicValuesMismatched() noexcept override
Definition: Values.h:489
gtsam::Values::shared_ptr
std::shared_ptr< Values > shared_ptr
A shared_ptr to this class.
Definition: Values.h:84
gtsam::Values::lower_bound
deref_iterator lower_bound(Key j) const
Definition: Values.h:213
gtsam::Values::ConstKeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition: Values.h:98
gtsam::DynamicValuesMismatched::what
const char * what() const noexcept override
Definition: Values.h:491
type
Definition: pytypes.h:1491
gtsam::Values::KeyValuePair::key
const Key key
The key.
Definition: Values.h:91
gtsam::ValueCloneAllocator::ValueCloneAllocator
ValueCloneAllocator()
Definition: Values.h:53
gtsam::ValueCloneAllocator::allocate_clone
static Value * allocate_clone(const Value &a)
Definition: Values.h:51
gtsam::FastSet< Key >
gtsam::Values::ConstKeyValuePair::ConstKeyValuePair
ConstKeyValuePair(Key _key, const Value &_value)
Definition: Values.h:102
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::Values::values_
KeyValueMap values_
Definition: Values.h:79
gtsam::ValuesIncorrectType::message_
std::string message_
Definition: Values.h:460
gtsam::Values::deref_iterator::operator!=
bool operator!=(const deref_iterator &other) const
Definition: Values.h:198
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::Values::upper_bound
deref_iterator upper_bound(Key j) const
Definition: Values.h:216
gtsam::GenericValue
Definition: GenericValue.h:44
gtsam::internal::FastDefaultAllocator
Default allocator for list, map, and set types.
Definition: FastDefaultAllocator.h:49
gtsam::ValuesIncorrectType::key
Key key() const noexcept
The key that was attempted to be accessed that does not exist.
Definition: Values.h:471
gtsam::Values::insertDouble
void insertDouble(Key j, double c)
version for double
Definition: Values.h:274
Key.h
gtsam::Values::KeyValueMap
std::map< Key, std::unique_ptr< Value >, std::less< Key >, std::allocator< std::pair< const Key, std::unique_ptr< Value > >> > KeyValueMap
Definition: Values.h:76
gtsam::Values::deref_iterator::operator++
deref_iterator & operator++()
Definition: Values.h:199
gtsam::NoMatchFoundForFixed::~NoMatchFoundForFixed
~NoMatchFoundForFixed() noexcept override
Definition: Values.h:511
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::Values::KeyValuePtrPairAllocator
internal::FastDefaultAllocator< typename std::pair< const Key, void * > >::type KeyValuePtrPairAllocator
Definition: Values.h:73
GenericValue.h
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::ValuesKeyAlreadyExists::message_
std::string message_
Definition: Values.h:413
gtsam::Values::end
deref_iterator end() const
Definition: Values.h:206
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::ValuesKeyDoesNotExist::key_
const Key key_
The key that does not exist.
Definition: Values.h:433
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::Values::const_shared_ptr
std::shared_ptr< const Values > const_shared_ptr
A const shared_ptr to this class.
Definition: Values.h:87
gtsam::_truePredicate
static bool _truePredicate(const T &)
Definition: Values.h:46
gtsam::NoMatchFoundForFixed
Definition: Values.h:497
gtsam::KeyFormatter
std::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::ValuesKeyAlreadyExists::key_
const Key key_
The key that already existed.
Definition: Values.h:410
gtsam::Values::KeyValuePair::KeyValuePair
KeyValuePair(Key _key, Value &_value)
Definition: Values.h:94
gtsam::Values::deref_iterator::it_
const_iterator_type it_
Definition: Values.h:189
gtsam::Value
Definition: Value.h:39
gtsam::Values::swap
void swap(Values &other)
Definition: Values.h:344
gtsam::Values::deref_iterator::operator*
ConstKeyValuePair operator*() const
Definition: Values.h:191
Eigen::Triplet
A small structure to hold a non zero as a triplet (i,j,value).
Definition: SparseUtil.h:162
gtsam::Values::deref_iterator::operator->
std::unique_ptr< ConstKeyValuePair > operator->()
Definition: Values.h:192
M1
MatrixXf M1
Definition: Tutorial_SlicingCol.cpp:1
gtsam::ValueCloneAllocator
Definition: Values.h:49
gtsam::Values::KeyValuePair::value
Value & value
The value.
Definition: Values.h:92
gtsam::ValuesKeyAlreadyExists
Definition: Values.h:408
gtsam::equals
Definition: Testable.h:112
gtsam::NoMatchFoundForFixed::NoMatchFoundForFixed
NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) noexcept
Definition: Values.h:507
str
Definition: pytypes.h:1524
key
const gtsam::Symbol key('X', 0)
gtsam::Values::begin
deref_iterator begin() const
Definition: Values.h:205
gtsam::ValueCloneAllocator::deallocate_clone
static void deallocate_clone(const Value *a)
Definition: Values.h:52
gtsam::ValuesKeyDoesNotExist::~ValuesKeyDoesNotExist
~ValuesKeyDoesNotExist() noexcept override
Definition: Values.h:443
gtsam::Values::KeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition: Values.h:90
gtsam::ValuesKeyAlreadyExists::ValuesKeyAlreadyExists
ValuesKeyAlreadyExists(Key key) noexcept
Construct with the key-value pair attempted to be added.
Definition: Values.h:417
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
Values
std::vector< float > Values
Definition: sparse_setter.cpp:45
gtsam
traits
Definition: chartTesting.h:28
gtsam::Values::ConstKeyValuePair::ConstKeyValuePair
ConstKeyValuePair(const KeyValuePair &kv)
Definition: Values.h:103
gtsam::Testable
Definition: Testable.h:152
gtsam::NoMatchFoundForFixed::N1_
const size_t N1_
Definition: Values.h:500
gtsam::ValuesIncorrectType::requestedTypeId_
const std::type_info & requestedTypeId_
Definition: Values.h:457
gtsam::traits
Definition: Group.h:36
gtsam::ValuesIncorrectType::~ValuesIncorrectType
~ValuesIncorrectType() noexcept override
Definition: Values.h:468
gtsam::Values::deref_iterator
Definition: Values.h:187
leaf::values
leaf::MyValues values
gtsam::Values
Definition: Values.h:65
Eigen::CwiseUnaryOp
Generic expression where a coefficient-wise unary operator is applied to an expression.
Definition: CwiseUnaryOp.h:55
gtsam::ValuesKeyAlreadyExists::what
const GTSAM_EXPORT char * what() const noexcept override
The message to be displayed to the user.
Definition: Values.cpp:268
std
Definition: BFloat16.h:88
gtsam::Values::clear
void clear()
Definition: Values.h:347
gtsam::Values::ConstKeyValuePair::value
const Value & value
The value.
Definition: Values.h:100
gtsam::ValuesKeyDoesNotExist::key
Key key() const noexcept
The key that was attempted to be accessed that does not exist.
Definition: Values.h:446
gtsam::ValuesKeyDoesNotExist
Definition: Values.h:430
gtsam::ValuesKeyAlreadyExists::~ValuesKeyAlreadyExists
~ValuesKeyAlreadyExists() noexcept override
Definition: Values.h:420
gtsam::Values::filterHelper
static bool filterHelper(const std::function< bool(Key)> filter, const ConstKeyValuePair &key_value)
Definition: Values.h:389
gtsam::ValuesKeyAlreadyExists::key
Key key() const noexcept
The duplicate key that was attempted to be added.
Definition: Values.h:423
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:464
gtsam::Values::value_type
KeyValuePair value_type
Definition: Values.h:106
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::ValuesKeyDoesNotExist::message_
std::string message_
Definition: Values.h:436
gtsam::ValuesIncorrectType::key_
const Key key_
The key requested.
Definition: Values.h:455
FastDefaultAllocator.h
An easy way to control which allocator is used for Fast* collections.
gtsam::NoMatchFoundForFixed::message_
std::string message_
Definition: Values.h:504
gtsam::ValuesIncorrectType::storedTypeId_
const std::type_info & storedTypeId_
Definition: Values.h:456
gtsam::DynamicValuesMismatched::DynamicValuesMismatched
DynamicValuesMismatched() noexcept
Definition: Values.h:487
gtsam::Values::empty
bool empty() const
Definition: Values.h:181
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
Values-inl.h
insert
A insert(1, 2)=0
init
Definition: TutorialInplaceLU.cpp:2
gtsam::ValuesKeyDoesNotExist::ValuesKeyDoesNotExist
ValuesKeyDoesNotExist(const char *operation, Key key) noexcept
Construct with the key that does not exist in the values.
Definition: Values.h:440
VectorSpace.h
test_callbacks.value
value
Definition: test_callbacks.py:158
exception
Definition: pybind11.h:2536
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
gtsam::Values::deref_iterator::deref_iterator
deref_iterator(const_iterator_type it)
Definition: Values.h:190
gtsam::Values::ConstKeyValuePair::key
const Key key
The key.
Definition: Values.h:99
gtsam::Values::find
deref_iterator find(Key j) const
Definition: Values.h:210


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:11:31