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/base/Vector.h>
#include <gtsam/base/ConcurrentMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/global_includes.h>
 
#include <memory>
 
 
#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 std::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);
 
    // We override the copy constructor; expicitly declare operator=
    VectorValues& operator=(const VectorValues& other) = default;

    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({j, Vector(std::forward<Args>(args)...)});
#endif
    }

    iterator insert(Key j, const Vector& value) {
      return insert({j, value});
    }

    VectorValues& insert(const VectorValues& values);

    VectorValues& insert(const Vector& values, const KeyVector& keys,
                         const Dims& dims);

    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({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>
    DenseIndex totalDim(const CONTAINER& keys) const {
      DenseIndex totalDim = 0;
      for (Key key : keys) {
        totalDim += static_cast<DenseIndex>(at(key).size());
      }
      return totalDim;
    }

    template <typename CONTAINER, typename Derived>
    void fillVector(const CONTAINER& keys,
                    const Eigen::MatrixBase<Derived>& result) const {
      auto& writable = const_cast<Eigen::MatrixBase<Derived>&>(result);
      DenseIndex pos = 0;
      for (Key key : keys) {
        const Vector& v = at(key);
        writable.segment(pos, v.size()) = v;
        pos += v.size();
      }
    }

    template <typename CONTAINER>
    Vector vector(const CONTAINER& keys) const {
      DenseIndex totalDim = 0;
      FastVector<const Vector*> items;
      items.reserve(keys.size());
      for (Key key : keys) {
        const Vector* v = &at(key);
        totalDim += v->size();
        items.emplace_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::map<Key, const Vector&> sorted() const;



    std::string html(
        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;

 
  private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_NVP(values_);
    }
#endif
  }; // VectorValues definition

  template<>
  struct traits<VectorValues> : public Testable<VectorValues> {
  };
 
} // \namespace gtsam