Vector.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/base/Vector.h>
21 #include <stdexcept>
22 #include <cstdarg>
23 #include <limits>
24 #include <iostream>
25 #include <fstream>
26 #include <sstream>
27 #include <iomanip>
28 #include <cmath>
29 #include <cstdio>
30 #include <vector>
31 
32 using namespace std;
33 
34 namespace gtsam {
35 
36 /* *************************************************************************
37  * References:
38  * 1. https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
39  * 2. https://floating-point-gui.de/errors/comparison/
40  * ************************************************************************* */
41 bool fpEqual(double a, double b, double tol, bool check_relative_also) {
42  using std::abs;
43  using std::isnan;
44  using std::isinf;
45 
46  double DOUBLE_MIN_NORMAL = numeric_limits<double>::min() + 1.0;
47  double larger = (abs(b) > abs(a)) ? abs(b) : abs(a);
48 
49  // handle NaNs
50  if(isnan(a) || isnan(b)) {
51  return isnan(a) && isnan(b);
52  }
53  // handle inf
54  else if(isinf(a) || isinf(b)) {
55  return isinf(a) && isinf(b);
56  }
57  // If the two values are zero or both are extremely close to it
58  // relative error is less meaningful here
59  else if(a == 0 || b == 0 || (abs(a) + abs(b)) < DOUBLE_MIN_NORMAL) {
60  return abs(a-b) <= tol * DOUBLE_MIN_NORMAL;
61  }
62  // Check if the numbers are really close.
63  // Needed when comparing numbers near zero or tol is in vicinity.
64  else if (abs(a - b) <= tol) {
65  return true;
66  }
67  // Check for relative error
68  else if (abs(a - b) <=
69  tol * min(larger, std::numeric_limits<double>::max()) &&
70  check_relative_also) {
71  return true;
72  }
73 
74  return false;
75 }
76 
77 /* ************************************************************************* */
78 //3 argument call
79 void print(const Vector& v, const string& s, ostream& stream) {
80  size_t n = v.size();
81 
82  stream << s << "[";
83  for(size_t i=0; i<n; i++) {
84  stream << setprecision(9) << v(i) << (i<n-1 ? "; " : "");
85  }
86  stream << "];" << endl;
87 }
88 
89 /* ************************************************************************* */
90 //1 or 2 argument call
91 void print(const Vector& v, const string& s) {
92  print(v, s, cout);
93 }
94 
95 /* ************************************************************************* */
96 void save(const Vector& v, const string &s, const string& filename) {
97  fstream stream(filename.c_str(), fstream::out | fstream::app);
98  print(v, s + "=", stream);
99  stream.close();
100 }
101 
102 /* ************************************************************************* */
103 bool operator==(const Vector& vec1,const Vector& vec2) {
104  if (vec1.size() != vec2.size()) return false;
105  size_t m = vec1.size();
106  for(size_t i=0; i<m; i++)
107  if(vec1[i] != vec2[i])
108  return false;
109  return true;
110 }
111 
112 /* ************************************************************************* */
113 bool greaterThanOrEqual(const Vector& vec1, const Vector& vec2) {
114  size_t m = vec1.size();
115  for(size_t i=0; i<m; i++)
116  if(!(vec1[i] >= vec2[i]))
117  return false;
118  return true;
119 }
120 
121 /* ************************************************************************* */
122 bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) {
123  if (vec1.size()!=vec2.size()) return false;
124  size_t m = vec1.size();
125  for(size_t i=0; i<m; ++i) {
126  if (!fpEqual(vec1[i], vec2[i], tol))
127  return false;
128  }
129  return true;
130 }
131 
132 /* ************************************************************************* */
133 bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol) {
134  if (vec1.size()!=vec2.size()) return false;
135  size_t m = vec1.size();
136  for(size_t i=0; i<m; ++i) {
137  if (!fpEqual(vec1[i], vec2[i], tol))
138  return false;
139  }
140  return true;
141 }
142 
143 /* ************************************************************************* */
144 bool assert_equal(const Vector& expected, const Vector& actual, double tol) {
145  if (equal_with_abs_tol(expected,actual,tol)) return true;
146  cout << "not equal:" << endl;
147  print(expected, "expected");
148  print(actual, "actual");
149  return false;
150 }
151 
152 /* ************************************************************************* */
153 bool assert_inequal(const Vector& expected, const Vector& actual, double tol) {
154  if (!equal_with_abs_tol(expected,actual,tol)) return true;
155  cout << "Erroneously equal:" << endl;
156  print(expected, "expected");
157  print(actual, "actual");
158  return false;
159 }
160 
161 /* ************************************************************************* */
162 bool assert_equal(const SubVector& expected, const SubVector& actual, double tol) {
163  if (equal_with_abs_tol(expected,actual,tol)) return true;
164  cout << "not equal:" << endl;
165  print(static_cast<Vector>(expected), "expected");
166  print(static_cast<Vector>(actual), "actual");
167  return false;
168 }
169 
170 /* ************************************************************************* */
171 bool assert_equal(const ConstSubVector& expected, const ConstSubVector& actual, double tol) {
172  if (equal_with_abs_tol(Vector(expected),Vector(actual),tol)) return true;
173  cout << "not equal:" << endl;
174  print(Vector(expected), "expected");
175  print(Vector(actual), "actual");
176  return false;
177 }
178 
179 /* ************************************************************************* */
180 bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) {
181  if (vec1.size()!=vec2.size()) return false;
182  bool flag = false; double scale = 1.0;
183  size_t m = vec1.size();
184  for(size_t i=0; i<m; i++) {
185  if((std::abs(vec1[i])>tol && std::abs(vec2[i])<tol) || (std::abs(vec1[i])<tol && std::abs(vec2[i])>tol))
186  return false;
187  if(vec1[i] == 0 && vec2[i] == 0) continue;
188  if (!flag) {
189  scale = vec1[i] / vec2[i];
190  flag = true ;
191  }
192  else if (std::abs(vec1[i] - vec2[i]*scale) > tol) return false;
193  }
194  return flag;
195 }
196 
197 /* ************************************************************************* */
198 Vector ediv_(const Vector &a, const Vector &b) {
199  size_t n = a.size();
200  assert (b.size()==a.size());
201  Vector c(n);
202  for( size_t i = 0; i < n; i++ ) {
203  const double &ai = a(i), &bi = b(i);
204  c(i) = (bi==0.0 && ai==0.0) ? 0.0 : ai/bi;
205  }
206  return c;
207 }
208 
209 /* ************************************************************************* */
210 // imperative version, pass in x
212  const double x0 = v(0);
213  const double x02 = x0*x0;
214 
215  // old code - GSL verison was actually a bit slower
216  const double sigma = v.squaredNorm() - x02;
217 
218  v(0) = 1.0;
219 
220  if( sigma == 0.0 )
221  return 0.0;
222  else {
223  double mu = sqrt(x02 + sigma);
224  if( x0 <= 0.0 )
225  v(0) = x0 - mu;
226  else
227  v(0) = -sigma / (x0 + mu);
228 
229  const double v02 = v(0)*v(0);
230  v = v / v(0);
231  return 2.0 * v02 / (sigma + v02);
232  }
233 }
234 
235 /* ************************************************************************* */
236 pair<double, Vector > house(const Vector &x) {
237  Vector v(x);
238  double beta = houseInPlace(v);
239  return make_pair(beta, v);
240 }
241 
242 /* ************************************************************************* */
243 // Fast version *no error checking* !
244 // Pass in initialized vector of size m or will crash !
245 double weightedPseudoinverse(const Vector& a, const Vector& weights,
246  Vector& pseudo) {
247 
248  size_t m = weights.size();
249  static const double inf = std::numeric_limits<double>::infinity();
250 
251  // Check once for zero entries of a. TODO: really needed ?
252  vector<bool> isZero;
253  for (size_t i = 0; i < m; ++i) isZero.push_back(std::abs(a[i]) < 1e-9);
254 
255  // If there is a valid (a!=0) constraint (sigma==0) return the first one
256  for (size_t i = 0; i < m; ++i) {
257  if (weights[i] == inf && !isZero[i]) {
258  // Basically, instead of doing a normal QR step with the weighted
259  // pseudoinverse, we enforce the constraint by turning
260  // ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0
261  pseudo = Vector::Unit(m,i)*(1.0/a[i]);
262  return inf;
263  }
264  }
265 
266  // Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma)
267  // For diagonal Sigma, inv(Sigma) = diag(precisions)
268  double precision = 0;
269  for (size_t i = 0; i < m; i++) {
270  double ai = a[i];
271  if (!isZero[i]) // also catches remaining sigma==0 rows
272  precision += weights[i] * (ai * ai);
273  }
274 
275  // precision = a'inv(Sigma)a
276  if (precision < 1e-9)
277  for (size_t i = 0; i < m; i++)
278  pseudo[i] = 0;
279  else {
280  // emul(precisions,a)/precision
281  double variance = 1.0 / precision;
282  for (size_t i = 0; i < m; i++)
283  pseudo[i] = isZero[i] ? 0 : variance * weights[i] * a[i];
284  }
285  return precision; // sum of weights
286 }
287 
288 /* ************************************************************************* */
289 // Slow version with error checking
290 pair<Vector, double>
291 weightedPseudoinverse(const Vector& a, const Vector& weights) {
292  DenseIndex m = weights.size();
293  if (a.size() != m)
294  throw invalid_argument("a and weights have different sizes!");
295  Vector pseudo(m);
296  double precision = weightedPseudoinverse(a, weights, pseudo);
297  return make_pair(pseudo, precision);
298 }
299 
300 /* ************************************************************************* */
301 Vector concatVectors(const std::list<Vector>& vs) {
302  size_t dim = 0;
303  for (const Vector& v : vs) dim += v.size();
304 
305  Vector A(dim);
306  size_t index = 0;
307  for (const Vector& v : vs) {
308  for (int d = 0; d < v.size(); d++) A(d + index) = v(d);
309  index += v.size();
310  }
311 
312  return A;
313 }
314 
315 /* ************************************************************************* */
316 Vector concatVectors(size_t nrVectors, ...)
317 {
318  va_list ap;
319  list<Vector> vs;
320  va_start(ap, nrVectors);
321  for( size_t i = 0 ; i < nrVectors ; i++) {
322  Vector* V = va_arg(ap, Vector*);
323  vs.push_back(*V);
324  }
325  va_end(ap);
326  return concatVectors(vs);
327 }
328 
329 } // namespace gtsam
Matrix3f m
bool linear_dependent(const Vector &vec1, const Vector &vec2, double tol)
Definition: Vector.cpp:180
#define max(a, b)
Definition: datatypes.h:20
Scalar * b
Definition: benchVecAdd.cpp:17
Vector ediv_(const Vector &a, const Vector &b)
Definition: Vector.cpp:198
std::ofstream out("Result.txt")
#define min(a, b)
Definition: datatypes.h:19
Matrix expected
Definition: testMatrix.cpp:971
double mu
int n
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
bool assert_equal(const ConstSubVector &expected, const ConstSubVector &actual, double tol)
Definition: Vector.cpp:171
Definition: BFloat16.h:88
bool greaterThanOrEqual(const Vector &vec1, const Vector &vec2)
Definition: Vector.cpp:113
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
#define isinf(X)
Definition: main.h:94
bool equal_with_abs_tol(const SubVector &vec1, const SubVector &vec2, double tol)
Definition: Vector.cpp:133
bool assert_inequal(const Vector &expected, const Vector &actual, double tol)
Definition: Vector.cpp:153
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:108
bool fpEqual(double a, double b, double tol, bool check_relative_also)
Definition: Vector.cpp:41
void print(const Vector &v, const string &s)
Definition: Vector.cpp:91
void save(const Vector &v, const string &s, const string &filename)
Definition: Vector.cpp:96
Expression of a fixed-size or dynamic-size sub-vector.
pair< double, Vector > house(const Vector &x)
Definition: Vector.cpp:236
Eigen::VectorXd Vector
Definition: Vector.h:38
pair< Vector, double > weightedPseudoinverse(const Vector &a, const Vector &weights)
Definition: Vector.cpp:291
Array< int, Dynamic, 1 > v
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
bool operator==(const Vector &vec1, const Vector &vec2)
Definition: Vector.cpp:103
Vector concatVectors(size_t nrVectors,...)
Definition: Vector.cpp:316
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
static Symbol x0('x', 0)
RowVectorXd vec1(3)
static double inf
Definition: testMatrix.cpp:31
double houseInPlace(Vector &v)
Definition: Vector.cpp:211
cout precision(2)
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 y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
static const double sigma
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
const G double tol
Definition: Group.h:86
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
#define abs(x)
Definition: datatypes.h:17
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
#define isnan(X)
Definition: main.h:93


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:43