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
12/**
13 * @file Values.h
14 * @author Richard Roberts
15 *
16 * @brief A non-templated config holding any types of Manifold-group elements
17 *
18 * Detailed story:
19 * A values structure is a map from keys to values. It is used to specify the value of a bunch
20 * of variables in a factor graph. A Values is a values structure which can hold variables that
21 * are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
22 * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
23 */
24
25#pragma once
26
27#include <utility>
28#include <gtsam/nonlinear/Values.h>
29
30namespace gtsam {
31
32
33 /* ************************************************************************* */
34 template<class ValueType>
35 struct _ValuesKeyValuePair {
36 const Key key; ///< The key
37 ValueType& value; ///< The value
38
39 _ValuesKeyValuePair(Key _key, ValueType& _value) : key(_key), value(_value) {}
40 };
41
42 /* ************************************************************************* */
43 template<class ValueType>
44 struct _ValuesConstKeyValuePair {
45 const Key key; ///< The key
46 const ValueType& value; ///< The value
47
48 _ValuesConstKeyValuePair(Key _key, const ValueType& _value) :
49 key(_key), value(_value) {
50 }
51 _ValuesConstKeyValuePair(const _ValuesKeyValuePair<ValueType>& rhs) :
52 key(rhs.key), value(rhs.value) {
53 }
54 };
55
56 /* ************************************************************************* */
57
58 // Cast helpers for making _Values[Const]KeyValuePair's from Values::[Const]KeyValuePair
59 // need to use a struct here for later partial specialization
60 template<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
61 struct ValuesCastHelper {
62 static CastedKeyValuePairType cast(KeyValuePairType key_value) {
63 // Static cast because we already checked the type during filtering
64 return CastedKeyValuePairType(key_value.key,
65 const_cast<GenericValue<ValueType>&>(static_cast<const GenericValue<
66 ValueType>&>(key_value.value)).value());
67 }
68 };
69 // partial specialized version for ValueType == Value
70 template<class CastedKeyValuePairType, class KeyValuePairType>
71 struct ValuesCastHelper<Value, CastedKeyValuePairType, KeyValuePairType> {
72 static CastedKeyValuePairType cast(KeyValuePairType key_value) {
73 // Static cast because we already checked the type during filtering
74 // in this case the casted and keyvalue pair are essentially the same type
75 // (key, Value&) so perhaps this could be done with just a cast of the key_value?
76 return CastedKeyValuePairType(key_value.key, key_value.value);
77 }
78 };
79 // partial specialized version for ValueType == Value
80 template<class CastedKeyValuePairType, class KeyValuePairType>
81 struct ValuesCastHelper<const Value, CastedKeyValuePairType, KeyValuePairType> {
82 static CastedKeyValuePairType cast(KeyValuePairType key_value) {
83 // Static cast because we already checked the type during filtering
84 // in this case the casted and keyvalue pair are essentially the same type
85 // (key, Value&) so perhaps this could be done with just a cast of the key_value?
86 return CastedKeyValuePairType(key_value.key, key_value.value);
87 }
88 };
89
90 /* ************************************************************************* */
91 template <class ValueType>
92 size_t Values::count() const {
93 size_t i = 0;
94 for (const auto& [_, value] : values_) {
95 if (dynamic_cast<const GenericValue<ValueType>*>(value.get())) ++i;
96 }
97 return i;
98 }
99
100 /* ************************************************************************* */
101 template <class ValueType>
102 std::map<Key, ValueType>
103 Values::extract(const std::function<bool(Key)>& filterFcn) const {
104 std::map<Key, ValueType> result;
105 for (const auto& [key,value] : values_) {
106 // Check if key matches
107 if (filterFcn(key)) {
108 // Check if type matches (typically does as symbols matched with types)
109 if (auto t =
110 dynamic_cast<const GenericValue<ValueType>*>(value.get()))
111 result[key] = t->value();
112 }
113 }
114 return result;
115 }
116
117 /* ************************************************************************* */
118 template<>
119 inline bool Values::filterHelper<Value>(const std::function<bool(Key)> filter,
120 const ConstKeyValuePair& key_value) {
121 // Filter and check the type
122 return filter(key_value.key);
123 }
124
125 /* ************************************************************************* */
126
127 namespace internal {
128
129 // Check the type and throw exception if incorrect
130 // Generic version, partially specialized below for various Eigen Matrix types
131 template <typename ValueType>
132 struct handle {
133 ValueType operator()(Key j, const Value* const pointer) {
134 auto ptr = dynamic_cast<const GenericValue<ValueType>*>(pointer);
135 if (ptr) {
136 // value returns a const ValueType&, and the return makes a copy !!!!!
137 return ptr->value();
138 } else {
139 throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
140 }
141 }
142 };
143
144 template <typename MatrixType, bool isDynamic>
145 struct handle_matrix;
146
147 // Handle dynamic matrices
148 template <int M, int N>
149 struct handle_matrix<Eigen::Matrix<double, M, N>, true> {
150 inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
151 auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
152 if (ptr) {
153 // value returns a const Matrix&, and the return makes a copy !!!!!
154 return ptr->value();
155 } else {
156 // If a fixed matrix was stored, we end up here as well.
157 throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>));
158 }
159 }
160 };
161
162 // Handle fixed matrices
163 template <int M, int N>
164 struct handle_matrix<Eigen::Matrix<double, M, N>, false> {
165 inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
166 auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
167 if (ptr) {
168 // value returns a const MatrixMN&, and the return makes a copy !!!!!
169 return ptr->value();
170 } else {
171 Matrix A;
172 // Check if a dynamic matrix was stored
173 auto ptr = dynamic_cast<const GenericValue<Eigen::MatrixXd>*>(pointer);
174 if (ptr) {
175 A = ptr->value();
176 } else {
177 // Or a dynamic vector
178 A = handle_matrix<Eigen::VectorXd, true>()(j, pointer); // will throw if not....
179 }
180 // Yes: check size, and throw if not a match
181 if (A.rows() != M || A.cols() != N)
182 throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
183 else
184 return A; // copy but not malloc
185 }
186 }
187 };
188
189 // Handle matrices
190 template <int M, int N>
191 struct handle<Eigen::Matrix<double, M, N>> {
192 Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
193 return handle_matrix<Eigen::Matrix<double, M, N>,
194 (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer);
195 }
196 };
197
198// Added this section for compile gtsam python on windows.
199// msvc don't deduct the template arguments correctly, due possible bug in msvc.
200#ifdef _WIN32
201#if _MSC_VER < 1937
202 // Handle dynamic matrices
203 template <int M, int N>
204 struct handle_matrix<Eigen::Matrix<double, M, N, 0, M, N>, true> {
205 inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
206 auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
207 if (ptr) {
208 // value returns a const Matrix&, and the return makes a copy !!!!!
209 return ptr->value();
210 } else {
211 // If a fixed matrix was stored, we end up here as well.
212 throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>));
213 }
214 }
215 };
216
217 // Handle fixed matrices
218 template <int M, int N>
219 struct handle_matrix<Eigen::Matrix<double, M, N, 0, M, N>, false> {
220 inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
221 auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
222 if (ptr) {
223 // value returns a const MatrixMN&, and the return makes a copy !!!!!
224 return ptr->value();
225 } else {
226 Matrix A;
227 // Check if a dynamic matrix was stored
228 auto ptr = dynamic_cast<const GenericValue<Eigen::MatrixXd>*>(pointer);
229 if (ptr) {
230 A = ptr->value();
231 } else {
232 // Or a dynamic vector
233 A = handle_matrix<Eigen::VectorXd, true>()(j, pointer); // will throw if not....
234 }
235 // Yes: check size, and throw if not a match
236 if (A.rows() != M || A.cols() != N)
237 throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
238 else
239 return A; // copy but not malloc
240 }
241 }
242 };
243
244 // Handle matrices
245 template <int M, int N>
246 struct handle<Eigen::Matrix<double, M, N, 0, M, N>> {
247 Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
248 return handle_matrix<Eigen::Matrix<double, M, N, 0, M, N>,
249 (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer);
250 }
251 };
252#endif // #if _MSC_VER < 1937
253#endif // #ifdef _WIN32
254
255 } // internal
256
257 /* ************************************************************************* */
258 template <typename ValueType>
259 const ValueType Values::at(Key j) const {
260 // Find the item
261 KeyValueMap::const_iterator item = values_.find(x: j);
262
263 // Throw exception if it does not exist
264 if (item == values_.end()) throw ValuesKeyDoesNotExist("at", j);
265
266 // Check the type and throw exception if incorrect
267 // h() split in two lines to avoid internal compiler error (MSVC2017)
268 auto h = internal::handle<ValueType>();
269 return h(j, item->second.get());
270 }
271
272 /* ************************************************************************* */
273 template<typename ValueType>
274 const ValueType * Values::exists(Key j) const {
275 // Find the item
276 KeyValueMap::const_iterator item = values_.find(x: j);
277
278 if(item != values_.end()) {
279 const Value* value = item->second.get();
280 // dynamic cast the type and throw exception if incorrect
281 auto ptr = dynamic_cast<const GenericValue<ValueType>*>(value);
282 if (ptr) {
283 return &ptr->value();
284 } else {
285 // NOTE(abe): clang warns about potential side effects if done in typeid
286 throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
287 }
288 } else {
289 return nullptr;
290 }
291 }
292
293 /* ************************************************************************* */
294
295 // insert a templated value
296 template<typename ValueType>
297 void Values::insert(Key j, const ValueType& val) {
298 insert(j, val: static_cast<const Value&>(GenericValue<ValueType>(val)));
299 }
300
301 // partial specialization to insert an expression involving unary operators
302 template <typename UnaryOp, typename ValueType>
303 void Values::insert(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val) {
304 insert(j, val.eval());
305 }
306
307 // partial specialization to insert an expression involving binary operators
308 template <typename BinaryOp, typename ValueType1, typename ValueType2>
309 void Values::insert(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val) {
310 insert(j, val.eval());
311 }
312
313 // update with templated value
314 template <typename ValueType>
315 void Values::update(Key j, const ValueType& val) {
316 update(j, val: static_cast<const Value&>(GenericValue<ValueType>(val)));
317 }
318
319 // partial specialization to update with an expression involving unary operators
320 template <typename UnaryOp, typename ValueType>
321 void Values::update(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val) {
322 update(j, val.eval());
323 }
324
325 // partial specialization to update with an expression involving binary operators
326 template <typename BinaryOp, typename ValueType1, typename ValueType2>
327 void Values::update(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val) {
328 update(j, val.eval());
329 }
330
331 // insert_or_assign with templated value
332 template <typename ValueType>
333 void Values::insert_or_assign(Key j, const ValueType& val) {
334 insert_or_assign(j, val: static_cast<const Value&>(GenericValue<ValueType>(val)));
335 }
336
337 template <typename UnaryOp, typename ValueType>
338 void Values::insert_or_assign(Key j, const Eigen::CwiseUnaryOp<UnaryOp, const ValueType>& val) {
339 insert_or_assign(j, val.eval());
340 }
341
342 template <typename BinaryOp, typename ValueType1, typename ValueType2>
343 void Values::insert_or_assign(Key j, const Eigen::CwiseBinaryOp<BinaryOp, const ValueType1, const ValueType2>& val) {
344 insert_or_assign(j, val.eval());
345 }
346
347}
348