testVector.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 
18 #include <gtsam/base/Vector.h>
19 #include <gtsam/base/VectorSpace.h>
20 #include <gtsam/base/testLie.h>
21 #include <CppUnitLite/TestHarness.h>
22 #include <boost/tuple/tuple.hpp>
23 #include <iostream>
24 
25 using namespace std;
26 using namespace gtsam;
27 
28 namespace {
29  /* ************************************************************************* */
30  template<typename Derived>
31  Vector testFcn1(const Eigen::DenseBase<Derived>& in)
32  {
33  return in;
34  }
35 
36  /* ************************************************************************* */
37  template<typename Derived>
38  Vector testFcn2(const Eigen::MatrixBase<Derived>& in)
39  {
40  return in;
41  }
42 }
43 
44 /* ************************************************************************* */
45 TEST(Vector, special_comma_initializer)
46 {
47  Vector expected(3);
48  expected(0) = 1;
49  expected(1) = 2;
50  expected(2) = 3;
51 
52  Vector actual1 = Vector3(1, 2, 3);
53  Vector actual2(Vector3(1, 2, 3));
54 
55  Vector subvec1 = Vector2(2, 3);
56  Vector actual4 = (Vector(3) << 1, subvec1).finished();
57 
58  Vector subvec2 = Vector2(1, 2);
59  Vector actual5 = (Vector(3) << subvec2, 3).finished();
60 
61  Vector actual6 = testFcn1(Vector3(1, 2, 3));
62  Vector actual7 = testFcn2(Vector3(1, 2, 3));
63 
64  EXPECT(assert_equal(expected, actual1));
65  EXPECT(assert_equal(expected, actual2));
66  EXPECT(assert_equal(expected, actual4));
67  EXPECT(assert_equal(expected, actual5));
68  EXPECT(assert_equal(expected, actual6));
69  EXPECT(assert_equal(expected, actual7));
70 }
71 
72 /* ************************************************************************* */
73 TEST(Vector, copy )
74 {
75  Vector a(2); a(0) = 10; a(1) = 20;
76  double data[] = {10,20};
77  Vector b(2);
78  copy(data,data+2,b.data());
79  EXPECT(assert_equal(a, b));
80 }
81 
82 /* ************************************************************************* */
83 TEST(Vector, scalar_multiply )
84 {
85  Vector a(2); a(0) = 10; a(1) = 20;
86  Vector b(2); b(0) = 1; b(1) = 2;
87  EXPECT(assert_equal(a,b*10.0));
88 }
89 
90 /* ************************************************************************* */
91 TEST(Vector, scalar_divide )
92 {
93  Vector a(2); a(0) = 10; a(1) = 20;
94  Vector b(2); b(0) = 1; b(1) = 2;
95  EXPECT(assert_equal(b,a/10.0));
96 }
97 
98 /* ************************************************************************* */
99 TEST(Vector, negate )
100 {
101  Vector a(2); a(0) = 10; a(1) = 20;
102  Vector b(2); b(0) = -10; b(1) = -20;
103  EXPECT(assert_equal(b, -a));
104 }
105 
106 /* ************************************************************************* */
107 TEST(Vector, householder )
108 {
109  Vector x(4);
110  x(0) = 3; x(1) = 1; x(2) = 5; x(3) = 1;
111 
112  Vector expected(4);
113  expected(0) = 1.0; expected(1) = -0.333333; expected(2) = -1.66667; expected(3) = -0.333333;
114 
115  pair<double, Vector> result = house(x);
116 
117  EXPECT(result.first==0.5);
118  EXPECT(equal_with_abs_tol(expected,result.second,1e-5));
119 }
120 
121 /* ************************************************************************* */
122 TEST(Vector, concatVectors)
123 {
124  Vector A(2);
125  for(int i = 0; i < 2; i++)
126  A(i) = i;
127  Vector B(5);
128  for(int i = 0; i < 5; i++)
129  B(i) = i;
130 
131  Vector C(7);
132  for(int i = 0; i < 2; i++) C(i) = A(i);
133  for(int i = 0; i < 5; i++) C(i+2) = B(i);
134 
135  list<Vector> vs;
136  vs.push_back(A);
137  vs.push_back(B);
138  Vector AB1 = concatVectors(vs);
139  EXPECT(AB1 == C);
140 
141  Vector AB2 = concatVectors(2, &A, &B);
142  EXPECT(AB2 == C);
143 }
144 
145 /* ************************************************************************* */
146 TEST(Vector, weightedPseudoinverse )
147 {
148  // column from a matrix
149  Vector x(2);
150  x(0) = 1.0; x(1) = 2.0;
151 
152  // create sigmas
153  Vector sigmas(2);
154  sigmas(0) = 0.1; sigmas(1) = 0.2;
155  Vector weights = sigmas.array().square().inverse();
156 
157  // perform solve
158  Vector actual; double precision;
159  boost::tie(actual, precision) = weightedPseudoinverse(x, weights);
160 
161  // construct expected
162  Vector expected(2);
163  expected(0) = 0.5; expected(1) = 0.25;
164  double expPrecision = 200.0;
165 
166  // verify
167  EXPECT(assert_equal(expected,actual));
168  EXPECT(std::abs(expPrecision-precision) < 1e-5);
169 }
170 
171 /* ************************************************************************* */
172 TEST(Vector, weightedPseudoinverse_constraint )
173 {
174  // column from a matrix
175  Vector x(2);
176  x(0) = 1.0; x(1) = 2.0;
177 
178  // create sigmas
179  Vector sigmas(2);
180  sigmas(0) = 0.0; sigmas(1) = 0.2;
181  Vector weights = sigmas.array().square().inverse();
182  // perform solve
183  Vector actual; double precision;
184  boost::tie(actual, precision) = weightedPseudoinverse(x, weights);
185 
186  // construct expected
187  Vector expected(2);
188  expected(0) = 1.0; expected(1) = 0.0;
189 
190  // verify
191  EXPECT(assert_equal(expected,actual));
192  EXPECT(std::isinf(precision));
193 }
194 
195 /* ************************************************************************* */
196 TEST(Vector, weightedPseudoinverse_nan )
197 {
198  Vector a = (Vector(4) << 1., 0., 0., 0.).finished();
199  Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished();
200  Vector weights = sigmas.array().square().inverse();
201  Vector pseudo; double precision;
202  boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);
203 
204  Vector expected = (Vector(4) << 1., 0., 0.,0.).finished();
205  EXPECT(assert_equal(expected, pseudo));
206  DOUBLES_EQUAL(100, precision, 1e-5);
207 }
208 
209 /* ************************************************************************* */
210 TEST(Vector, dot )
211 {
212  Vector a = Vector3(10., 20., 30.);
213  Vector b = Vector3(2.0, 5.0, 6.0);
214  DOUBLES_EQUAL(20+100+180,dot(a,b),1e-9);
215 }
216 
217 /* ************************************************************************* */
218 TEST(Vector, axpy )
219 {
220  Vector x = Vector3(10., 20., 30.);
221  Vector y0 = Vector3(2.0, 5.0, 6.0);
222  Vector y1 = y0, y2 = y0;
223  axpy(0.1,x,y1);
224  axpy(0.1,x,y2.head(3));
225  Vector expected = Vector3(3.0, 7.0, 9.0);
226  EXPECT(assert_equal(expected,y1));
227  EXPECT(assert_equal(expected,Vector(y2)));
228 }
229 
230 /* ************************************************************************* */
231 TEST(Vector, equals )
232 {
233  Vector v1 = (Vector(1) << 0.0/std::numeric_limits<double>::quiet_NaN()).finished(); //testing nan
234  Vector v2 = (Vector(1) << 1.0).finished();
235  double tol = 1.;
236  EXPECT(!equal_with_abs_tol(v1, v2, tol));
237 }
238 
239 /* ************************************************************************* */
240 TEST(Vector, greater_than )
241 {
242  Vector v1 = Vector3(1.0, 2.0, 3.0),
243  v2 = Z_3x1;
244  EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than
245  EXPECT(greaterThanOrEqual(v1, v2)); // test equals
246 }
247 
248 /* ************************************************************************* */
249 TEST(Vector, linear_dependent )
250 {
251  Vector v1 = Vector3(1.0, 2.0, 3.0);
252  Vector v2 = Vector3(-2.0, -4.0, -6.0);
253  EXPECT(linear_dependent(v1, v2));
254 }
255 
256 /* ************************************************************************* */
257 TEST(Vector, linear_dependent2 )
258 {
259  Vector v1 = Vector3(0.0, 2.0, 0.0);
260  Vector v2 = Vector3(0.0, -4.0, 0.0);
261  EXPECT(linear_dependent(v1, v2));
262 }
263 
264 /* ************************************************************************* */
265 TEST(Vector, linear_dependent3 )
266 {
267  Vector v1 = Vector3(0.0, 2.0, 0.0);
268  Vector v2 = Vector3(0.1, -4.1, 0.0);
269  EXPECT(!linear_dependent(v1, v2));
270 }
271 
272 //******************************************************************************
273 TEST(Vector, IsVectorSpace) {
274  BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>));
275  BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector>));
276  typedef Eigen::Matrix<double,1,-1> RowVector;
277  BOOST_CONCEPT_ASSERT((IsVectorSpace<RowVector>));
278 }
279 
280 /* ************************************************************************* */
281 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
282 /* ************************************************************************* */
gtsam::weightedPseudoinverse
double weightedPseudoinverse(const Vector &a, const Vector &weights, Vector &pseudo)
Definition: Vector.cpp:246
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
v2
Vector v2
Definition: testSerializationBase.cpp:36
b
Scalar * b
Definition: benchVecAdd.cpp:17
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
v1
Vector v1
Definition: testSerializationBase.cpp:35
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:142
gtsam::BOOST_CONCEPT_ASSERT
BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept< ListOfOneContainer< int > >))
std
Definition: Half.h:150
isinf
#define isinf(X)
Definition: main.h:73
gtsam::IsVectorSpace
Vector Space concept.
Definition: VectorSpace.h:470
B
Matrix< SCALARB, Dynamic, Dynamic > B
Definition: bench_gemm.cpp:36
Eigen::DenseBase
Base class for all dense matrices, vectors, and arrays.
Definition: DenseBase.h:41
a
Array33i a
Definition: Cwise_product.cpp:1
Eigen::internal::negate
T negate(const T &x)
Definition: packetmath.cpp:27
householder
void householder(const MatrixType &m)
Definition: householder.cpp:13
dot
Scalar EIGEN_BLAS_FUNC() dot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Definition: level1_real_impl.h:27
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
data
int data[]
Definition: Map_placement_new.cpp:1
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::greaterThanOrEqual
bool greaterThanOrEqual(const Vector &vec1, const Vector &vec2)
Definition: Vector.cpp:114
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:84
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:37
gtsam
traits
Definition: chartTesting.h:28
Vector.h
typedef and functions to augment Eigen&#39;s VectorXd
gtsam::linear_dependent
bool linear_dependent(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:116
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
precision
cout precision(2)
TEST
TEST(Vector, special_comma_initializer)
Definition: testVector.cpp:45
gtsam::house
pair< double, Vector > house(const Vector &x)
Definition: Vector.cpp:237
copy
int EIGEN_BLAS_FUNC() copy(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Definition: level1_impl.h:29
gtsam::tol
const G double tol
Definition: Group.h:83
axpy
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
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
main
int main()
Definition: testVector.cpp:281
x
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
Definition: gnuplot_common_settings.hh:12
abs
#define abs(x)
Definition: datatypes.h:17
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
Eigen::MatrixBase
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
gtsam::concatVectors
Vector concatVectors(const std::list< Vector > &vs)
Definition: Vector.cpp:302
gtsam::RowVector
Eigen::RowVectorXd RowVector
Definition: LinearCost.h:25

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