Values-inl.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 <utility>
28 #include <gtsam/nonlinear/Values.h>
29 
30 #include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition
31 
32 namespace gtsam {
33 
34 
35  /* ************************************************************************* */
36  template<class ValueType>
38  const Key key;
39  ValueType& value;
40 
41  _ValuesKeyValuePair(Key _key, ValueType& _value) : key(_key), value(_value) {}
42  };
43 
44  /* ************************************************************************* */
45  template<class ValueType>
47  const Key key;
48  const ValueType& value;
49 
50  _ValuesConstKeyValuePair(Key _key, const ValueType& _value) :
51  key(_key), value(_value) {
52  }
54  key(rhs.key), value(rhs.value) {
55  }
56  };
57 
58  /* ************************************************************************* */
59 
60  // Cast helpers for making _Values[Const]KeyValuePair's from Values::[Const]KeyValuePair
61  // need to use a struct here for later partial specialization
62  template<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
64  static CastedKeyValuePairType cast(KeyValuePairType key_value) {
65  // Static cast because we already checked the type during filtering
66  return CastedKeyValuePairType(key_value.key,
67  const_cast<GenericValue<ValueType>&>(static_cast<const GenericValue<
68  ValueType>&>(key_value.value)).value());
69  }
70  };
71  // partial specialized version for ValueType == Value
72  template<class CastedKeyValuePairType, class KeyValuePairType>
73  struct ValuesCastHelper<Value, CastedKeyValuePairType, KeyValuePairType> {
74  static CastedKeyValuePairType cast(KeyValuePairType key_value) {
75  // Static cast because we already checked the type during filtering
76  // in this case the casted and keyvalue pair are essentially the same type
77  // (key, Value&) so perhaps this could be done with just a cast of the key_value?
78  return CastedKeyValuePairType(key_value.key, key_value.value);
79  }
80  };
81  // partial specialized version for ValueType == Value
82  template<class CastedKeyValuePairType, class KeyValuePairType>
83  struct ValuesCastHelper<const Value, CastedKeyValuePairType, KeyValuePairType> {
84  static CastedKeyValuePairType cast(KeyValuePairType key_value) {
85  // Static cast because we already checked the type during filtering
86  // in this case the casted and keyvalue pair are essentially the same type
87  // (key, Value&) so perhaps this could be done with just a cast of the key_value?
88  return CastedKeyValuePairType(key_value.key, key_value.value);
89  }
90  };
91 
92  /* ************************************************************************* */
93  template <class ValueType>
94  size_t Values::count() const {
95  size_t i = 0;
96  for (const auto& [_, value] : values_) {
97  if (dynamic_cast<const GenericValue<ValueType>*>(value.get())) ++i;
98  }
99  return i;
100  }
101 
102  /* ************************************************************************* */
103  template <class ValueType>
104  std::map<Key, ValueType>
105  Values::extract(const std::function<bool(Key)>& filterFcn) const {
106  std::map<Key, ValueType> result;
107  for (const auto& [key,value] : values_) {
108  // Check if key matches
109  if (filterFcn(key)) {
110  // Check if type matches (typically does as symbols matched with types)
111  if (auto t =
112  dynamic_cast<const GenericValue<ValueType>*>(value.get()))
113  result[key] = t->value();
114  }
115  }
116  return result;
117  }
118 
119  /* ************************************************************************* */
120  template<>
121  inline bool Values::filterHelper<Value>(const std::function<bool(Key)> filter,
122  const ConstKeyValuePair& key_value) {
123  // Filter and check the type
124  return filter(key_value.key);
125  }
126 
127  /* ************************************************************************* */
128 
129  namespace internal {
130 
131  // Check the type and throw exception if incorrect
132  // Generic version, partially specialized below for various Eigen Matrix types
133  template <typename ValueType>
134  struct handle {
135  ValueType operator()(Key j, const Value* const pointer) {
136  auto ptr = dynamic_cast<const GenericValue<ValueType>*>(pointer);
137  if (ptr) {
138  // value returns a const ValueType&, and the return makes a copy !!!!!
139  return ptr->value();
140  } else {
141  throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
142  }
143  }
144  };
145 
146  template <typename MatrixType, bool isDynamic>
148 
149  // Handle dynamic matrices
150  template <int M, int N>
151  struct handle_matrix<Eigen::Matrix<double, M, N>, true> {
152  inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
153  auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
154  if (ptr) {
155  // value returns a const Matrix&, and the return makes a copy !!!!!
156  return ptr->value();
157  } else {
158  // If a fixed matrix was stored, we end up here as well.
159  throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>));
160  }
161  }
162  };
163 
164  // Handle fixed matrices
165  template <int M, int N>
166  struct handle_matrix<Eigen::Matrix<double, M, N>, false> {
167  inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
168  auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
169  if (ptr) {
170  // value returns a const MatrixMN&, and the return makes a copy !!!!!
171  return ptr->value();
172  } else {
173  Matrix A;
174  // Check if a dynamic matrix was stored
175  auto ptr = dynamic_cast<const GenericValue<Eigen::MatrixXd>*>(pointer);
176  if (ptr) {
177  A = ptr->value();
178  } else {
179  // Or a dynamic vector
180  A = handle_matrix<Eigen::VectorXd, true>()(j, pointer); // will throw if not....
181  }
182  // Yes: check size, and throw if not a match
183  if (A.rows() != M || A.cols() != N)
184  throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
185  else
186  return A; // copy but not malloc
187  }
188  }
189  };
190 
191  // Handle matrices
192  template <int M, int N>
193  struct handle<Eigen::Matrix<double, M, N>> {
196  (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer);
197  }
198  };
199 
200  } // internal
201 
202  /* ************************************************************************* */
203  template <typename ValueType>
204  const ValueType Values::at(Key j) const {
205  // Find the item
206  KeyValueMap::const_iterator item = values_.find(j);
207 
208  // Throw exception if it does not exist
209  if (item == values_.end()) throw ValuesKeyDoesNotExist("at", j);
210 
211  // Check the type and throw exception if incorrect
212  // h() split in two lines to avoid internal compiler error (MSVC2017)
214  return h(j, item->second.get());
215  }
216 
217  /* ************************************************************************* */
218  template<typename ValueType>
219  const ValueType * Values::exists(Key j) const {
220  // Find the item
221  KeyValueMap::const_iterator item = values_.find(j);
222 
223  if(item != values_.end()) {
224  const Value* value = item->second.get();
225  // dynamic cast the type and throw exception if incorrect
226  auto ptr = dynamic_cast<const GenericValue<ValueType>*>(value);
227  if (ptr) {
228  return &ptr->value();
229  } else {
230  // NOTE(abe): clang warns about potential side effects if done in typeid
231  throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
232  }
233  } else {
234  return nullptr;
235  }
236  }
237 
238  /* ************************************************************************* */
239 
240  // insert a templated value
241  template<typename ValueType>
242  void Values::insert(Key j, const ValueType& val) {
243  insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
244  }
245 
246  // update with templated value
247  template <typename ValueType>
248  void Values::update(Key j, const ValueType& val) {
249  update(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
250  }
251 
252  // insert_or_assign with templated value
253  template <typename ValueType>
254  void Values::insert_or_assign(Key j, const ValueType& val) {
255  insert_or_assign(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
256  }
257 
258 }
Eigen::Matrix< double, M, N > operator()(Key j, const Value *const pointer)
Definition: Values-inl.h:194
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
A insert(1, 2)=0
_ValuesConstKeyValuePair(const _ValuesKeyValuePair< ValueType > &rhs)
Definition: Values-inl.h:53
def update(text)
Definition: relicense.py:46
A non-templated config holding any types of Manifold-group elements.
const ValueType at(Key j) const
Definition: Values-inl.h:204
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Definition: Values-inl.h:105
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
void update(Key j, const Value &val)
Definition: Values.cpp:169
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Eigen::Matrix< double, M, N > operator()(Key j, const Value *const pointer)
Definition: Values-inl.h:152
#define N
Definition: gksort.c:12
A key-value pair, which you get by dereferencing iterators.
Definition: Values.h:98
const Key key
The key.
Definition: Values-inl.h:47
ValueType operator()(Key j, const Value *const pointer)
Definition: Values-inl.h:135
static CastedKeyValuePairType cast(KeyValuePairType key_value)
Definition: Values-inl.h:64
Values result
const Key key
The key.
Definition: Values-inl.h:38
Eigen::Matrix< double, M, N > operator()(Key j, const Value *const pointer)
Definition: Values-inl.h:167
static CastedKeyValuePairType cast(KeyValuePairType key_value)
Definition: Values-inl.h:84
traits
Definition: chartTesting.h:28
size_t count() const
Definition: Values-inl.h:94
const double h
void insert_or_assign(Key j, const Value &val)
If key j exists, update value, else perform an insert.
Definition: Values.cpp:192
constexpr descr< N - 1 > _(char const (&text)[N])
Definition: descr.h:109
ValueType & value
The value.
Definition: Values-inl.h:39
static CastedKeyValuePairType cast(KeyValuePairType key_value)
Definition: Values-inl.h:74
void insert(Key j, const Value &val)
Definition: Values.cpp:155
const ValueType & value
The value.
Definition: Values-inl.h:48
const int Dynamic
Definition: Constants.h:22
The matrix class, also used for vectors and row-vectors.
bool exists(Key j) const
Definition: Values.cpp:93
_ValuesConstKeyValuePair(Key _key, const ValueType &_value)
Definition: Values-inl.h:50
_ValuesKeyValuePair(Key _key, ValueType &_value)
Definition: Values-inl.h:41
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
std::ptrdiff_t j
Point2 t(10, 10)
const T & value() const
Return a constant value.
Definition: GenericValue.h:64


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:43