Errors.cpp
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 
20 #include <boost/range/adaptor/map.hpp>
21 #include <gtsam/linear/Errors.h>
23 
24 using namespace std;
25 
26 namespace gtsam {
27 
28 /* ************************************************************************* */
29 Errors::Errors(){}
30 
31 /* ************************************************************************* */
32 Errors::Errors(const VectorValues& V) {
33  for(const Vector& e: V | boost::adaptors::map_values) {
34  push_back(e);
35  }
36 }
37 
38 /* ************************************************************************* */
39 void Errors::print(const std::string& s) const {
40  cout << s << endl;
41  for(const Vector& v: *this)
42  gtsam::print(v);
43 }
44 
45 /* ************************************************************************* */
46 struct equalsVector : public std::function<bool(const Vector&, const Vector&)> {
47  double tol_;
48  equalsVector(double tol = 1e-9) : tol_(tol) {}
49  bool operator()(const Vector& expected, const Vector& actual) {
50  return equal_with_abs_tol(expected, actual,tol_);
51  }
52 };
53 
54 bool Errors::equals(const Errors& expected, double tol) const {
55  if( size() != expected.size() ) return false;
56  return equal(begin(),end(),expected.begin(),equalsVector(tol));
57 }
58 
59 /* ************************************************************************* */
61 #ifndef NDEBUG
62  size_t m = size();
63  if (b.size()!=m)
64  throw(std::invalid_argument("Errors::operator+: incompatible sizes"));
65 #endif
66  Errors result;
67  Errors::const_iterator it = b.begin();
68  for(const Vector& ai: *this)
69  result.push_back(ai + *(it++));
70  return result;
71 }
72 
73 
74 /* ************************************************************************* */
76 #ifndef NDEBUG
77  size_t m = size();
78  if (b.size()!=m)
79  throw(std::invalid_argument("Errors::operator-: incompatible sizes"));
80 #endif
81  Errors result;
82  Errors::const_iterator it = b.begin();
83  for(const Vector& ai: *this)
84  result.push_back(ai - *(it++));
85  return result;
86 }
87 
88 /* ************************************************************************* */
90  Errors result;
91  for(const Vector& ai: *this)
92  result.push_back(-ai);
93  return result;
94 }
95 
96 
97 
98 /* ************************************************************************* */
99 double dot(const Errors& a, const Errors& b) {
100 #ifndef NDEBUG
101  size_t m = a.size();
102  if (b.size()!=m)
103  throw(std::invalid_argument("Errors::dot: incompatible sizes"));
104 #endif
105  double result = 0.0;
106  Errors::const_iterator it = b.begin();
107  for(const Vector& ai: a)
108  result += gtsam::dot(ai, *(it++));
109  return result;
110 }
111 
112 /* ************************************************************************* */
113 template<>
114 void axpy<Errors,Errors>(double alpha, const Errors& x, Errors& y) {
115  Errors::const_iterator it = x.begin();
116  for(Vector& yi: y)
117  axpy(alpha,*(it++),yi);
118 }
119 
120 /* ************************************************************************* */
121 void print(const Errors& a, const string& s) {
122  a.print(s);
123 }
124 
125 /* ************************************************************************* */
126 
127 } // gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Matrix3f m
const mpreal ai(const mpreal &x, mp_rnd_t r=mpreal::get_default_rnd())
Definition: mpreal.h:2467
vector of errors
Scalar * y
Scalar * b
Definition: benchVecAdd.cpp:17
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:194
Matrix expected
Definition: testMatrix.cpp:974
ArrayXcf v
Definition: Cwise_arg.cpp:1
Definition: Half.h:150
Array33i a
equalsVector(double tol=1e-9)
Definition: Errors.cpp:48
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
Factor Graph Values.
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
RealScalar alpha
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:84
traits
Definition: chartTesting.h:28
bool operator()(const Vector &expected, const Vector &actual)
Definition: Errors.cpp:49
void print(const Errors &a, const string &s)
Definition: Errors.cpp:121
void axpy< Errors, Errors >(double alpha, const Errors &x, Errors &y)
Definition: Errors.cpp:114
Jet< T, N > operator-(const Jet< T, N > &f)
Definition: jet.h:258
GTSAM_EXPORT void print(const std::string &s="Errors") const
Definition: Errors.cpp:39
const G double tol
Definition: Group.h:83
int EIGEN_BLAS_FUNC() axpy(const int *n, const RealScalar *palpha, const RealScalar *px, const int *incx, RealScalar *py, const int *incy)
Definition: level1_impl.h:12
bool equal(const T &obj1, const T &obj2, double tol)
Definition: Testable.h:83
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Jet< T, N > const & operator+(const Jet< T, N > &f)
Definition: jet.h:249
double dot(const Errors &a, const Errors &b)
Definition: Errors.cpp:99


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:42:02