gtsam  4.0.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
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
9
10  * -------------------------------------------------------------------------- */
11
20 // \callgraph
21
22 #pragma once
23 #ifndef MKL_BLAS
24 #define MKL_BLAS MKL_DOMAIN_BLAS
25 #endif
26
27 #include <gtsam/global_includes.h>
28 #include <Eigen/Core>
29 #include <iosfwd>
30 #include <list>
31
32 namespace gtsam {
33
34 // Vector is just a typedef of the Eigen dynamic vector type
35
36 // Typedef arbitary length vector
37 typedef Eigen::VectorXd Vector;
38
39 // Commonly used fixed size vectors
40 typedef Eigen::Matrix<double, 1, 1> Vector1;
41 typedef Eigen::Vector2d Vector2;
42 typedef Eigen::Vector3d Vector3;
43
44 static const Eigen::MatrixBase<Vector2>::ConstantReturnType Z_2x1 = Vector2::Zero();
45 static const Eigen::MatrixBase<Vector3>::ConstantReturnType Z_3x1 = Vector3::Zero();
46
47 // Create handy typedefs and constants for vectors with N>3
48 // VectorN and Z_Nx1, for N=1..9
49 #define GTSAM_MAKE_VECTOR_DEFS(N) \
50  typedef Eigen::Matrix<double, N, 1> Vector##N; \
51  static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero();
52
53 GTSAM_MAKE_VECTOR_DEFS(4);
54 GTSAM_MAKE_VECTOR_DEFS(5);
55 GTSAM_MAKE_VECTOR_DEFS(6);
56 GTSAM_MAKE_VECTOR_DEFS(7);
57 GTSAM_MAKE_VECTOR_DEFS(8);
58 GTSAM_MAKE_VECTOR_DEFS(9);
59 GTSAM_MAKE_VECTOR_DEFS(10);
60 GTSAM_MAKE_VECTOR_DEFS(11);
61 GTSAM_MAKE_VECTOR_DEFS(12);
62
63 typedef Eigen::VectorBlock<Vector> SubVector;
64 typedef Eigen::VectorBlock<const Vector> ConstSubVector;
65
69 GTSAM_EXPORT void print(const Vector& v, const std::string& s, std::ostream& stream);
70
74 GTSAM_EXPORT void print(const Vector& v, const std::string& s = "");
75
79 GTSAM_EXPORT void save(const Vector& A, const std::string &s, const std::string& filename);
80
84 GTSAM_EXPORT bool operator==(const Vector& vec1,const Vector& vec2);
85
91 GTSAM_EXPORT bool greaterThanOrEqual(const Vector& v1, const Vector& v2);
92
96 GTSAM_EXPORT bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol=1e-9);
97 GTSAM_EXPORT bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol=1e-9);
98
102 inline bool equal(const Vector& vec1, const Vector& vec2, double tol) {
103  return equal_with_abs_tol(vec1, vec2, tol);
104 }
105
109 inline bool equal(const Vector& vec1, const Vector& vec2) {
110  return equal_with_abs_tol(vec1, vec2);
111 }
112
120 GTSAM_EXPORT bool assert_equal(const Vector& vec1, const Vector& vec2, double tol=1e-9);
121
129 GTSAM_EXPORT bool assert_inequal(const Vector& vec1, const Vector& vec2, double tol=1e-9);
130
138 GTSAM_EXPORT bool assert_equal(const SubVector& vec1, const SubVector& vec2, double tol=1e-9);
139 GTSAM_EXPORT bool assert_equal(const ConstSubVector& vec1, const ConstSubVector& vec2, double tol=1e-9);
140
148 GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol=1e-9);
149
156 GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b);
157
161 template<class V1, class V2>
162 inline double dot(const V1 &a, const V2& b) {
163  assert (b.size()==a.size());
164  return a.dot(b);
165 }
166
168 template<class V1, class V2>
169 inline double inner_prod(const V1 &a, const V2& b) {
170  assert (b.size()==a.size());
171  return a.dot(b);
172 }
173
178 inline void scal(double alpha, Vector& x) { x *= alpha; }
179
184 template<class V1, class V2>
185 inline void axpy(double alpha, const V1& x, V2& y) {
186  assert (y.size()==x.size());
187  y += alpha * x;
188 }
189 inline void axpy(double alpha, const Vector& x, SubVector y) {
190  assert (y.size()==x.size());
191  y += alpha * x;
192 }
193
199 GTSAM_EXPORT std::pair<double,Vector> house(const Vector &x);
200
202 GTSAM_EXPORT double houseInPlace(Vector &x);
203
214 GTSAM_EXPORT std::pair<Vector, double>
215 weightedPseudoinverse(const Vector& v, const Vector& weights);
216
217 /*
218  * Fast version *no error checking* !
219  * Pass in initialized vector pseudo of size(weights) or will crash !
220  * @return the precision, pseudoinverse in third argument
221  */
222 GTSAM_EXPORT double weightedPseudoinverse(const Vector& a, const Vector& weights, Vector& pseudo);
223
227 GTSAM_EXPORT Vector concatVectors(const std::list<Vector>& vs);
228
232 GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...);
233
234 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
235 inline Vector abs(const Vector& v){return v.cwiseAbs();}
236 inline Vector basis(size_t n, size_t i) { return Vector::Unit(n,i); }
237 inline Vector delta(size_t n, size_t i, double value){ return Vector::Unit(n, i) * value;}
238 inline size_t dim(const Vector& v) { return v.size(); }
239 inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);}
240 inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();}
241 inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);}
242 inline double max(const Vector &a){return a.maxCoeff();}
243 inline double norm_2(const Vector& v) {return v.norm();}
244 inline Vector ones(size_t n) { return Vector::Ones(n); }
245 inline Vector reciprocal(const Vector &a) {return a.array().inverse();}
246 inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);}
247 inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);}
248 inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;}
249 inline double sum(const Vector &a){return a.sum();}
250 inline bool zero(const Vector& v){ return v.isZero(); }
251 inline Vector zero(size_t n) { return Vector::Zero(n); }
252 #endif
253 } // namespace gtsam
254
255 #include <boost/serialization/nvp.hpp>
256 #include <boost/serialization/array.hpp>
257 #include <boost/serialization/split_free.hpp>
258
259 namespace boost {
260  namespace serialization {
261
262  // split version - copies into an STL vector for serialization
263  template<class Archive>
264  void save(Archive & ar, const gtsam::Vector & v, unsigned int /*version*/) {
265  const size_t size = v.size();
266  ar << BOOST_SERIALIZATION_NVP(size);
267  ar << make_nvp("data", make_array(v.data(), v.size()));
268  }
269
270  template<class Archive>
271  void load(Archive & ar, gtsam::Vector & v, unsigned int /*version*/) {
272  size_t size;
273  ar >> BOOST_SERIALIZATION_NVP(size);
274  v.resize(size);
275  ar >> make_nvp("data", make_array(v.data(), v.size()));
276  }
277
278  // split version - copies into an STL vector for serialization
279  template<class Archive, int D>
280  void save(Archive & ar, const Eigen::Matrix<double,D,1> & v, unsigned int /*version*/) {
281  ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
282  }
283
284  template<class Archive, int D>
285  void load(Archive & ar, Eigen::Matrix<double,D,1> & v, unsigned int /*version*/) {
286  ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
287  }
288
289  } // namespace serialization
290 } // namespace boost
291
292 BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector)
293 BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector2)
294 BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector3)
295 BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector6)
double houseInPlace(Vector &v)
beta = house(x) computes the HouseHolder vector in place
Definition: Vector.cpp:174
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:162
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:141
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
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:82
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition: Matrix.cpp:42
double inner_prod(const V1 &a, const V2 &b)
compatibility version for ublas' inner_prod()
Definition: Vector.h:169
void scal(double alpha, Vector &x)
BLAS Level 1 scal: x <- alpha*x.
Definition: Vector.h:178
Included from all GTSAM files.
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:162
void axpy(double alpha, const V1 &x, V2 &y)
BLAS Level 1 axpy: y <- alpha*x + y.
Definition: Vector.h:185
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:199
Vector concatVectors(const std::list< Vector > &vs)
concatenate Vectors
Definition: Vector.cpp:264
Eigen::Block< const MATRIX > sub(const MATRIX &A, size_t i1, size_t i2, size_t j1, size_t j2)
extract submatrix, slice semantics, i.e.
Definition: Matrix.h:183
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition: Testable.h:83
Vector ediv_(const Vector &a, const Vector &b)
elementwise division, but 0/0 = 0, not inf
Definition: Vector.cpp:161
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
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition: Matrix.h:102
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:72