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/base/Vector.h>
#include <gtsam/base/ConcurrentMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/global_includes.h>
#include <gtsam/inference/Ordering.h>

#include <boost/shared_ptr.hpp>

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 Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values
    //typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values
    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(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);

    static VectorValues Zero(const VectorValues& other);


    Key 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); }

    void update(const VectorValues& values);

    iterator insert(Key j, const Vector& value) {
      return insert(std::make_pair(j, value)); // Note only passing a reference to the Vector
    }

    iterator insert(const std::pair<Key, Vector>& key_value) {
      // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as
      // it is inserted into the values_ map.
      std::pair<iterator, bool> result = values_.insert(key_value);
      if(!result.second)
        throw std::invalid_argument(
        "Requested to insert variable '" + DefaultKeyFormatter(key_value.first)
        + "' already in this VectorValues.");
      return result.first;
    }

    void insert(const VectorValues& values);

    std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
      return values_.insert(std::make_pair(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(); }    
    //reverse_iterator rbegin()             { return values_.rbegin(); } ///< Reverse iterator over variables
    //const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables
    //reverse_iterator rend()               { return values_.rend(); }   ///< Reverse iterator over variables
    //const_reverse_iterator rend() const   { return values_.rend(); }   ///< Reverse iterator over variables

    iterator find(Key j) { return values_.find(j); }

    const_iterator find(Key j) const { return values_.find(j); }

    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;

    Vector vector(const FastVector<Key>& keys) const;

    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);



    //inline VectorValues scale(const double a, const VectorValues& c) const { return a * (*this); }


    //friend VectorValues operator*(const double a, const VectorValues &v) {
    //  VectorValues result = VectorValues::SameStructure(v);
    //  for(Key j = 0; j < v.size(); ++j)
    //    result.values_[j] = a * v.values_[j];
    //  return result;
    //}

    //friend void axpy(double alpha, const VectorValues& x, VectorValues& y) {
    //  if(x.size() != y.size())
    //    throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
    //  for(Key j = 0; j < x.size(); ++j)
    //    if(x.values_[j].size() == y.values_[j].size())
    //      y.values_[j] += alpha * x.values_[j];
    //    else
    //      throw std::invalid_argument("axpy(VectorValues) called with different vector sizes");
    //}
    //friend void sqrt(VectorValues &x) {
    //  for(Key j = 0; j < x.size(); ++j)
    //    x.values_[j] = x.values_[j].cwiseSqrt();
    //}

    //friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) {
    //  if(numerator.size() != denominator.size() || numerator.size() != result.size())
    //    throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
    //  for(Key j = 0; j < numerator.size(); ++j)
    //    if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size())
    //      result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]);
    //    else
    //      throw std::invalid_argument("ediv(VectorValues) called with different vector sizes");
    //}

    //friend void edivInPlace(VectorValues& x, const VectorValues& y) {
    //  if(x.size() != y.size())
    //    throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
    //  for(Key j = 0; j < x.size(); ++j)
    //    if(x.values_[j].size() == y.values_[j].size())
    //      x.values_[j].array() /= y.values_[j].array();
    //    else
    //      throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes");
    //}

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

} // \namespace gtsam