Vector.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

 * -------------------------------------------------------------------------- */

// \callgraph

#pragma once

#include <list>
#include <vector>
#include <iostream>
#include <gtsam/global_includes.h>
#include <gtsam/3rdparty/gtsam_eigen_includes.h>

namespace gtsam {

// Vector is just a typedef of the Eigen dynamic vector type

// Typedef arbitary length vector
typedef Eigen::VectorXd Vector;

// Commonly used fixed size vectors
typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Matrix<double, 6, 1> Vector6;

typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector;

GTSAM_EXPORT Vector repeat(size_t n, double value);

GTSAM_EXPORT Vector delta(size_t n, size_t i, double value);

inline Vector basis(size_t n, size_t i) { return delta(n, i, 1.0); }

inline Vector zero(size_t n) { return Vector::Zero(n);}

inline Vector ones(size_t n) { return Vector::Ones(n); }
  
GTSAM_EXPORT bool zero(const Vector& v);

inline size_t dim(const Vector& v) { return v.size(); }

GTSAM_EXPORT void print(const Vector& v, const std::string& s = "", std::ostream& stream = std::cout);

GTSAM_EXPORT void save(const Vector& A, const std::string &s, const std::string& filename);

GTSAM_EXPORT bool operator==(const Vector& vec1,const Vector& vec2);

GTSAM_EXPORT bool greaterThanOrEqual(const Vector& v1, const Vector& v2);

GTSAM_EXPORT bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol=1e-9);
GTSAM_EXPORT bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol=1e-9);

inline bool equal(const Vector& vec1, const Vector& vec2, double tol) {
  return equal_with_abs_tol(vec1, vec2, tol);
}

inline bool equal(const Vector& vec1, const Vector& vec2) {
  return equal_with_abs_tol(vec1, vec2);
}

GTSAM_EXPORT bool assert_equal(const Vector& vec1, const Vector& vec2, double tol=1e-9);

GTSAM_EXPORT bool assert_inequal(const Vector& vec1, const Vector& vec2, double tol=1e-9);

GTSAM_EXPORT bool assert_equal(const SubVector& vec1, const SubVector& vec2, double tol=1e-9);
GTSAM_EXPORT bool assert_equal(const ConstSubVector& vec1, const ConstSubVector& vec2, double tol=1e-9);

GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol=1e-9);

GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2);

GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i);

GTSAM_EXPORT Vector emul(const Vector &a, const Vector &b);

GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b);

GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b);

GTSAM_EXPORT double sum(const Vector &a);

GTSAM_EXPORT double norm_2(const Vector& v);

GTSAM_EXPORT Vector reciprocal(const Vector &a);

GTSAM_EXPORT Vector esqrt(const Vector& v);

GTSAM_EXPORT Vector abs(const Vector& v);

GTSAM_EXPORT double max(const Vector &a);

template<class V1, class V2>
inline double dot(const V1 &a, const V2& b) {
  assert (b.size()==a.size());
  return a.dot(b);
}

template<class V1, class V2>
inline double inner_prod(const V1 &a, const V2& b) {
  assert (b.size()==a.size());
  return a.dot(b);
}

inline void scal(double alpha, Vector& x) { x *= alpha; }

template<class V1, class V2>
inline void axpy(double alpha, const V1& x, V2& y) {
  assert (y.size()==x.size());
  y += alpha * x;
}
inline void axpy(double alpha, const Vector& x, SubVector y) {
  assert (y.size()==x.size());
  y += alpha * x;
}

GTSAM_EXPORT std::pair<double,Vector> house(const Vector &x);

GTSAM_EXPORT double houseInPlace(Vector &x);

GTSAM_EXPORT std::pair<Vector, double>
weightedPseudoinverse(const Vector& v, const Vector& weights);

/*
 * Fast version *no error checking* !
 * Pass in initialized vector pseudo of size(weights) or will crash !
 * @return the precision, pseudoinverse in third argument
 */
GTSAM_EXPORT double weightedPseudoinverse(const Vector& a, const Vector& weights, Vector& pseudo);

GTSAM_EXPORT Vector concatVectors(const std::list<Vector>& vs);

GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...);

} // namespace gtsam

#include <boost/serialization/nvp.hpp>
#include <boost/serialization/array.hpp>
#include <boost/serialization/split_free.hpp>

namespace boost {
  namespace serialization {

    // split version - copies into an STL vector for serialization
    template<class Archive>
    void save(Archive & ar, const gtsam::Vector & v, unsigned int version) {
      const size_t size = v.size();
      ar << BOOST_SERIALIZATION_NVP(size);
      ar << make_nvp("data", make_array(v.data(), v.size()));
    }

    template<class Archive>
    void load(Archive & ar, gtsam::Vector & v, unsigned int version) {
      size_t size;
      ar >> BOOST_SERIALIZATION_NVP(size);
      v.resize(size);
      ar >> make_nvp("data", make_array(v.data(), v.size()));
    }

    // split version - copies into an STL vector for serialization
    template<class Archive, int D>
    void save(Archive & ar, const Eigen::Matrix<double,D,1> & v, unsigned int version) {
      ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
    }

    template<class Archive, int D>
    void load(Archive & ar, Eigen::Matrix<double,D,1> & v, unsigned int version) {
      ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
    }

  } // namespace serialization
} // namespace boost

BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector)
BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector2)
BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector3)
BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector6)