gtsam  4.1.0
gtsam
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>
23 #include <gtsam/base/ConcurrentMap.h>
24 #include <gtsam/base/FastVector.h>
25 #include <gtsam/global_includes.h>
26 
27 #include <boost/shared_ptr.hpp>
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;
79 
80  public:
83  typedef boost::shared_ptr<This> shared_ptr;
86  typedef std::map<Key, size_t> Dims;
87 
90 
95 
98  VectorValues(const VectorValues& first, const VectorValues& second);
99 
101  template<class CONTAINER>
102  explicit VectorValues(const CONTAINER& c) : values_(c.begin(), c.end()) {}
103 
105  VectorValues(const VectorValues& c) : values_(c.values_) {}
106 
108  template<typename ITERATOR>
109  VectorValues(ITERATOR first, ITERATOR last) : values_(first, last) {}
110 
112  VectorValues(const Vector& c, const Dims& dims);
113 
115  VectorValues(const Vector& c, const Scatter& scatter);
116 
118  static VectorValues Zero(const VectorValues& other);
119 
123 
125  size_t size() const { return values_.size(); }
126 
128  size_t dim(Key j) const { return at(j).rows(); }
129 
131  bool exists(Key j) const { return find(j) != end(); }
132 
137  Vector& at(Key j) {
138  iterator item = find(j);
139  if (item == end())
140  throw std::out_of_range(
141  "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues.");
142  else
143  return item->second;
144  }
145 
150  const Vector& at(Key j) const {
151  const_iterator item = find(j);
152  if (item == end())
153  throw std::out_of_range(
154  "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues.");
155  else
156  return item->second;
157  }
158 
161  Vector& operator[](Key j) { return at(j); }
162 
165  const Vector& operator[](Key j) const { return at(j); }
166 
170  void update(const VectorValues& values);
171 
176  iterator insert(const std::pair<Key, Vector>& key_value);
177 
182  template<class... Args>
183  inline std::pair<VectorValues::iterator, bool> emplace(Key j, Args&&... args) {
184 #if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020)
185  return values_.emplace(std::piecewise_construct, std::forward_as_tuple(j), std::forward_as_tuple(args...));
186 #else
187  return values_.insert(std::make_pair(j, Vector(std::forward<Args>(args)...)));
188 #endif
189  }
190 
195  iterator insert(Key j, const Vector& value) {
196  return insert(std::make_pair(j, value));
197  }
198 
201  void insert(const VectorValues& values);
202 
207  inline std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
208 #ifdef TBB_GREATER_EQUAL_2020
209  return values_.emplace(j, value);
210 #else
211  return values_.insert(std::make_pair(j, value));
212 #endif
213  }
214 
216  void erase(Key var) {
217  if (values_.unsafe_erase(var) == 0)
218  throw std::invalid_argument("Requested variable '" +
219  DefaultKeyFormatter(var) +
220  "', is not in this VectorValues.");
221  }
222 
224  void setZero();
225 
226  iterator begin() { return values_.begin(); }
227  const_iterator begin() const { return values_.begin(); }
228  iterator end() { return values_.end(); }
229  const_iterator end() const { return values_.end(); }
230 
235  iterator find(Key j) { return values_.find(j); }
236 
241  const_iterator find(Key j) const { return values_.find(j); }
242 
244  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream&, const VectorValues&);
245 
247  void print(const std::string& str = "VectorValues",
248  const KeyFormatter& formatter = DefaultKeyFormatter) const;
249 
251  bool equals(const VectorValues& x, double tol = 1e-9) const;
252 
256 
258  Vector vector() const;
259 
261  template <typename CONTAINER>
262  Vector vector(const CONTAINER& keys) const {
263  DenseIndex totalDim = 0;
264  FastVector<const Vector*> items;
265  items.reserve(keys.end() - keys.begin());
266  for (Key key : keys) {
267  const Vector* v = &at(key);
268  totalDim += v->size();
269  items.push_back(v);
270  }
271 
272  Vector result(totalDim);
273  DenseIndex pos = 0;
274  for (const Vector* v : items) {
275  result.segment(pos, v->size()) = *v;
276  pos += v->size();
277  }
278 
279  return result;
280  }
281 
283  Vector vector(const Dims& dims) const;
284 
286  void swap(VectorValues& other);
287 
289  bool hasSameStructure(const VectorValues other) const;
290 
294 
298  double dot(const VectorValues& v) const;
299 
301  double norm() const;
302 
304  double squaredNorm() const;
305 
308  VectorValues operator+(const VectorValues& c) const;
309 
312  VectorValues add(const VectorValues& c) const;
313 
316  VectorValues& operator+=(const VectorValues& c);
317 
320  VectorValues& addInPlace(const VectorValues& c);
321 
323  VectorValues& addInPlace_(const VectorValues& c);
324 
327  VectorValues operator-(const VectorValues& c) const;
328 
331  VectorValues subtract(const VectorValues& c) const;
332 
334  friend GTSAM_EXPORT VectorValues operator*(const double a, const VectorValues &v);
335 
337  VectorValues scale(const double a) const;
338 
340  VectorValues& operator*=(double alpha);
341 
343  VectorValues& scaleInPlace(double alpha);
344 
346 
350 
351  //inline VectorValues scale(const double a, const VectorValues& c) const { return a * (*this); }
352 
354 
355  private:
357  friend class boost::serialization::access;
358  template<class ARCHIVE>
359  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
360  ar & BOOST_SERIALIZATION_NVP(values_);
361  }
362  }; // VectorValues definition
363 
365  template<>
366  struct traits<VectorValues> : public Testable<VectorValues> {
367  };
368 
369 } // \namespace gtsam
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
Scatter.h
Maps global variable indices to slot indices.
gtsam::VectorValues::VectorValues
VectorValues(const VectorValues &c)
Implicit copy constructor to specialize the explicit constructor from any container.
Definition: VectorValues.h:105
gtsam::ConcurrentMap< Key, Vector >
gtsam::VectorValues::end
const_iterator end() const
Iterator over variables.
Definition: VectorValues.h:229
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::VectorValues::values_
Values values_
Vectors making up this VectorValues.
Definition: VectorValues.h:78
gtsam::VectorValues::emplace
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Emplace a vector value with key j.
Definition: VectorValues.h:183
gtsam::VectorValues::VectorValues
VectorValues(const CONTAINER &c)
Create from another container holding pair<Key,Vector>.
Definition: VectorValues.h:102
gtsam::VectorValues::size
size_t size() const
Number of variables stored.
Definition: VectorValues.h:125
gtsam::VectorValues::find
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:241
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
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::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::VectorValues::vector
Vector vector(const CONTAINER &keys) const
Access a vector that is a subset of relevant keys.
Definition: VectorValues.h:262
gtsam::VectorValues::erase
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:216
gtsam::VectorValues::begin
const_iterator begin() const
Iterator over variables.
Definition: VectorValues.h:227
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
global_includes.h
Included from all GTSAM files.
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::VectorValues::tryInsert
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:207
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::VectorValues::operator[]
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:161
Vector.h
typedef and functions to augment Eigen's VectorXd
gtsam::VectorValues::VectorValues
VectorValues(ITERATOR first, ITERATOR last)
Create from a pair of iterators over pair<Key,Vector>.
Definition: VectorValues.h:109
gtsam::VectorValues::dim
size_t dim(Key j) const
Return the dimension of variable j.
Definition: VectorValues.h:128
gtsam::operator-
BinarySumExpression< T > operator-(const Expression< T > &e1, const Expression< T > &e2)
Construct an expression that subtracts one expression from another.
Definition: Expression.h:279
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:67
gtsam::VectorValues::begin
iterator begin()
Iterator over variables.
Definition: VectorValues.h:226
gtsam::Scatter
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition: Scatter.h:49
gtsam::VectorValues::VectorValues
VectorValues()
Default constructor creates an empty VectorValues.
Definition: VectorValues.h:94
gtsam::VectorValues::Values
ConcurrentMap< Key, Vector > Values
Collection of Vectors making up a VectorValues.
Definition: VectorValues.h:77
gtsam::KeyFormatter
boost::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
gtsam::VectorValues::const_iterator
Values::const_iterator const_iterator
Const iterator over vector values.
Definition: VectorValues.h:82
gtsam::VectorValues::value_type
Values::value_type value_type
Typedef to pair<Key, Vector>
Definition: VectorValues.h:84
gtsam::VectorValues::operator[]
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:165
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:45
gtsam::VectorValues::KeyValuePair
value_type KeyValuePair
Typedef to pair<Key, Vector>
Definition: VectorValues.h:85
gtsam::VectorValues::Dims
std::map< Key, size_t > Dims
Keyed vector dimensions.
Definition: VectorValues.h:86
gtsam::VectorValues::iterator
Values::iterator iterator
Iterator over vector values.
Definition: VectorValues.h:81
Ordering.h
Variable ordering for the elimination algorithm.
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
gtsam::VectorValues::at
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:150
gtsam::VectorValues::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: VectorValues.h:83
gtsam::VectorValues::exists
bool exists(Key j) const
Check whether a variable with key j exists.
Definition: VectorValues.h:131
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:194
gtsam::VectorValues::end
iterator end()
Iterator over variables.
Definition: VectorValues.h:228
gtsam::VectorValues::find
iterator find(Key j)
Return the iterator corresponding to the requested key, or end() if no variable is present with this ...
Definition: VectorValues.h:235
gtsam::VectorValues::insert
iterator insert(Key j, const Vector &value)
Insert a vector value with key j.
Definition: VectorValues.h:195
gtsam::VectorValues::at
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:137
gtsam::operator+
BinarySumExpression< T > operator+(const Expression< T > &e1, const Expression< T > &e2)
Construct an expression that sums two input expressions of the same type T The type T must be a vecto...
Definition: Expression.h:273