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 
20 #include <gtsam/linear/Scatter.h>
22 #include <gtsam/base/Vector.h>
24 #include <gtsam/base/FastVector.h>
25 #include <gtsam/global_includes.h>
26 
27 #include <memory>
28 
29 
30 #include <map>
31 #include <string>
32 #include <iosfwd>
33 
34 namespace gtsam {
35 
74  class GTSAM_EXPORT VectorValues {
75  protected:
76  typedef VectorValues This;
78  Values values_;
79 
80  public:
81  typedef Values::iterator iterator;
82  typedef Values::const_iterator const_iterator;
83  typedef std::shared_ptr<This> shared_ptr;
85  typedef value_type KeyValuePair;
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 
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 
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({j, Vector(std::forward<Args>(args)...)});
190 #endif
191  }
192 
197  iterator insert(Key j, const Vector& value) {
198  return insert({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({j, value});
214 #endif
215  }
216 
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",
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 
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:
371 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
372 
373  friend class boost::serialization::access;
374  template<class ARCHIVE>
375  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
376  ar & BOOST_SERIALIZATION_NVP(values_);
377  }
378 #endif
379  }; // VectorValues definition
380 
382  template<>
383  struct traits<VectorValues> : public Testable<VectorValues> {
384  };
385 
386 } // \namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
const gtsam::Symbol key('X', 0)
iterator find(Key j)
Definition: VectorValues.h:245
const_iterator begin() const
Iterator over variables.
Definition: VectorValues.h:237
A insert(1, 2)=0
iterator insert(Key j, const Vector &value)
Definition: VectorValues.h:197
EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 & operator+=(bfloat16 &a, const bfloat16 &b)
Definition: BFloat16.h:184
VectorValues(const CONTAINER &c)
Definition: VectorValues.h:104
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: VectorValues.h:83
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
Values::iterator iterator
Iterator over vector values.
Definition: VectorValues.h:81
def update(text)
Definition: relicense.py:46
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
const_iterator find(Key j) const
Definition: VectorValues.h:251
std::string serialize(const T &input)
serializes to a string
Vector & operator[](Key j)
Definition: VectorValues.h:163
Definition: pytypes.h:2012
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
Definition: FastVector.h:34
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
leaf::MyValues values
Values::value_type value_type
Typedef to pair<Key, Vector>
Definition: VectorValues.h:84
iterator end()
Iterator over variables.
Definition: VectorValues.h:238
std::map< Key, size_t > Dims
Keyed vector dimensions.
Definition: VectorValues.h:86
Vector & at(Key j)
Definition: VectorValues.h:139
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:52
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
static const symbolic::SymbolExpr< internal::symbolic_last_tag > last
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:108
const KeyFormatter & formatter
std::pair< iterator, bool > tryInsert(Key j, const Vector &value)
Definition: VectorValues.h:209
Included from all GTSAM files.
Vector vector(const CONTAINER &keys) const
Definition: VectorValues.h:272
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Definition: VectorValues.h:185
bool exists(Key j) const
Definition: VectorValues.h:133
const Vector & operator[](Key j) const
Definition: VectorValues.h:167
ConcurrentMap< Key, Vector > Values
Collection of Vectors making up a VectorValues.
Definition: VectorValues.h:77
int EIGEN_BLAS_FUNC() swap(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Definition: level1_impl.h:130
KeyValuePair value_type
Definition: Values.h:106
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of html.
VectorValues()
Default constructor creates an empty VectorValues.
Definition: VectorValues.h:92
Definition: pytypes.h:1403
void insert_or_assign(Key j, const Vector &value)
Definition: VectorValues.h:219
Array< int, Dynamic, 1 > v
VectorValues(ITERATOR first, ITERATOR last)
Definition: VectorValues.h:111
size_t dim(Key j) const
Definition: VectorValues.h:130
Values values_
Vectors making up this VectorValues.
Definition: VectorValues.h:78
Errors operator+(const Errors &a, const Errors &b)
Addition.
Definition: Errors.cpp:59
Errors operator-(const Errors &a, const Errors &b)
Subtraction.
Definition: Errors.cpp:74
RealScalar alpha
Array< double, 1, 3 > e(1./3., 0.5, 2.)
value_type KeyValuePair
Typedef to pair<Key, Vector>
Definition: VectorValues.h:85
Values::const_iterator const_iterator
Const iterator over vector values.
Definition: VectorValues.h:82
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
Maps global variable indices to slot indices.
VectorValues(const VectorValues &c)
Definition: VectorValues.h:107
A thin wrapper around std::vector that uses a custom allocator.
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 & operator*=(bfloat16 &a, const bfloat16 &b)
Definition: BFloat16.h:188
void unsafe_erase(typename Base::iterator position)
Definition: ConcurrentMap.h:94
VectorValues(std::initializer_list< std::pair< Key, Vector >> init)
Construct from initializer list.
Definition: VectorValues.h:95
static double scale(double x, double a, double b, double t1, double t2)
Scale x from [a, b] to [t1, t2].
Definition: Chebyshev.cpp:35
static EIGEN_DEPRECATED const end_t end
void erase(Key var)
Definition: VectorValues.h:226
const_iterator end() const
Iterator over variables.
Definition: VectorValues.h:239
size_t size() const
Definition: VectorValues.h:127
graph add(PriorFactor< Pose2 >(1, priorMean, priorNoise))
const Vector & at(Key j) const
Definition: VectorValues.h:152
const G double tol
Definition: Group.h:86
const KeyVector keys
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
iterator begin()
Iterator over variables.
Definition: VectorValues.h:236
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
std::ptrdiff_t j
VectorValues This
Definition: VectorValues.h:76
v setZero(3)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:43