gtsam  4.1.0
gtsam
Values-inl.h
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 
29 #include <gtsam/nonlinear/Values.h> // Only so Eclipse finds class definition
30 
31 namespace gtsam {
32 
33 
34  /* ************************************************************************* */
35  template<class ValueType>
37  const Key key;
38  ValueType& value;
39 
40  _ValuesKeyValuePair(Key _key, ValueType& _value) : key(_key), value(_value) {}
41  };
42 
43  /* ************************************************************************* */
44  template<class ValueType>
46  const Key key;
47  const ValueType& value;
48 
49  _ValuesConstKeyValuePair(Key _key, const ValueType& _value) :
50  key(_key), value(_value) {
51  }
53  key(rhs.key), value(rhs.value) {
54  }
55  };
56 
57  /* ************************************************************************* */
58 
59  // Cast helpers for making _Values[Const]KeyValuePair's from Values::[Const]KeyValuePair
60  // need to use a struct here for later partial specialization
61  template<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
63  static CastedKeyValuePairType cast(KeyValuePairType key_value) {
64  // Static cast because we already checked the type during filtering
65  return CastedKeyValuePairType(key_value.key,
66  const_cast<GenericValue<ValueType>&>(static_cast<const GenericValue<
67  ValueType>&>(key_value.value)).value());
68  }
69  };
70  // partial specialized version for ValueType == Value
71  template<class CastedKeyValuePairType, class KeyValuePairType>
72  struct ValuesCastHelper<Value, CastedKeyValuePairType, KeyValuePairType> {
73  static CastedKeyValuePairType cast(KeyValuePairType key_value) {
74  // Static cast because we already checked the type during filtering
75  // in this case the casted and keyvalue pair are essentially the same type
76  // (key, Value&) so perhaps this could be done with just a cast of the key_value?
77  return CastedKeyValuePairType(key_value.key, key_value.value);
78  }
79  };
80  // partial specialized version for ValueType == Value
81  template<class CastedKeyValuePairType, class KeyValuePairType>
82  struct ValuesCastHelper<const Value, CastedKeyValuePairType, KeyValuePairType> {
83  static CastedKeyValuePairType cast(KeyValuePairType key_value) {
84  // Static cast because we already checked the type during filtering
85  // in this case the casted and keyvalue pair are essentially the same type
86  // (key, Value&) so perhaps this could be done with just a cast of the key_value?
87  return CastedKeyValuePairType(key_value.key, key_value.value);
88  }
89  };
90 
91  /* ************************************************************************* */
92  template<class ValueType>
94  public:
98  typedef KeyValuePair value_type;
99 
100  typedef
101  boost::transform_iterator<
103  boost::filter_iterator<
104  boost::function<bool(const Values::ConstKeyValuePair&)>,
106  iterator;
107 
108  typedef iterator const_iterator;
109 
110  typedef
111  boost::transform_iterator<
113  boost::filter_iterator<
114  boost::function<bool(const Values::ConstKeyValuePair&)>,
116  const_const_iterator;
117 
118  iterator begin() { return begin_; }
119  iterator end() { return end_; }
120  const_iterator begin() const { return begin_; }
121  const_iterator end() const { return end_; }
122  const_const_iterator beginConst() const { return constBegin_; }
123  const_const_iterator endConst() const { return constEnd_; }
124 
126  size_t size() const {
127  size_t i = 0;
128  for (const_const_iterator it = beginConst(); it != endConst(); ++it)
129  ++i;
130  return i;
131  }
132 
133  private:
134  Filtered(
135  const boost::function<bool(const Values::ConstKeyValuePair&)>& filter,
136  Values& values) :
137  begin_(
138  boost::make_transform_iterator(
139  boost::make_filter_iterator(filter, values.begin(), values.end()),
140  &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)), end_(
141  boost::make_transform_iterator(
142  boost::make_filter_iterator(filter, values.end(), values.end()),
143  &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)), constBegin_(
144  boost::make_transform_iterator(
145  boost::make_filter_iterator(filter,
146  ((const Values&) values).begin(),
147  ((const Values&) values).end()),
149  Values::ConstKeyValuePair>::cast)), constEnd_(
150  boost::make_transform_iterator(
151  boost::make_filter_iterator(filter,
152  ((const Values&) values).end(),
153  ((const Values&) values).end()),
155  Values::ConstKeyValuePair>::cast)) {
156  }
157 
158  friend class Values;
159  iterator begin_;
160  iterator end_;
161  const_const_iterator constBegin_;
162  const_const_iterator constEnd_;
163  };
164 
165  /* ************************************************************************* */
166  template<class ValueType>
168  public:
171  typedef KeyValuePair value_type;
172 
173  typedef typename Filtered<ValueType>::const_const_iterator iterator;
174  typedef typename Filtered<ValueType>::const_const_iterator const_iterator;
175 
178  begin_(rhs.beginConst()),
179  end_(rhs.endConst()) {}
180 
181  iterator begin() { return begin_; }
182  iterator end() { return end_; }
183  const_iterator begin() const { return begin_; }
184  const_iterator end() const { return end_; }
185 
187  size_t size() const {
188  size_t i = 0;
189  for (const_iterator it = begin(); it != end(); ++it)
190  ++i;
191  return i;
192  }
193 
194  FastList<Key> keys() const {
195  FastList<Key> result;
196  for(const_iterator it = begin(); it != end(); ++it)
197  result.push_back(it->key);
198  return result;
199  }
200 
201  private:
202  friend class Values;
203  const_iterator begin_;
204  const_iterator end_;
206  const boost::function<bool(const Values::ConstKeyValuePair&)>& filter,
207  const Values& values) {
208  // We remove the const from values to create a non-const Filtered
209  // view, then pull the const_iterators out of it.
210  const Filtered<ValueType> filtered(filter, const_cast<Values&>(values));
211  begin_ = filtered.beginConst();
212  end_ = filtered.endConst();
213  }
214  };
215 
216  /* ************************************************************************* */
218  template<class ValueType>
220  for(const typename Filtered<ValueType>::KeyValuePair& key_value: view) {
221  Key key = key_value.key;
222  insert(key, static_cast<const ValueType&>(key_value.value));
223  }
224  }
225 
226  /* ************************************************************************* */
227  template<class ValueType>
229  for(const typename ConstFiltered<ValueType>::KeyValuePair& key_value: view) {
230  Key key = key_value.key;
231  insert(key, static_cast<const ValueType&>(key_value.value));
232  }
233  }
234 
235  /* ************************************************************************* */
237  inline Values::filter(const boost::function<bool(Key)>& filterFcn) {
238  return filter<Value>(filterFcn);
239  }
240 
241  /* ************************************************************************* */
242  template<class ValueType>
244  Values::filter(const boost::function<bool(Key)>& filterFcn) {
245  return Filtered<ValueType>(boost::bind(&filterHelper<ValueType>, filterFcn, _1), *this);
246  }
247 
248  /* ************************************************************************* */
250  inline Values::filter(const boost::function<bool(Key)>& filterFcn) const {
251  return filter<Value>(filterFcn);
252  }
253 
254  /* ************************************************************************* */
255  template<class ValueType>
257  Values::filter(const boost::function<bool(Key)>& filterFcn) const {
258  return ConstFiltered<ValueType>(boost::bind(&filterHelper<ValueType>, filterFcn, _1), *this);
259  }
260 
261  /* ************************************************************************* */
262  template<>
263  inline bool Values::filterHelper<Value>(const boost::function<bool(Key)> filter,
264  const ConstKeyValuePair& key_value) {
265  // Filter and check the type
266  return filter(key_value.key);
267  }
268 
269  /* ************************************************************************* */
270 
271  namespace internal {
272 
273  // Check the type and throw exception if incorrect
274  // Generic version, partially specialized below for various Eigen Matrix types
275  template <typename ValueType>
276  struct handle {
277  ValueType operator()(Key j, const Value* const pointer) {
278  try {
279  // value returns a const ValueType&, and the return makes a copy !!!!!
280  return dynamic_cast<const GenericValue<ValueType>&>(*pointer).value();
281  } catch (std::bad_cast&) {
282  throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
283  }
284  }
285  };
286 
287  template <typename MatrixType, bool isDynamic>
289 
290  // Handle dynamic matrices
291  template <int M, int N>
292  struct handle_matrix<Eigen::Matrix<double, M, N>, true> {
293  Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
294  try {
295  // value returns a const Matrix&, and the return makes a copy !!!!!
296  return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value();
297  } catch (std::bad_cast&) {
298  // If a fixed matrix was stored, we end up here as well.
299  throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>));
300  }
301  }
302  };
303 
304  // Handle fixed matrices
305  template <int M, int N>
306  struct handle_matrix<Eigen::Matrix<double, M, N>, false> {
307  Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
308  try {
309  // value returns a const MatrixMN&, and the return makes a copy !!!!!
310  return dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>&>(*pointer).value();
311  } catch (std::bad_cast&) {
312  Matrix A;
313  try {
314  // Check if a dynamic matrix was stored
315  A = handle_matrix<Eigen::MatrixXd, true>()(j, pointer); // will throw if not....
316  } catch (const ValuesIncorrectType&) {
317  // Or a dynamic vector
318  A = handle_matrix<Eigen::VectorXd, true>()(j, pointer); // will throw if not....
319  }
320  // Yes: check size, and throw if not a match
321  if (A.rows() != M || A.cols() != N)
322  throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
323  else
324  return A; // copy but not malloc
325  }
326  }
327  };
328 
329  // Handle matrices
330  template <int M, int N>
331  struct handle<Eigen::Matrix<double, M, N>> {
332  Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
334  (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer);
335  }
336  };
337 
338  } // internal
339 
340  /* ************************************************************************* */
341  template<typename ValueType>
342  ValueType Values::at(Key j) const {
343  // Find the item
344  KeyValueMap::const_iterator item = values_.find(j);
345 
346  // Throw exception if it does not exist
347  if(item == values_.end())
348  throw ValuesKeyDoesNotExist("at", j);
349 
350  // Check the type and throw exception if incorrect
351  // h() split in two lines to avoid internal compiler error (MSVC2017)
352  auto h = internal::handle<ValueType>();
353  return h(j,item->second);
354  }
355 
356  /* ************************************************************************* */
357  template<typename ValueType>
358  boost::optional<const ValueType&> Values::exists(Key j) const {
359  // Find the item
360  KeyValueMap::const_iterator item = values_.find(j);
361 
362  if(item != values_.end()) {
363  // dynamic cast the type and throw exception if incorrect
364  const Value& value = *item->second;
365  try {
366  return dynamic_cast<const GenericValue<ValueType>&>(value).value();
367  } catch (std::bad_cast &) {
368  // NOTE(abe): clang warns about potential side effects if done in typeid
369  const Value* value = item->second;
370  throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
371  }
372  } else {
373  return boost::none;
374  }
375  }
376 
377  /* ************************************************************************* */
378 
379  // insert a templated value
380  template<typename ValueType>
381  void Values::insert(Key j, const ValueType& val) {
382  insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
383  }
384 
385  // update with templated value
386  template <typename ValueType>
387  void Values::update(Key j, const ValueType& val) {
388  update(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
389  }
390 
391 }
gtsam::NoMatchFoundForFixed
Definition: Values.h:522
gtsam::_ValuesConstKeyValuePair::key
const Key key
The key.
Definition: Values-inl.h:46
gtsam::ValuesKeyDoesNotExist
Definition: Values.h:455
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::_ValuesKeyValuePair::key
const Key key
The key.
Definition: Values-inl.h:37
gtsam::GenericValue
Wraps any type T so it can play as a Value.
Definition: GenericValue.h:47
gtsam::Values::ConstFiltered::size
size_t size() const
Returns the number of values in this view.
Definition: Values-inl.h:187
gtsam::_ValuesConstKeyValuePair
Definition: Values-inl.h:45
gtsam::Values::filter
Filtered< Value > filter(const boost::function< bool(Key)> &filterFcn)
Return a filtered view of this Values class, without copying any data.
Definition: Values-inl.h:237
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::Values::KeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition: Values.h:102
gtsam::Values::Filtered::KeyValuePair
_ValuesKeyValuePair< ValueType > KeyValuePair
A key-value pair, with the value a specific derived Value type.
Definition: Values-inl.h:96
gtsam::Values::ConstFiltered::KeyValuePair
_ValuesConstKeyValuePair< ValueType > KeyValuePair
A const key-value pair, with the value a specific derived Value type.
Definition: Values-inl.h:170
gtsam::FastList
Definition: FastList.h:38
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
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::ConstFiltered::ConstFiltered
ConstFiltered(const Filtered< ValueType > &rhs)
Conversion from Filtered to ConstFiltered.
Definition: Values-inl.h:177
gtsam::_ValuesConstKeyValuePair::value
const ValueType & value
The value.
Definition: Values-inl.h:47
gtsam::Values::Filtered::size
size_t size() const
Returns the number of values in this view.
Definition: Values-inl.h:126
gtsam::internal::handle
Definition: Values-inl.h:276
gtsam::Values::Filtered
A filtered view of a Values, returned from Values::filter.
Definition: Values-inl.h:93
gtsam::ValuesCastHelper
Definition: Values-inl.h:62
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition: Values.cpp:140
gtsam::Values::at
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
gtsam::_ValuesKeyValuePair::value
ValueType & value
The value.
Definition: Values-inl.h:38
gtsam::_ValuesKeyValuePair
Definition: Values-inl.h:36
gtsam::ValuesIncorrectType
Definition: Values.h:478
gtsam::Values::Values
Values()
Default constructor creates an empty Values class.
Definition: Values.h:145
gtsam::GenericValue::value
const T & value() const
Return a constant value.
Definition: GenericValue.h:67
gtsam::Values::exists
bool exists(Key j) const
Check if a value exists with key j.
Definition: Values.cpp:104
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::Values::update
void update(Key j, const Value &val)
single element change of existing element
Definition: Values.cpp:161
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
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::internal::handle_matrix
Definition: Values-inl.h:288