Loading...
Searching...
No Matches
VectorValues.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/linear/Scatter.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/ConcurrentMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/global_includes.h>
#include <boost/shared_ptr.hpp>
#include <map>
#include <string>
#include <iosfwd>
namespace gtsam {
class GTSAM_EXPORT VectorValues {
protected:
typedef VectorValues This;
typedef ConcurrentMap<Key, Vector> Values;
Values values_;
public:
typedef Values::iterator iterator;
typedef Values::const_iterator const_iterator;
typedef boost::shared_ptr<This> shared_ptr;
typedef Values::value_type value_type;
typedef value_type KeyValuePair;
typedef std::map<Key, size_t> Dims;
VectorValues() {}
VectorValues(std::initializer_list<std::pair<Key, Vector>> init)
: values_(init.begin(), init.end()) {}
VectorValues(const VectorValues& first, const VectorValues& second);
template<class CONTAINER>
explicit VectorValues(const CONTAINER& c) : values_(c.begin(), c.end()) {}
VectorValues(const VectorValues& c) : values_(c.values_) {}
template<typename ITERATOR>
VectorValues(ITERATOR first, ITERATOR last) : values_(first, last) {}
VectorValues(const Vector& c, const Dims& dims);
VectorValues(const Vector& c, const Scatter& scatter);
static VectorValues Zero(const VectorValues& other);
size_t size() const { return values_.size(); }
size_t dim(Key j) const { return at(j).rows(); }
bool exists(Key j) const { return find(j) != end(); }
Vector& at(Key j) {
iterator item = find(j);
if (item == end())
throw std::out_of_range(
"Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues.");
else
return item->second;
}
const Vector& at(Key j) const {
const_iterator item = find(j);
if (item == end())
throw std::out_of_range(
"Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues.");
else
return item->second;
}
Vector& operator[](Key j) { return at(j); }
const Vector& operator[](Key j) const { return at(j); }
VectorValues& update(const VectorValues& values);
iterator insert(const std::pair<Key, Vector>& key_value);
template<class... Args>
inline std::pair<VectorValues::iterator, bool> emplace(Key j, Args&&... args) {
#if ! defined(GTSAM_USE_TBB) || defined (TBB_GREATER_EQUAL_2020)
return values_.emplace(std::piecewise_construct, std::forward_as_tuple(j), std::forward_as_tuple(args...));
#else
return values_.insert(std::make_pair(j, Vector(std::forward<Args>(args)...)));
#endif
}
iterator insert(Key j, const Vector& value) {
return insert(std::make_pair(j, value));
}
VectorValues& insert(const VectorValues& values);
inline std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
#ifdef TBB_GREATER_EQUAL_2020
return values_.emplace(j, value);
#else
return values_.insert(std::make_pair(j, value));
#endif
}
void insert_or_assign(Key j, const Vector& value) {
if (!tryInsert(j, value).second) {
(*this)[j] = value;
}
}
void erase(Key var) {
if (values_.unsafe_erase(var) == 0)
throw std::invalid_argument("Requested variable '" +
DefaultKeyFormatter(var) +
"', is not in this VectorValues.");
}
void setZero();
iterator begin() { return values_.begin(); }
const_iterator begin() const { return values_.begin(); }
iterator end() { return values_.end(); }
const_iterator end() const { return values_.end(); }
iterator find(Key j) { return values_.find(j); }
const_iterator find(Key j) const { return values_.find(j); }
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream&, const VectorValues&);
void print(const std::string& str = "VectorValues",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
bool equals(const VectorValues& x, double tol = 1e-9) const;
Vector vector() const;
template <typename CONTAINER>
Vector vector(const CONTAINER& keys) const {
DenseIndex totalDim = 0;
FastVector<const Vector*> items;
items.reserve(keys.end() - keys.begin());
for (Key key : keys) {
const Vector* v = &at(key);
totalDim += v->size();
items.push_back(v);
}
Vector result(totalDim);
DenseIndex pos = 0;
for (const Vector* v : items) {
result.segment(pos, v->size()) = *v;
pos += v->size();
}
return result;
}
Vector vector(const Dims& dims) const;
void swap(VectorValues& other);
bool hasSameStructure(const VectorValues other) const;
double dot(const VectorValues& v) const;
double norm() const;
double squaredNorm() const;
VectorValues operator+(const VectorValues& c) const;
VectorValues add(const VectorValues& c) const;
VectorValues& operator+=(const VectorValues& c);
VectorValues& addInPlace(const VectorValues& c);
VectorValues& addInPlace_(const VectorValues& c);
VectorValues operator-(const VectorValues& c) const;
VectorValues subtract(const VectorValues& c) const;
friend GTSAM_EXPORT VectorValues operator*(const double a, const VectorValues &v);
VectorValues scale(const double a) const;
VectorValues& operator*=(double alpha);
VectorValues& scaleInPlace(double alpha);
std::string html(
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(values_);
}
}; // VectorValues definition
template<>
struct traits<VectorValues> : public Testable<VectorValues> {
};
} // \namespace gtsam
Vector.h
typedef and functions to augment Eigen's VectorXd
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
Ordering.h
Variable ordering for the elimination algorithm.
Scatter.h
Maps global variable indices to slot indices.
global_includes.h
Included from all GTSAM files.
gtsam::FastVector
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:33
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::html
string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of html.
Definition DiscreteValues.cpp:134
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::operator+
Errors operator+(const Errors &a, const Errors &b)
Addition.
Definition Errors.cpp:60
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
gtsam::KeyFormatter
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
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::operator-
Errors operator-(const Errors &a, const Errors &b)
Subtraction.
Definition Errors.cpp:75
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::ConcurrentMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition ConcurrentMap.h:68
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::Scatter
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition Scatter.h:49
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Insert a vector value with key j.
Definition VectorValues.cpp:91
gtsam::VectorValues::KeyValuePair
value_type KeyValuePair
Typedef to pair<Key, Vector>.
Definition VectorValues.h:85
gtsam::VectorValues::end
const_iterator end() const
Iterator over variables.
Definition VectorValues.h:239
gtsam::VectorValues::value_type
Values::value_type value_type
Typedef to pair<Key, Vector>.
Definition VectorValues.h:84
gtsam::VectorValues::const_iterator
Values::const_iterator const_iterator
Const iterator over vector values.
Definition VectorValues.h:82
gtsam::VectorValues::end
iterator end()
Iterator over variables.
Definition VectorValues.h:238
gtsam::VectorValues::iterator
Values::iterator iterator
Iterator over vector values.
Definition VectorValues.h:81
gtsam::VectorValues::insert
iterator insert(Key j, const Vector &value)
Insert a vector value with key j.
Definition VectorValues.h:197
gtsam::VectorValues::VectorValues
VectorValues(const CONTAINER &c)
Create from another container holding pair<Key,Vector>.
Definition VectorValues.h:104
gtsam::VectorValues::values_
Values values_
Vectors making up this VectorValues.
Definition VectorValues.h:78
gtsam::VectorValues::Dims
std::map< Key, size_t > Dims
Keyed vector dimensions.
Definition VectorValues.h:86
gtsam::VectorValues::Values
ConcurrentMap< Key, Vector > Values
Collection of Vectors making up a VectorValues.
Definition VectorValues.h:77
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:163
gtsam::VectorValues::emplace
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Emplace a vector value with key j.
Definition VectorValues.h:185
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:245
gtsam::VectorValues::dim
size_t dim(Key j) const
Return the dimension of variable j.
Definition VectorValues.h:130
gtsam::VectorValues::size
size_t size() const
Number of variables stored.
Definition VectorValues.h:127
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:167
gtsam::VectorValues::exists
bool exists(Key j) const
Check whether a variable with key j exists.
Definition VectorValues.h:133
gtsam::VectorValues::VectorValues
VectorValues()
Default constructor creates an empty VectorValues.
Definition VectorValues.h:92
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:251
gtsam::VectorValues::VectorValues
VectorValues(ITERATOR first, ITERATOR last)
Create from a pair of iterators over pair<Key,Vector>.
Definition VectorValues.h:111
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:152
gtsam::VectorValues::vector
Vector vector(const CONTAINER &keys) const
Access a vector that is a subset of relevant keys.
Definition VectorValues.h:272
gtsam::VectorValues::insert_or_assign
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
gtsam::VectorValues::begin
const_iterator begin() const
Iterator over variables.
Definition VectorValues.h:237
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:139
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:226
gtsam::VectorValues::VectorValues
VectorValues(std::initializer_list< std::pair< Key, Vector > > init)
Construct from initializer list.
Definition VectorValues.h:95
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:209
gtsam::VectorValues::VectorValues
VectorValues(const VectorValues &c)
Implicit copy constructor to specialize the explicit constructor from any container.
Definition VectorValues.h:107
gtsam::VectorValues::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition VectorValues.h:83
gtsam::VectorValues::begin
iterator begin()
Iterator over variables.
Definition VectorValues.h:236