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 <gtsam/linear/Errors.h>
22 
23 using namespace std;
24 
25 namespace gtsam {
26 
27 /* ************************************************************************* */
29  Errors result;
30  for (const auto& [key, e] : V) {
31  result.push_back(e);
32  }
33  return result;
34 }
35 
36 /* ************************************************************************* */
37 void print(const Errors& e, const string& s) {
38  cout << s << endl;
39  for(const Vector& v: e)
40  gtsam::print(v);
41 }
42 
43 /* ************************************************************************* */
44 struct equalsVector : public std::function<bool(const Vector&, const Vector&)> {
45  double tol_;
46  equalsVector(double tol = 1e-9) : tol_(tol) {}
47  bool operator()(const Vector& expected, const Vector& actual) {
48  return equal_with_abs_tol(expected, actual,tol_);
49  }
50 };
51 
52 bool equality(const Errors& actual, const Errors& expected, double tol) {
53  if (actual.size() != expected.size()) return false;
54  return equal(actual.begin(), actual.end(), expected.begin(),
55  equalsVector(tol));
56 }
57 
58 /* ************************************************************************* */
59 Errors operator+(const Errors& a, const Errors& b) {
60 #ifndef NDEBUG
61  size_t m = a.size();
62  if (b.size()!=m)
63  throw(std::invalid_argument("Errors::operator+: incompatible sizes"));
64 #endif
65  Errors result;
66  Errors::const_iterator it = b.begin();
67  for(const Vector& ai: a)
68  result.push_back(ai + *(it++));
69  return result;
70 }
71 
72 
73 /* ************************************************************************* */
74 Errors operator-(const Errors& a, const Errors& b) {
75 #ifndef NDEBUG
76  size_t m = a.size();
77  if (b.size()!=m)
78  throw(std::invalid_argument("Errors::operator-: incompatible sizes"));
79 #endif
80  Errors result;
81  Errors::const_iterator it = b.begin();
82  for(const Vector& ai: a)
83  result.push_back(ai - *(it++));
84  return result;
85 }
86 
87 /* ************************************************************************* */
89  Errors result;
90  for(const Vector& ai: a)
91  result.push_back(-ai);
92  return result;
93 }
94 
95 /* ************************************************************************* */
96 double dot(const Errors& a, const Errors& b) {
97 #ifndef NDEBUG
98  size_t m = a.size();
99  if (b.size()!=m)
100  throw(std::invalid_argument("Errors::dot: incompatible sizes"));
101 #endif
102  double result = 0.0;
103  Errors::const_iterator it = b.begin();
104  for(const Vector& ai: a)
105  result += gtsam::dot<Vector,Vector>(ai, *(it++));
106  return result;
107 }
108 
109 /* ************************************************************************* */
110 void axpy(double alpha, const Errors& x, Errors& y) {
111  Errors::const_iterator it = x.begin();
112  for(Vector& yi: y)
113  yi += alpha * (*(it++));
114 }
115 
116 /* ************************************************************************* */
117 
118 } // gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
const gtsam::Symbol key('X', 0)
Matrix3f m
vector of errors
Scalar * y
Scalar * b
Definition: benchVecAdd.cpp:17
Matrix expected
Definition: testMatrix.cpp:971
void print(const Errors &e, const string &s)
Print an Errors instance.
Definition: Errors.cpp:37
Definition: BFloat16.h:88
equalsVector(double tol=1e-9)
Definition: Errors.cpp:46
Factor Graph Values.
Eigen::VectorXd Vector
Definition: Vector.h:38
Errors operator-(const Errors &a)
Negation.
Definition: Errors.cpp:88
Values result
Array< int, Dynamic, 1 > v
double dot(const Errors &a, const Errors &b)
Dot product.
Definition: Errors.cpp:96
Errors operator+(const Errors &a, const Errors &b)
Addition.
Definition: Errors.cpp:59
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:80
Errors createErrors(const VectorValues &V)
Break V into pieces according to its start indices.
Definition: Errors.cpp:28
traits
Definition: chartTesting.h:28
bool operator()(const Vector &expected, const Vector &actual)
Definition: Errors.cpp:47
bool equality(const Errors &actual, const Errors &expected, double tol)
Definition: Errors.cpp:52
const G double tol
Definition: Group.h:86
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:85
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


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:12