gtsam  4.1.0
gtsam
Vector.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 
21 // \callgraph
22 
23 #pragma once
24 #ifndef MKL_BLAS
25 #define MKL_BLAS MKL_DOMAIN_BLAS
26 #endif
27 
28 #include <gtsam/global_includes.h>
29 #include <Eigen/Core>
30 #include <iosfwd>
31 #include <list>
32 
33 namespace gtsam {
34 
35 // Vector is just a typedef of the Eigen dynamic vector type
36 
37 // Typedef arbitary length vector
38 typedef Eigen::VectorXd Vector;
39 
40 // Commonly used fixed size vectors
41 typedef Eigen::Matrix<double, 1, 1> Vector1;
42 typedef Eigen::Vector2d Vector2;
43 typedef Eigen::Vector3d Vector3;
44 
45 static const Eigen::MatrixBase<Vector2>::ConstantReturnType Z_2x1 = Vector2::Zero();
46 static const Eigen::MatrixBase<Vector3>::ConstantReturnType Z_3x1 = Vector3::Zero();
47 
48 // Create handy typedefs and constants for vectors with N>3
49 // VectorN and Z_Nx1, for N=1..9
50 #define GTSAM_MAKE_VECTOR_DEFS(N) \
51  typedef Eigen::Matrix<double, N, 1> Vector##N; \
52  static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero();
53 
54 GTSAM_MAKE_VECTOR_DEFS(4);
55 GTSAM_MAKE_VECTOR_DEFS(5);
56 GTSAM_MAKE_VECTOR_DEFS(6);
57 GTSAM_MAKE_VECTOR_DEFS(7);
58 GTSAM_MAKE_VECTOR_DEFS(8);
59 GTSAM_MAKE_VECTOR_DEFS(9);
60 GTSAM_MAKE_VECTOR_DEFS(10);
61 GTSAM_MAKE_VECTOR_DEFS(11);
62 GTSAM_MAKE_VECTOR_DEFS(12);
63 
64 typedef Eigen::VectorBlock<Vector> SubVector;
65 typedef Eigen::VectorBlock<const Vector> ConstSubVector;
66 
72 #if defined(GTSAM_EIGEN_VERSION_WORLD)
73 static_assert(
74  GTSAM_EIGEN_VERSION_WORLD==EIGEN_WORLD_VERSION &&
75  GTSAM_EIGEN_VERSION_MAJOR==EIGEN_MAJOR_VERSION,
76  "Error: GTSAM was built against a different version of Eigen");
77 #endif
78 
95 GTSAM_EXPORT bool fpEqual(double a, double b, double tol,
96  bool check_relative_also = true);
97 
101 GTSAM_EXPORT void print(const Vector& v, const std::string& s, std::ostream& stream);
102 
106 GTSAM_EXPORT void print(const Vector& v, const std::string& s = "");
107 
111 GTSAM_EXPORT void save(const Vector& A, const std::string &s, const std::string& filename);
112 
116 GTSAM_EXPORT bool operator==(const Vector& vec1,const Vector& vec2);
117 
123 GTSAM_EXPORT bool greaterThanOrEqual(const Vector& v1, const Vector& v2);
124 
128 GTSAM_EXPORT bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol=1e-9);
129 GTSAM_EXPORT bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol=1e-9);
130 
134 inline bool equal(const Vector& vec1, const Vector& vec2, double tol) {
135  return equal_with_abs_tol(vec1, vec2, tol);
136 }
137 
141 inline bool equal(const Vector& vec1, const Vector& vec2) {
142  return equal_with_abs_tol(vec1, vec2);
143 }
144 
152 GTSAM_EXPORT bool assert_equal(const Vector& vec1, const Vector& vec2, double tol=1e-9);
153 
161 GTSAM_EXPORT bool assert_inequal(const Vector& vec1, const Vector& vec2, double tol=1e-9);
162 
170 GTSAM_EXPORT bool assert_equal(const SubVector& vec1, const SubVector& vec2, double tol=1e-9);
171 GTSAM_EXPORT bool assert_equal(const ConstSubVector& vec1, const ConstSubVector& vec2, double tol=1e-9);
172 
180 GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol=1e-9);
181 
188 GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b);
189 
193 template<class V1, class V2>
194 inline double dot(const V1 &a, const V2& b) {
195  assert (b.size()==a.size());
196  return a.dot(b);
197 }
198 
200 template<class V1, class V2>
201 inline double inner_prod(const V1 &a, const V2& b) {
202  assert (b.size()==a.size());
203  return a.dot(b);
204 }
205 
210 inline void scal(double alpha, Vector& x) { x *= alpha; }
211 
216 template<class V1, class V2>
217 inline void axpy(double alpha, const V1& x, V2& y) {
218  assert (y.size()==x.size());
219  y += alpha * x;
220 }
221 inline void axpy(double alpha, const Vector& x, SubVector y) {
222  assert (y.size()==x.size());
223  y += alpha * x;
224 }
225 
231 GTSAM_EXPORT std::pair<double,Vector> house(const Vector &x);
232 
234 GTSAM_EXPORT double houseInPlace(Vector &x);
235 
246 GTSAM_EXPORT std::pair<Vector, double>
247 weightedPseudoinverse(const Vector& v, const Vector& weights);
248 
249 /*
250  * Fast version *no error checking* !
251  * Pass in initialized vector pseudo of size(weights) or will crash !
252  * @return the precision, pseudoinverse in third argument
253  */
254 GTSAM_EXPORT double weightedPseudoinverse(const Vector& a, const Vector& weights, Vector& pseudo);
255 
259 GTSAM_EXPORT Vector concatVectors(const std::list<Vector>& vs);
260 
264 GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...);
265 } // namespace gtsam
266 
267 #include <boost/serialization/nvp.hpp>
268 #include <boost/serialization/array.hpp>
269 #include <boost/serialization/split_free.hpp>
270 
271 namespace boost {
272  namespace serialization {
273 
274  // split version - copies into an STL vector for serialization
275  template<class Archive>
276  void save(Archive & ar, const gtsam::Vector & v, unsigned int /*version*/) {
277  const size_t size = v.size();
278  ar << BOOST_SERIALIZATION_NVP(size);
279  ar << make_nvp("data", make_array(v.data(), v.size()));
280  }
281 
282  template<class Archive>
283  void load(Archive & ar, gtsam::Vector & v, unsigned int /*version*/) {
284  size_t size;
285  ar >> BOOST_SERIALIZATION_NVP(size);
286  v.resize(size);
287  ar >> make_nvp("data", make_array(v.data(), v.size()));
288  }
289 
290  // split version - copies into an STL vector for serialization
291  template<class Archive, int D>
292  void save(Archive & ar, const Eigen::Matrix<double,D,1> & v, unsigned int /*version*/) {
293  ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
294  }
295 
296  template<class Archive, int D>
297  void load(Archive & ar, Eigen::Matrix<double,D,1> & v, unsigned int /*version*/) {
298  ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
299  }
300 
301  } // namespace serialization
302 } // namespace boost
303 
304 BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector)
305 BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector2)
306 BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector3)
307 BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector6)
gtsam::ediv_
Vector ediv_(const Vector &a, const Vector &b)
elementwise division, but 0/0 = 0, not inf
Definition: Vector.cpp:199
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition: Matrix.h:84
gtsam::axpy
void axpy(double alpha, const V1 &x, V2 &y)
BLAS Level 1 axpy: y <- alpha*x + y.
Definition: Vector.h:217
gtsam::inner_prod
double inner_prod(const V1 &a, const V2 &b)
compatibility version for ublas' inner_prod()
Definition: Vector.h:201
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition: Testable.h:83
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
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::houseInPlace
double houseInPlace(Vector &v)
beta = house(x) computes the HouseHolder vector in place
Definition: Vector.cpp:212
gtsam::assert_inequal
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
inequals with an tolerance, prints out message if within tolerance
Definition: Matrix.cpp:62
gtsam::concatVectors
Vector concatVectors(const std::list< Vector > &vs)
concatenate Vectors
Definition: Vector.cpp:302
gtsam::linear_dependent
bool linear_dependent(const Matrix &A, const Matrix &B, double tol)
check whether the rows of two matrices are linear dependent
Definition: Matrix.cpp:116
gtsam::house
pair< double, Vector > house(const Vector &x)
house(x,j) computes HouseHolder vector v and scaling factor beta from x, such that the corresponding ...
Definition: Vector.cpp:237
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition: Matrix.cpp:166
gtsam::fpEqual
bool fpEqual(double a, double b, double tol, bool check_relative_also)
Ensure we are not including a different version of Eigen in user code than while compiling gtsam,...
Definition: Vector.cpp:42
gtsam::greaterThanOrEqual
bool greaterThanOrEqual(const Vector &vec1, const Vector &vec2)
Greater than or equal to operation returns true if all elements in v1 are greater than corresponding ...
Definition: Vector.cpp:114
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition: Matrix.cpp:42
gtsam::scal
void scal(double alpha, Vector &x)
BLAS Level 1 scal: x <- alpha*x.
Definition: Vector.h:210
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition: Matrix.h:103
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:194