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 // Added this section for compile gtsam python on windows.
201 // msvc don't deduct the template arguments correctly, due possible bug in msvc.
202 #ifdef _WIN32
203 #if _MSC_VER < 1937
204  // Handle dynamic matrices
205  template <int M, int N>
206  struct handle_matrix<Eigen::Matrix<double, M, N, 0, M, N>, true> {
207  inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
208  auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
209  if (ptr) {
210  // value returns a const Matrix&, and the return makes a copy !!!!!
211  return ptr->value();
212  } else {
213  // If a fixed matrix was stored, we end up here as well.
214  throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>));
215  }
216  }
217  };
218 
219  // Handle fixed matrices
220  template <int M, int N>
221  struct handle_matrix<Eigen::Matrix<double, M, N, 0, M, N>, false> {
222  inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
223  auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
224  if (ptr) {
225  // value returns a const MatrixMN&, and the return makes a copy !!!!!
226  return ptr->value();
227  } else {
228  Matrix A;
229  // Check if a dynamic matrix was stored
230  auto ptr = dynamic_cast<const GenericValue<Eigen::MatrixXd>*>(pointer);
231  if (ptr) {
232  A = ptr->value();
233  } else {
234  // Or a dynamic vector
235  A = handle_matrix<Eigen::VectorXd, true>()(j, pointer); // will throw if not....
236  }
237  // Yes: check size, and throw if not a match
238  if (A.rows() != M || A.cols() != N)
239  throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
240  else
241  return A; // copy but not malloc
242  }
243  }
244  };
245 
246  // Handle matrices
247  template <int M, int N>
248  struct handle<Eigen::Matrix<double, M, N, 0, M, N>> {
249  Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
250  return handle_matrix<Eigen::Matrix<double, M, N, 0, M, N>,
251  (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer);
252  }
253  };
254 #endif // #if _MSC_VER < 1937
255 #endif // #ifdef _WIN32
256 
257  } // internal
258 
259  /* ************************************************************************* */
260  template <typename ValueType>
261  const ValueType Values::at(Key j) const {
262  // Find the item
263  KeyValueMap::const_iterator item = values_.find(j);
264 
265  // Throw exception if it does not exist
266  if (item == values_.end()) throw ValuesKeyDoesNotExist("at", j);
267 
268  // Check the type and throw exception if incorrect
269  // h() split in two lines to avoid internal compiler error (MSVC2017)
271  return h(j, item->second.get());
272  }
273 
274  /* ************************************************************************* */
275  template<typename ValueType>
276  const ValueType * Values::exists(Key j) const {
277  // Find the item
278  KeyValueMap::const_iterator item = values_.find(j);
279 
280  if(item != values_.end()) {
281  const Value* value = item->second.get();
282  // dynamic cast the type and throw exception if incorrect
283  auto ptr = dynamic_cast<const GenericValue<ValueType>*>(value);
284  if (ptr) {
285  return &ptr->value();
286  } else {
287  // NOTE(abe): clang warns about potential side effects if done in typeid
288  throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
289  }
290  } else {
291  return nullptr;
292  }
293  }
294 
295  /* ************************************************************************* */
296 
297  // insert a templated value
298  template<typename ValueType>
299  void Values::insert(Key j, const ValueType& val) {
300  insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
301  }
302 
303  // partial specialization to insert an expression involving unary operators
304  template <typename UnaryOp, typename ValueType>
306  insert(j, val.eval());
307  }
308 
309  // partial specialization to insert an expression involving binary operators
310  template <typename BinaryOp, typename ValueType1, typename ValueType2>
312  insert(j, val.eval());
313  }
314 
315  // update with templated value
316  template <typename ValueType>
317  void Values::update(Key j, const ValueType& val) {
318  update(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
319  }
320 
321  // partial specialization to update with an expression involving unary operators
322  template <typename UnaryOp, typename ValueType>
324  update(j, val.eval());
325  }
326 
327  // partial specialization to update with an expression involving binary operators
328  template <typename BinaryOp, typename ValueType1, typename ValueType2>
330  update(j, val.eval());
331  }
332 
333  // insert_or_assign with templated value
334  template <typename ValueType>
335  void Values::insert_or_assign(Key j, const ValueType& val) {
336  insert_or_assign(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
337  }
338 
339  template <typename UnaryOp, typename ValueType>
341  insert_or_assign(j, val.eval());
342  }
343 
344  template <typename BinaryOp, typename ValueType1, typename ValueType2>
346  insert_or_assign(j, val.eval());
347  }
348 
349 }
gtsam::Values::exists
bool exists(Key j) const
Definition: Values.cpp:93
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
gtsam::_ValuesConstKeyValuePair::_ValuesConstKeyValuePair
_ValuesConstKeyValuePair(Key _key, const ValueType &_value)
Definition: Values-inl.h:50
Eigen::CwiseBinaryOp
Generic expression where a coefficient-wise binary operator is applied to two expressions.
Definition: CwiseBinaryOp.h:77
gtsam::ValuesIncorrectType
Definition: Values.h:453
gtsam::_ValuesConstKeyValuePair::_ValuesConstKeyValuePair
_ValuesConstKeyValuePair(const _ValuesKeyValuePair< ValueType > &rhs)
Definition: Values-inl.h:53
gtsam::Values::ConstKeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition: Values.h:98
gtsam::_ValuesKeyValuePair::value
ValueType & value
The value.
Definition: Values-inl.h:39
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Values::update
void update(Key j, const Value &val)
Definition: Values.cpp:169
h
const double h
Definition: testSimpleHelicopter.cpp:19
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::Values::values_
KeyValueMap values_
Definition: Values.h:79
gtsam::_ValuesConstKeyValuePair::key
const Key key
The key.
Definition: Values-inl.h:47
gtsam::_ValuesConstKeyValuePair
Definition: Values-inl.h:46
gtsam::GenericValue
Definition: GenericValue.h:44
gtsam::internal::handle_matrix< Eigen::Matrix< double, M, N >, false >::operator()
Eigen::Matrix< double, M, N > operator()(Key j, const Value *const pointer)
Definition: Values-inl.h:167
gtsam::internal::handle< Eigen::Matrix< double, M, N > >::operator()
Eigen::Matrix< double, M, N > operator()(Key j, const Value *const pointer)
Definition: Values-inl.h:194
gtsam::internal::handle::operator()
ValueType operator()(Key j, const Value *const pointer)
Definition: Values-inl.h:135
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
A
Definition: test_numpy_dtypes.cpp:298
handle
Definition: pytypes.h:226
gtsam::Values::extract
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Definition: Values-inl.h:105
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::ValuesCastHelper
Definition: Values-inl.h:63
operator()
internal::enable_if< internal::valid_indexed_view_overload< RowIndices, ColIndices >::value &&internal::traits< typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::ReturnAsIndexedView, typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::type operator()(const RowIndices &rowIndices, const ColIndices &colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
Definition: IndexedViewMethods.h:73
gtsam::NoMatchFoundForFixed
Definition: Values.h:497
Eigen::Dynamic
const int Dynamic
Definition: Constants.h:22
gtsam::Values::count
size_t count() const
Definition: Values-inl.h:94
gtsam::GenericValue::value
const T & value() const
Return a constant value.
Definition: GenericValue.h:64
gtsam::Value
Definition: Value.h:39
gtsam::internal::handle_matrix
Definition: Values-inl.h:147
gtsam::_ValuesKeyValuePair
Definition: Values-inl.h:37
key
const gtsam::Symbol key('X', 0)
gtsam::ValuesCastHelper::cast
static CastedKeyValuePairType cast(KeyValuePairType key_value)
Definition: Values-inl.h:64
gtsam
traits
Definition: SFMdata.h:40
gtsam::_ValuesConstKeyValuePair::value
const ValueType & value
The value.
Definition: Values-inl.h:48
gtsam::ValuesCastHelper< Value, CastedKeyValuePairType, KeyValuePairType >::cast
static CastedKeyValuePairType cast(KeyValuePairType key_value)
Definition: Values-inl.h:74
gtsam::_ValuesKeyValuePair::key
const Key key
The key.
Definition: Values-inl.h:38
Eigen::CwiseUnaryOp
Generic expression where a coefficient-wise unary operator is applied to an expression.
Definition: CwiseUnaryOp.h:55
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::ValuesKeyDoesNotExist
Definition: Values.h:430
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
N
#define N
Definition: igam.h:9
internal
Definition: BandTriangularSolver.h:13
gtsam::internal::handle
Definition: Values-inl.h:134
align_3::t
Point2 t(10, 10)
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
_
constexpr descr< N - 1 > _(char const (&text)[N])
Definition: descr.h:109
gtsam::_ValuesKeyValuePair::_ValuesKeyValuePair
_ValuesKeyValuePair(Key _key, ValueType &_value)
Definition: Values-inl.h:41
gtsam::ValuesCastHelper< const Value, CastedKeyValuePairType, KeyValuePairType >::cast
static CastedKeyValuePairType cast(KeyValuePairType key_value)
Definition: Values-inl.h:84
test_callbacks.value
value
Definition: test_callbacks.py:160
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::internal::handle_matrix< Eigen::Matrix< double, M, N >, true >::operator()
Eigen::Matrix< double, M, N > operator()(Key j, const Value *const pointer)
Definition: Values-inl.h:152
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::Values::insert_or_assign
void insert_or_assign(Key j, const Value &val)
If key j exists, update value, else perform an insert.
Definition: Values.cpp:192
gtsam::Values::ConstKeyValuePair::key
const Key key
The key.
Definition: Values.h:99
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:09:40