gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
VectorValues.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
18#pragma once
19
22#include <gtsam/base/Vector.h>
23#include <gtsam/base/ConcurrentMap.h>
26
27#include <boost/shared_ptr.hpp>
28
29
30#include <map>
31#include <string>
32#include <iosfwd>
33
34namespace gtsam {
35
74 class GTSAM_EXPORT VectorValues {
75 protected:
76 typedef VectorValues This;
79
80 public:
81 typedef Values::iterator iterator;
82 typedef Values::const_iterator const_iterator;
83 typedef boost::shared_ptr<This> shared_ptr;
86 typedef std::map<Key, size_t> Dims;
87
90
93
95 VectorValues(std::initializer_list<std::pair<Key, Vector>> init)
96 : values_(init.begin(), init.end()) {}
97
100 VectorValues(const VectorValues& first, const VectorValues& second);
101
103 template<class CONTAINER>
104 explicit VectorValues(const CONTAINER& c) : values_(c.begin(), c.end()) {}
105
107 VectorValues(const VectorValues& c) : values_(c.values_) {}
108
110 template<typename ITERATOR>
111 VectorValues(ITERATOR first, ITERATOR last) : values_(first, last) {}
112
114 VectorValues(const Vector& c, const Dims& dims);
115
117 VectorValues(const Vector& c, const Scatter& scatter);
118
120 static VectorValues Zero(const VectorValues& other);
121
125
127 size_t size() const { return values_.size(); }
128
130 size_t dim(Key j) const { return at(j).rows(); }
131
133 bool exists(Key j) const { return find(j) != end(); }
134
139 Vector& at(Key j) {
140 iterator item = find(j);
141 if (item == end())
142 throw std::out_of_range(
143 "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues.");
144 else
145 return item->second;
146 }
147
152 const Vector& at(Key j) const {
153 const_iterator item = find(j);
154 if (item == end())
155 throw std::out_of_range(
156 "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues.");
157 else
158 return item->second;
159 }
160
163 Vector& operator[](Key j) { return at(j); }
164
167 const Vector& operator[](Key j) const { return at(j); }
168
172 VectorValues& update(const VectorValues& values);
173
178 iterator insert(const std::pair<Key, Vector>& key_value);
179
184 template<class... Args>
185 inline std::pair<VectorValues::iterator, bool> emplace(Key j, Args&&... args) {
186#if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020)
187 return values_.emplace(std::piecewise_construct, std::forward_as_tuple(j), std::forward_as_tuple(args...));
188#else
189 return values_.insert(std::make_pair(j, Vector(std::forward<Args>(args)...)));
190#endif
191 }
192
197 iterator insert(Key j, const Vector& value) {
198 return insert(std::make_pair(j, value));
199 }
200
203 VectorValues& insert(const VectorValues& values);
204
209 inline std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
210#ifdef TBB_GREATER_EQUAL_2020
211 return values_.emplace(j, value);
212#else
213 return values_.insert(std::make_pair(j, value));
214#endif
215 }
216
219 void insert_or_assign(Key j, const Vector& value) {
220 if (!tryInsert(j, value).second) {
221 (*this)[j] = value;
222 }
223 }
224
226 void erase(Key var) {
227 if (values_.unsafe_erase(var) == 0)
228 throw std::invalid_argument("Requested variable '" +
229 DefaultKeyFormatter(var) +
230 "', is not in this VectorValues.");
231 }
232
234 void setZero();
235
236 iterator begin() { return values_.begin(); }
237 const_iterator begin() const { return values_.begin(); }
238 iterator end() { return values_.end(); }
239 const_iterator end() const { return values_.end(); }
240
245 iterator find(Key j) { return values_.find(j); }
246
251 const_iterator find(Key j) const { return values_.find(j); }
252
254 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream&, const VectorValues&);
255
257 void print(const std::string& str = "VectorValues",
258 const KeyFormatter& formatter = DefaultKeyFormatter) const;
259
261 bool equals(const VectorValues& x, double tol = 1e-9) const;
262
266
268 Vector vector() const;
269
271 template <typename CONTAINER>
272 Vector vector(const CONTAINER& keys) const {
273 DenseIndex totalDim = 0;
275 items.reserve(keys.end() - keys.begin());
276 for (Key key : keys) {
277 const Vector* v = &at(key);
278 totalDim += v->size();
279 items.push_back(v);
280 }
281
282 Vector result(totalDim);
283 DenseIndex pos = 0;
284 for (const Vector* v : items) {
285 result.segment(pos, v->size()) = *v;
286 pos += v->size();
287 }
288
289 return result;
290 }
291
293 Vector vector(const Dims& dims) const;
294
296 void swap(VectorValues& other);
297
299 bool hasSameStructure(const VectorValues other) const;
300
304
308 double dot(const VectorValues& v) const;
309
311 double norm() const;
312
314 double squaredNorm() const;
315
318 VectorValues operator+(const VectorValues& c) const;
319
322 VectorValues add(const VectorValues& c) const;
323
326 VectorValues& operator+=(const VectorValues& c);
327
330 VectorValues& addInPlace(const VectorValues& c);
331
333 VectorValues& addInPlace_(const VectorValues& c);
334
337 VectorValues operator-(const VectorValues& c) const;
338
341 VectorValues subtract(const VectorValues& c) const;
342
344 friend GTSAM_EXPORT VectorValues operator*(const double a, const VectorValues &v);
345
347 VectorValues scale(const double a) const;
348
350 VectorValues& operator*=(double alpha);
351
353 VectorValues& scaleInPlace(double alpha);
354
356
359
365 std::string html(
366 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
367
369
370 private:
372 friend class boost::serialization::access;
373 template<class ARCHIVE>
374 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
375 ar & BOOST_SERIALIZATION_NVP(values_);
376 }
377 }; // VectorValues definition
378
380 template<>
381 struct traits<VectorValues> : public Testable<VectorValues> {
382 };
383
384} // \namespace gtsam
A thin wrapper around std::vector that uses a custom allocator.
typedef and functions to augment Eigen's VectorXd
Variable ordering for the elimination algorithm.
Maps global variable indices to slot indices.
Included from all GTSAM files.
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
Global functions in a separate testing namespace.
Definition chartTesting.h:28
string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of html.
Definition DiscreteValues.cpp:134
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
Errors operator+(const Errors &a, const Errors &b)
Addition.
Definition Errors.cpp:60
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
Errors operator-(const Errors &a, const Errors &b)
Subtraction.
Definition Errors.cpp:75
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition ConcurrentMap.h:68
Template to create a binary predicate.
Definition Testable.h:111
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition Scatter.h:49
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
value_type KeyValuePair
Typedef to pair<Key, Vector>
Definition VectorValues.h:85
const_iterator end() const
Iterator over variables.
Definition VectorValues.h:239
Values::value_type value_type
Typedef to pair<Key, Vector>
Definition VectorValues.h:84
Values::const_iterator const_iterator
Const iterator over vector values.
Definition VectorValues.h:82
iterator end()
Iterator over variables.
Definition VectorValues.h:238
Values::iterator iterator
Iterator over vector values.
Definition VectorValues.h:81
iterator insert(Key j, const Vector &value)
Insert a vector value with key j.
Definition VectorValues.h:197
VectorValues(const CONTAINER &c)
Create from another container holding pair<Key,Vector>.
Definition VectorValues.h:104
Values values_
Vectors making up this VectorValues.
Definition VectorValues.h:78
std::map< Key, size_t > Dims
Keyed vector dimensions.
Definition VectorValues.h:86
ConcurrentMap< Key, Vector > Values
Collection of Vectors making up a VectorValues.
Definition VectorValues.h:77
Vector & operator[](Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist,...
Definition VectorValues.h:163
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Emplace a vector value with key j.
Definition VectorValues.h:185
iterator find(Key j)
Return the iterator corresponding to the requested key, or end() if no variable is present with this ...
Definition VectorValues.h:245
size_t dim(Key j) const
Return the dimension of variable j.
Definition VectorValues.h:130
size_t size() const
Number of variables stored.
Definition VectorValues.h:127
const Vector & operator[](Key j) const
Access the vector value with key j (const version), throws std::out_of_range if j does not exist,...
Definition VectorValues.h:167
bool exists(Key j) const
Check whether a variable with key j exists.
Definition VectorValues.h:133
VectorValues()
Default constructor creates an empty VectorValues.
Definition VectorValues.h:92
const_iterator find(Key j) const
Return the iterator corresponding to the requested key, or end() if no variable is present with this ...
Definition VectorValues.h:251
VectorValues(ITERATOR first, ITERATOR last)
Create from a pair of iterators over pair<Key,Vector>.
Definition VectorValues.h:111
const Vector & at(Key j) const
Access the vector value with key j (const version), throws std::out_of_range if j does not exist,...
Definition VectorValues.h:152
Vector vector(const CONTAINER &keys) const
Access a vector that is a subset of relevant keys.
Definition VectorValues.h:272
void insert_or_assign(Key j, const Vector &value)
insert_or_assign that mimics the STL map insert_or_assign - if the value already exists,...
Definition VectorValues.h:219
const_iterator begin() const
Iterator over variables.
Definition VectorValues.h:237
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist,...
Definition VectorValues.h:139
void erase(Key var)
Erase the vector with the given key, or throw std::out_of_range if it does not exist.
Definition VectorValues.h:226
VectorValues(std::initializer_list< std::pair< Key, Vector > > init)
Construct from initializer list.
Definition VectorValues.h:95
std::pair< iterator, bool > tryInsert(Key j, const Vector &value)
insert that mimics the STL map insert - if the value already exists, the map is not modified and an i...
Definition VectorValues.h:209
VectorValues(const VectorValues &c)
Implicit copy constructor to specialize the explicit constructor from any container.
Definition VectorValues.h:107
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition VectorValues.h:83
iterator begin()
Iterator over variables.
Definition VectorValues.h:236
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:93