gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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
28#include <gtsam/base/GenericValue.h>
29#include <gtsam/base/VectorSpace.h>
30#include <gtsam/inference/Key.h>
31#include <boost/ptr_container/serialize_ptr_map.hpp>
32#include <boost/shared_ptr.hpp>
33#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
34#include <boost/iterator/transform_iterator.hpp>
35#include <boost/iterator/filter_iterator.hpp>
36#endif
37
38#include <string>
39#include <utility>
40
41namespace 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).
73 typedef internal::FastDefaultAllocator<typename std::pair<const Key, void*>>::type KeyValuePtrPairAllocator;
74 typedef boost::ptr_map<
75 Key,
76 Value,
77 std::less<Key>,
79 KeyValuePtrPairAllocator > KeyValueMap;
80
81 // The member to store the values, see just above
82 KeyValueMap values_;
83
84 public:
85
87 typedef boost::shared_ptr<Values> shared_ptr;
88
90 typedef boost::shared_ptr<const Values> const_shared_ptr;
91
93 struct GTSAM_EXPORT KeyValuePair {
94 const Key key;
96
97 KeyValuePair(Key _key, Value& _value) : key(_key), value(_value) {}
98 };
99
101 struct GTSAM_EXPORT ConstKeyValuePair {
102 const Key key;
103 const Value& value;
104
105 ConstKeyValuePair(Key _key, const Value& _value) : key(_key), value(_value) {}
106 ConstKeyValuePair(const KeyValuePair& kv) : key(kv.key), value(kv.value) {}
107 };
108
109 typedef KeyValuePair value_type;
110
113
115 Values() = default;
116
118 Values(const Values& other);
119
121 Values(Values&& other);
122
128 Values(std::initializer_list<ConstKeyValuePair> init);
129
131 Values(const Values& other, const VectorValues& delta);
132
136
138 void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
139
141 bool equals(const Values& other, double tol=1e-9) const;
142
146
155 template <typename ValueType>
156 const ValueType at(Key j) const;
157
159 double atDouble(size_t key) const { return at<double>(key);}
160
166 const Value& at(Key j) const;
167
171 bool exists(Key j) const;
172
177 template<typename ValueType>
178 boost::optional<const ValueType&> exists(Key j) const;
179
181 size_t size() const { return values_.size(); }
182
184 bool empty() const { return values_.empty(); }
185
189
191 using const_iterator_type = typename KeyValueMap::const_iterator;
192 const_iterator_type it_;
193 deref_iterator(const_iterator_type it) : it_(it) {}
194 ConstKeyValuePair operator*() const { return {it_->first, *(it_->second)}; }
195 boost::shared_ptr<ConstKeyValuePair> operator->() {
196 return boost::make_shared<ConstKeyValuePair>(it_->first, *(it_->second));
197 }
198 bool operator==(const deref_iterator& other) const {
199 return it_ == other.it_;
200 }
201 bool operator!=(const deref_iterator& other) const { return it_ != other.it_; }
202 deref_iterator& operator++() {
203 ++it_;
204 return *this;
205 }
206 };
207
208 deref_iterator begin() const { return deref_iterator(values_.begin()); }
209 deref_iterator end() const { return deref_iterator(values_.end()); }
210
213 deref_iterator find(Key j) const { return deref_iterator(values_.find(j)); }
214
216 deref_iterator lower_bound(Key j) const { return deref_iterator(values_.lower_bound(j)); }
217
219 deref_iterator upper_bound(Key j) const { return deref_iterator(values_.upper_bound(j)); }
220
224
226 Values retract(const VectorValues& delta) const;
227
232 void retractMasked(const VectorValues& delta, const KeySet& mask);
233
235 VectorValues localCoordinates(const Values& cp) const;
236
238
240 void insert(Key j, const Value& val);
241
243 void insert(const Values& values);
244
248 template <typename ValueType>
249 void insert(Key j, const ValueType& val);
250
252 void insertDouble(Key j, double c) { insert<double>(j,c); }
253
255 void update(Key j, const Value& val);
256
261 template <typename T>
262 void update(Key j, const T& val);
263
265 void update(const Values& values);
266
268 void insert_or_assign(Key j, const Value& val);
269
274 void insert_or_assign(const Values& values);
275
277 template <typename ValueType>
278 void insert_or_assign(Key j, const ValueType& val);
279
281 void erase(Key j);
282
287 KeyVector keys() const;
288
292 KeySet keySet() const;
293
295 Values& operator=(const Values& rhs);
296
298 void swap(Values& other) { values_.swap(other.values_); }
299
301 void clear() { values_.clear(); }
302
304 size_t dim() const;
305
307 std::map<Key,size_t> dims() const;
308
310 VectorValues zeroVectors() const;
311
312 // Count values of given type \c ValueType
313 template<class ValueType>
314 size_t count() const {
315 size_t i = 0;
316 for (const auto key_value : values_) {
317 if (dynamic_cast<const GenericValue<ValueType>*>(key_value.second))
318 ++i;
319 }
320 return i;
321 }
322
342 template <class ValueType>
343 std::map<Key, ValueType> // , std::less<Key>, Eigen::aligned_allocator<ValueType>
344 extract(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
345
346#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
347 // Types obtained by iterating
348 typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair;
349 typedef KeyValueMap::iterator::value_type KeyValuePtrPair;
350
352 typedef boost::transform_iterator<
353 std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::iterator> iterator;
354
356 typedef boost::transform_iterator<
357 std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_iterator> const_iterator;
358
360 typedef boost::transform_iterator<
361 std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::reverse_iterator> reverse_iterator;
362
364 typedef boost::transform_iterator<
365 std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
366
371 std::pair<iterator, bool> tryInsert(Key j, const Value& value);
372
373 static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) {
374 return ConstKeyValuePair(key_value.first, *key_value.second); }
375
376 static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) {
377 return KeyValuePair(key_value.first, *key_value.second); }
378
379 const_iterator _begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); }
380 const_iterator _end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); }
381 iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); }
382 iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); }
383 const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); }
384 const_reverse_iterator rend() const { return boost::make_transform_iterator(values_.rend(), &make_const_deref_pair); }
385 reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); }
386 reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); }
387
390 iterator find(Key j) { return boost::make_transform_iterator(values_.find(j), &make_deref_pair); }
391
393 iterator lower_bound(Key j) { return boost::make_transform_iterator(values_.lower_bound(j), &make_deref_pair); }
394
396 iterator upper_bound(Key j) { return boost::make_transform_iterator(values_.upper_bound(j), &make_deref_pair); }
397
399 template <class ValueType = Value>
400 class Filtered;
401
403 template <class ValueType = Value>
404 class ConstFiltered;
405
407 template <class ValueType>
408 Values(const Filtered<ValueType>& view);
409
411 template <class ValueType>
412 Values(const ConstFiltered<ValueType>& view);
413
415 Filtered<Value> GTSAM_DEPRECATED
416 filter(const std::function<bool(Key)>& filterFcn);
417
419 template <class ValueType>
420 Filtered<ValueType> GTSAM_DEPRECATED
421 filter(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>);
422
424 ConstFiltered<Value> GTSAM_DEPRECATED
425 filter(const std::function<bool(Key)>& filterFcn) const;
426
428 template <class ValueType>
429 ConstFiltered<ValueType> GTSAM_DEPRECATED filter(
430 const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
431#endif
432
433 private:
434 // Filters based on ValueType (if not Value) and also based on the user-
435 // supplied \c filter function.
436 template<class ValueType>
437 static bool filterHelper(const std::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
438 BOOST_STATIC_ASSERT((!boost::is_same<ValueType, Value>::value));
439 // Filter and check the type
440 return filter(key_value.key) && (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value));
441 }
442
444 friend class boost::serialization::access;
445 template<class ARCHIVE>
446 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
447 ar & BOOST_SERIALIZATION_NVP(values_);
448 }
449
450 };
451
452 /* ************************************************************************* */
453 class ValuesKeyAlreadyExists : public std::exception {
454 protected:
455 const Key key_;
456
457 private:
458 mutable std::string message_;
459
460 public:
463 key_(key) {}
464
465 ~ValuesKeyAlreadyExists() noexcept override {}
466
468 Key key() const noexcept { return key_; }
469
471 GTSAM_EXPORT const char* what() const noexcept override;
472 };
473
474 /* ************************************************************************* */
475 class ValuesKeyDoesNotExist : public std::exception {
476 protected:
477 const char* operation_;
478 const Key key_;
479
480 private:
481 mutable std::string message_;
482
483 public:
485 ValuesKeyDoesNotExist(const char* operation, Key key) noexcept :
486 operation_(operation), key_(key) {}
487
488 ~ValuesKeyDoesNotExist() noexcept override {}
489
491 Key key() const noexcept { return key_; }
492
494 GTSAM_EXPORT const char* what() const noexcept override;
495 };
496
497 /* ************************************************************************* */
498 class ValuesIncorrectType : public std::exception {
499 protected:
500 const Key key_;
501 const std::type_info& storedTypeId_;
502 const std::type_info& requestedTypeId_;
503
504 private:
505 mutable std::string message_;
506
507 public:
510 const std::type_info& storedTypeId, const std::type_info& requestedTypeId) noexcept :
511 key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
512
513 ~ValuesIncorrectType() noexcept override {}
514
516 Key key() const noexcept { return key_; }
517
519 const std::type_info& storedTypeId() const { return storedTypeId_; }
520
522 const std::type_info& requestedTypeId() const { return requestedTypeId_; }
523
525 GTSAM_EXPORT const char* what() const noexcept override;
526 };
527
528 /* ************************************************************************* */
529 class DynamicValuesMismatched : public std::exception {
530
531 public:
532 DynamicValuesMismatched() noexcept {}
533
534 ~DynamicValuesMismatched() noexcept override {}
535
536 const char* what() const noexcept override {
537 return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values";
538 }
539 };
540
541 /* ************************************************************************* */
542 class NoMatchFoundForFixed: public std::exception {
543
544 protected:
545 const size_t M1_, N1_;
546 const size_t M2_, N2_;
547
548 private:
549 mutable std::string message_;
550
551 public:
552 NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) noexcept :
553 M1_(M1), N1_(N1), M2_(M2), N2_(N2) {
554 }
555
556 ~NoMatchFoundForFixed() noexcept override {
557 }
558
559 GTSAM_EXPORT const char* what() const noexcept override;
560 };
561
562 /* ************************************************************************* */
564 template<>
565 struct traits<Values> : public Testable<Values> {
566 };
567
568} //\ namespace gtsam
569
570
571#include <gtsam/nonlinear/Values-inl.h>
An easy way to control which allocator is used for Fast* collections.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
bool operator!=(const Matrix &A, const Matrix &B)
inequality
Definition Matrix.h:107
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
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
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
Default allocator for list, map, and set types.
Definition FastDefaultAllocator.h:50
Wraps any type T so it can play as a Value.
Definition GenericValue.h:47
Template to create a binary predicate.
Definition Testable.h:111
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
This is the base class for any type to be stored in Values.
Definition Value.h:37
virtual void deallocate_() const =0
Deallocate a raw pointer of this value.
virtual Value * clone_() const =0
Clone this value in a special memory pool, must be deleted with Value::deallocate_,...
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
Definition Values.h:49
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
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...
bool empty() const
whether the config is empty
Definition Values.h:184
deref_iterator upper_bound(Key j) const
Find the lowest-ordered element greater than the specified key.
Definition Values.h:219
boost::shared_ptr< Values > shared_ptr
A shared_ptr to this class.
Definition Values.h:87
void clear()
Remove all variables from the config.
Definition Values.h:301
void swap(Values &other)
Swap the contents of two Values without copying data.
Definition Values.h:298
void insertDouble(Key j, double c)
version for double
Definition Values.h:252
size_t size() const
The number of variables in this config.
Definition Values.h:181
deref_iterator find(Key j) const
Find an element by key, returning an iterator, or end() if the key was not found.
Definition Values.h:213
boost::shared_ptr< const Values > const_shared_ptr
A const shared_ptr to this class.
Definition Values.h:90
Values()=default
Default constructor creates an empty Values class.
deref_iterator lower_bound(Key j) const
Find the element greater than or equal to the specified key.
Definition Values.h:216
double atDouble(size_t key) const
version for double
Definition Values.h:159
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:93
Value & value
The value.
Definition Values.h:95
const Key key
The key.
Definition Values.h:94
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:101
const Key key
The key.
Definition Values.h:102
const Value & value
The value.
Definition Values.h:103
Definition Values.h:190
Definition Values.h:453
const Key key_
The key that already existed.
Definition Values.h:455
GTSAM_EXPORT const char * what() const noexcept override
The message to be displayed to the user.
Definition Values.cpp:280
ValuesKeyAlreadyExists(Key key) noexcept
Construct with the key-value pair attempted to be added.
Definition Values.h:462
Key key() const noexcept
The duplicate key that was attempted to be added.
Definition Values.h:468
Definition Values.h:475
ValuesKeyDoesNotExist(const char *operation, Key key) noexcept
Construct with the key that does not exist in the values.
Definition Values.h:485
const Key key_
The key that does not exist.
Definition Values.h:478
Key key() const noexcept
The key that was attempted to be accessed that does not exist.
Definition Values.h:491
const char * operation_
The operation that attempted to access the key.
Definition Values.h:477
Definition Values.h:498
const std::type_info & storedTypeId() const
The typeid of the value stores in the Values.
Definition Values.h:519
Key key() const noexcept
The key that was attempted to be accessed that does not exist.
Definition Values.h:516
const Key key_
The key requested.
Definition Values.h:500
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:509
const std::type_info & requestedTypeId() const
The requested typeid.
Definition Values.h:522
Definition Values.h:529
Definition Values.h:542
The Factor::error simply extracts the.
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...