gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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#include <boost/bind/bind.hpp>
30
31namespace 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>
93 std::map<Key, ValueType>
94 Values::extract(const std::function<bool(Key)>& filterFcn) const {
95 std::map<Key, ValueType> result;
96 for (const auto& key_value : values_) {
97 // Check if key matches
98 if (filterFcn(key_value.first)) {
99 // Check if type matches (typically does as symbols matched with types)
100 if (auto t =
101 dynamic_cast<const GenericValue<ValueType>*>(key_value.second))
102 result[key_value.first] = t->value();
103 }
104 }
105 return result;
106 }
107
108/* ************************************************************************* */
109#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
110#include <boost/bind/bind.hpp>
111
112 template<class ValueType>
113 class Values::Filtered {
114 public:
116 typedef _ValuesKeyValuePair<ValueType> KeyValuePair;
117 typedef _ValuesConstKeyValuePair<ValueType> ConstKeyValuePair;
118 typedef KeyValuePair value_type;
119
120 typedef
121 boost::transform_iterator<
122 KeyValuePair(*)(Values::KeyValuePair),
123 boost::filter_iterator<
124 std::function<bool(const Values::ConstKeyValuePair&)>,
125 Values::iterator> >
126 iterator;
127
128 typedef iterator const_iterator;
129
130 typedef
131 boost::transform_iterator<
132 ConstKeyValuePair(*)(Values::ConstKeyValuePair),
133 boost::filter_iterator<
134 std::function<bool(const Values::ConstKeyValuePair&)>,
135 Values::const_iterator> >
136 const_const_iterator;
137
138 iterator begin() { return begin_; }
139 iterator end() { return end_; }
140 const_iterator begin() const { return begin_; }
141 const_iterator end() const { return end_; }
142 const_const_iterator beginConst() const { return constBegin_; }
143 const_const_iterator endConst() const { return constEnd_; }
144
146 size_t size() const {
147 size_t i = 0;
148 for (const_const_iterator it = beginConst(); it != endConst(); ++it)
149 ++i;
150 return i;
151 }
152
153 private:
154 Filtered(
155 const std::function<bool(const Values::ConstKeyValuePair&)>& filter,
156 Values& values) :
157 begin_(
158 boost::make_transform_iterator(
159 boost::make_filter_iterator(filter, values.begin(), values.end()),
160 &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)), end_(
161 boost::make_transform_iterator(
162 boost::make_filter_iterator(filter, values.end(), values.end()),
163 &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)), constBegin_(
164 boost::make_transform_iterator(
165 boost::make_filter_iterator(filter,
166 values._begin(),
167 values._end()),
168 &ValuesCastHelper<ValueType, ConstKeyValuePair,
169 Values::ConstKeyValuePair>::cast)), constEnd_(
170 boost::make_transform_iterator(
171 boost::make_filter_iterator(filter,
172 values._end(),
173 values._end()),
174 &ValuesCastHelper<ValueType, ConstKeyValuePair,
175 Values::ConstKeyValuePair>::cast)) {
176 }
177
178 friend class Values;
179 iterator begin_;
180 iterator end_;
181 const_const_iterator constBegin_;
182 const_const_iterator constEnd_;
183 };
184
185 template<class ValueType>
186 class Values::ConstFiltered {
187 public:
189 typedef _ValuesConstKeyValuePair<ValueType> KeyValuePair;
190 typedef KeyValuePair value_type;
191
192 typedef typename Filtered<ValueType>::const_const_iterator iterator;
193 typedef typename Filtered<ValueType>::const_const_iterator const_iterator;
194
196 ConstFiltered(const Filtered<ValueType>& rhs) :
197 begin_(rhs.beginConst()),
198 end_(rhs.endConst()) {}
199
200 iterator begin() { return begin_; }
201 iterator end() { return end_; }
202 const_iterator begin() const { return begin_; }
203 const_iterator end() const { return end_; }
204
206 size_t size() const {
207 size_t i = 0;
208 for (const_iterator it = begin(); it != end(); ++it)
209 ++i;
210 return i;
211 }
212
213 FastList<Key> keys() const {
214 FastList<Key> result;
215 for(const_iterator it = begin(); it != end(); ++it)
216 result.push_back(it->key);
217 return result;
218 }
219
220 private:
221 friend class Values;
222 const_iterator begin_;
223 const_iterator end_;
224 ConstFiltered(
225 const std::function<bool(const Values::ConstKeyValuePair&)>& filter,
226 const Values& values) {
227 // We remove the const from values to create a non-const Filtered
228 // view, then pull the const_iterators out of it.
229 const Filtered<ValueType> filtered(filter, const_cast<Values&>(values));
230 begin_ = filtered.beginConst();
231 end_ = filtered.endConst();
232 }
233 };
234
235 template<class ValueType>
236 Values::Values(const Values::Filtered<ValueType>& view) {
237 for(const auto key_value: view) {
238 Key key = key_value.key;
239 insert(key, static_cast<const ValueType&>(key_value.value));
240 }
241 }
242
243 template<class ValueType>
244 Values::Values(const Values::ConstFiltered<ValueType>& view) {
245 for(const auto key_value: view) {
246 Key key = key_value.key;
247 insert(key, static_cast<const ValueType&>(key_value.value));
248 }
249 }
250
251 Values::Filtered<Value>
252 inline Values::filter(const std::function<bool(Key)>& filterFcn) {
253 return filter<Value>(filterFcn);
254 }
255
256 template<class ValueType>
257 Values::Filtered<ValueType>
258 Values::filter(const std::function<bool(Key)>& filterFcn) {
259 return Filtered<ValueType>(std::bind(&filterHelper<ValueType>, filterFcn,
260 std::placeholders::_1), *this);
261 }
262
263 Values::ConstFiltered<Value>
264 inline Values::filter(const std::function<bool(Key)>& filterFcn) const {
265 return filter<Value>(filterFcn);
266 }
267
268 template<class ValueType>
269 Values::ConstFiltered<ValueType>
270 Values::filter(const std::function<bool(Key)>& filterFcn) const {
271 return ConstFiltered<ValueType>(std::bind(&filterHelper<ValueType>,
272 filterFcn, std::placeholders::_1), *this);
273 }
274#endif
275
276 /* ************************************************************************* */
277 template<>
278 inline bool Values::filterHelper<Value>(const std::function<bool(Key)> filter,
279 const ConstKeyValuePair& key_value) {
280 // Filter and check the type
281 return filter(key_value.key);
282 }
283
284 /* ************************************************************************* */
285
286 namespace internal {
287
288 // Check the type and throw exception if incorrect
289 // Generic version, partially specialized below for various Eigen Matrix types
290 template <typename ValueType>
291 struct handle {
292 ValueType operator()(Key j, const Value* const pointer) {
293 auto ptr = dynamic_cast<const GenericValue<ValueType>*>(pointer);
294 if (ptr) {
295 // value returns a const ValueType&, and the return makes a copy !!!!!
296 return ptr->value();
297 } else {
298 throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
299 }
300 }
301 };
302
303 template <typename MatrixType, bool isDynamic>
305
306 // Handle dynamic matrices
307 template <int M, int N>
308 struct handle_matrix<Eigen::Matrix<double, M, N>, true> {
309 inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
310 auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
311 if (ptr) {
312 // value returns a const Matrix&, and the return makes a copy !!!!!
313 return ptr->value();
314 } else {
315 // If a fixed matrix was stored, we end up here as well.
316 throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>));
317 }
318 }
319 };
320
321 // Handle fixed matrices
322 template <int M, int N>
323 struct handle_matrix<Eigen::Matrix<double, M, N>, false> {
324 inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
325 auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
326 if (ptr) {
327 // value returns a const MatrixMN&, and the return makes a copy !!!!!
328 return ptr->value();
329 } else {
330 Matrix A;
331 // Check if a dynamic matrix was stored
332 auto ptr = dynamic_cast<const GenericValue<Eigen::MatrixXd>*>(pointer);
333 if (ptr) {
334 A = ptr->value();
335 } else {
336 // Or a dynamic vector
337 A = handle_matrix<Eigen::VectorXd, true>()(j, pointer); // will throw if not....
338 }
339 // Yes: check size, and throw if not a match
340 if (A.rows() != M || A.cols() != N)
341 throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
342 else
343 return A; // copy but not malloc
344 }
345 }
346 };
347
348 // Handle matrices
349 template <int M, int N>
350 struct handle<Eigen::Matrix<double, M, N>> {
351 Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
353 (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer);
354 }
355 };
356
357 } // internal
358
359 /* ************************************************************************* */
360 template <typename ValueType>
361 const ValueType Values::at(Key j) const {
362 // Find the item
363 KeyValueMap::const_iterator item = values_.find(j);
364
365 // Throw exception if it does not exist
366 if (item == values_.end()) throw ValuesKeyDoesNotExist("at", j);
367
368 // Check the type and throw exception if incorrect
369 // h() split in two lines to avoid internal compiler error (MSVC2017)
371 return h(j, item->second);
372 }
373
374 /* ************************************************************************* */
375 template<typename ValueType>
376 boost::optional<const ValueType&> Values::exists(Key j) const {
377 // Find the item
378 KeyValueMap::const_iterator item = values_.find(j);
379
380 if(item != values_.end()) {
381 // dynamic cast the type and throw exception if incorrect
382 auto ptr = dynamic_cast<const GenericValue<ValueType>*>(item->second);
383 if (ptr) {
384 return ptr->value();
385 } else {
386 // NOTE(abe): clang warns about potential side effects if done in typeid
387 const Value* value = item->second;
388 throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
389 }
390 } else {
391 return boost::none;
392 }
393 }
394
395 /* ************************************************************************* */
396
397 // insert a templated value
398 template<typename ValueType>
399 void Values::insert(Key j, const ValueType& val) {
400 insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
401 }
402
403 // update with templated value
404 template <typename ValueType>
405 void Values::update(Key j, const ValueType& val) {
406 update(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
407 }
408
409 // insert_or_assign with templated value
410 template <typename ValueType>
411 void Values::insert_or_assign(Key j, const ValueType& val) {
412 insert_or_assign(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
413 }
414
415}
A non-templated config holding any types of Manifold-group elements.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
Wraps any type T so it can play as a Value.
Definition GenericValue.h:47
const T & value() const
Return a constant value.
Definition GenericValue.h:67
This is the base class for any type to be stored in Values.
Definition Value.h:37
Definition Values-inl.h:36
const Key key
The key.
Definition Values-inl.h:37
ValueType & value
The value.
Definition Values-inl.h:38
Definition Values-inl.h:45
const Key key
The key.
Definition Values-inl.h:46
const ValueType & value
The value.
Definition Values-inl.h:47
Definition Values-inl.h:62
Definition Values-inl.h:291
Definition Values-inl.h:304
void update(Key j, const Value &val)
single element change of existing element
Definition Values.cpp:180
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Extract a subset of values of the given type ValueType.
Definition Values-inl.h:94
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
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:157
void insert_or_assign(Key j, const Value &val)
If key j exists, update value, else perform an insert.
Definition Values.cpp:203
Values()=default
Default constructor creates an empty Values class.
bool exists(Key j) const
Check if a value exists with key j.
Definition Values.cpp:94
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:93
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:101
Definition Values.h:475
Definition Values.h:498
Definition Values.h:542
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...