testMatrix.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 
19 #include <gtsam/base/Matrix.h>
20 #include <gtsam/base/VectorSpace.h>
21 #include <gtsam/base/testLie.h>
22 #include <CppUnitLite/TestHarness.h>
23 #include <iostream>
24 #include <sstream>
25 #include <optional>
26 #include <functional>
27 
28 using namespace std;
29 using namespace gtsam;
30 
31 static double inf = std::numeric_limits<double>::infinity();
32 static const double tol = 1e-9;
33 
34 /* ************************************************************************* */
35 TEST(Matrix, constructor_data )
36 {
37  Matrix A = (Matrix(2, 2) << -5, 3, 0, -5).finished();
38 
39  Matrix B(2, 2);
40  B(0, 0) = -5;
41  B(0, 1) = 3;
42  B(1, 0) = 0;
43  B(1, 1) = -5;
44 
45  EQUALITY(A,B);
46 }
47 
48 /* ************************************************************************* */
49 TEST(Matrix, Matrix_ )
50 {
51  Matrix A = (Matrix(2, 2) << -5.0, 3.0, 0.0, -5.0).finished();
52  Matrix B(2, 2);
53  B(0, 0) = -5;
54  B(0, 1) = 3;
55  B(1, 0) = 0;
56  B(1, 1) = -5;
57 
58  EQUALITY(A,B);
59 
60 }
61 
62 namespace {
63  /* ************************************************************************* */
64  template<typename Derived>
65  Matrix testFcn1(const Eigen::DenseBase<Derived>& in)
66  {
67  return in;
68  }
69 
70  /* ************************************************************************* */
71  template<typename Derived>
72  Matrix testFcn2(const Eigen::MatrixBase<Derived>& in)
73  {
74  return in;
75  }
76 }
77 
78 /* ************************************************************************* */
79 TEST(Matrix, special_comma_initializer)
80 {
81  Matrix expected(2,2);
82  expected(0,0) = 1;
83  expected(0,1) = 2;
84  expected(1,0) = 3;
85  expected(1,1) = 4;
86 
87  Matrix actual1 = (Matrix(2,2) << 1, 2, 3, 4).finished();
88  Matrix actual2((Matrix(2,2) << 1, 2, 3, 4).finished());
89 
90  Matrix submat1 = (Matrix(1,2) << 3, 4).finished();
91  Matrix actual3 = (Matrix(2,2) << 1, 2, submat1).finished();
92 
93  Matrix submat2 = (Matrix(1,2) << 1, 2).finished();
94  Matrix actual4 = (Matrix(2,2) << submat2, 3, 4).finished();
95 
96  Matrix actual5 = testFcn1((Matrix(2,2) << 1, 2, 3, 4).finished());
97  Matrix actual6 = testFcn2((Matrix(2,2) << 1, 2, 3, 4).finished());
98 
99  EXPECT(assert_equal(expected, actual1));
100  EXPECT(assert_equal(expected, actual2));
101  EXPECT(assert_equal(expected, actual3));
102  EXPECT(assert_equal(expected, actual4));
103  EXPECT(assert_equal(expected, actual5));
104  EXPECT(assert_equal(expected, actual6));
105 }
106 
107 /* ************************************************************************* */
108 TEST(Matrix, col_major )
109 {
110  Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
111  const double * const a = &A(0, 0);
112  EXPECT_DOUBLES_EQUAL(1, a[0], tol);
113  EXPECT_DOUBLES_EQUAL(3, a[1], tol);
114  EXPECT_DOUBLES_EQUAL(2, a[2], tol);
115  EXPECT_DOUBLES_EQUAL(4, a[3], tol);
116 }
117 
118 /* ************************************************************************* */
119 TEST(Matrix, collect1 )
120 {
121  Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
122  Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
123  Matrix AB = collect(2, &A, &B);
124  Matrix C(2, 5);
125  for (int i = 0; i < 2; i++)
126  for (int j = 0; j < 2; j++)
127  C(i, j) = A(i, j);
128  for (int i = 0; i < 2; i++)
129  for (int j = 0; j < 3; j++)
130  C(i, j + 2) = B(i, j);
131 
132  EQUALITY(C,AB);
133 }
134 
135 /* ************************************************************************* */
136 TEST(Matrix, collect2 )
137 {
138  Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
139  Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
140  vector<const Matrix*> matrices;
141  matrices.push_back(&A);
142  matrices.push_back(&B);
143  Matrix AB = collect(matrices);
144  Matrix C(2, 5);
145  for (int i = 0; i < 2; i++)
146  for (int j = 0; j < 2; j++)
147  C(i, j) = A(i, j);
148  for (int i = 0; i < 2; i++)
149  for (int j = 0; j < 3; j++)
150  C(i, j + 2) = B(i, j);
151 
152  EQUALITY(C,AB);
153 }
154 
155 /* ************************************************************************* */
156 TEST(Matrix, collect3 )
157 {
158  Matrix A, B;
159  A = Matrix::Identity(2,3);
160  B = Matrix::Identity(2,3);
161  vector<const Matrix*> matrices;
162  matrices.push_back(&A);
163  matrices.push_back(&B);
164  Matrix AB = collect(matrices, 2, 3);
165  Matrix exp = (Matrix(2, 6) <<
166  1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
167  0.0, 1.0, 0.0, 0.0, 1.0, 0.0).finished();
168 
169  EQUALITY(exp,AB);
170 }
171 
172 /* ************************************************************************* */
173 TEST(Matrix, stack )
174 {
175  Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
176  Matrix B = (Matrix(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
177  Matrix AB = gtsam::stack(2, &A, &B);
178  Matrix C(5, 2);
179  for (int i = 0; i < 2; i++)
180  for (int j = 0; j < 2; j++)
181  C(i, j) = A(i, j);
182  for (int i = 0; i < 3; i++)
183  for (int j = 0; j < 2; j++)
184  C(i + 2, j) = B(i, j);
185 
186  EQUALITY(C,AB);
187 
188  std::vector<gtsam::Matrix> matrices;
189  matrices.push_back(A);
190  matrices.push_back(B);
191  Matrix AB2 = gtsam::stack(matrices);
192  EQUALITY(C,AB2);
193 }
194 
195 /* ************************************************************************* */
196 TEST(Matrix, column )
197 {
198  Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1.,
199  0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1.,
200  -0.1).finished();
201  Vector a1 = column(A, 0);
202  Vector exp1 = (Vector(4) << -1., 0., 1., 0.).finished();
203  EXPECT(assert_equal(a1, exp1));
204 
205  Vector a2 = column(A, 3);
206  Vector exp2 = (Vector(4) << 0., 1., 0., 0.).finished();
207  EXPECT(assert_equal(a2, exp2));
208 
209  Vector a3 = column(A, 6);
210  Vector exp3 = (Vector(4) << -0.2, 0.3, 0.2, -0.1).finished();
211  EXPECT(assert_equal(a3, exp3));
212 }
213 
214 /* ************************************************************************* */
215 TEST(Matrix, row )
216 {
217  Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1.,
218  0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1.,
219  -0.1).finished();
220  Vector a1 = row(A, 0);
221  Vector exp1 = (Vector(7) << -1., 0., 1., 0., 0., 0., -0.2).finished();
222  EXPECT(assert_equal(a1, exp1));
223 
224  Vector a2 = row(A, 2);
225  Vector exp2 = (Vector(7) << 1., 0., 0., 0., -1., 0., 0.2).finished();
226  EXPECT(assert_equal(a2, exp2));
227 
228  Vector a3 = row(A, 3);
229  Vector exp3 = (Vector(7) << 0., 1., 0., 0., 0., -1., -0.1).finished();
230  EXPECT(assert_equal(a3, exp3));
231 }
232 
233 /* ************************************************************************* */
234 TEST(Matrix, insert_sub )
235 {
236  Matrix big = Matrix::Zero(5,6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0,
237  1.0).finished();
238 
239  insertSub(big, small, 1, 2);
240 
241  Matrix expected = (Matrix(5, 6) << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
242  1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0,
243  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished();
244 
245  EXPECT(assert_equal(expected, big));
246 }
247 
248 /* ************************************************************************* */
249 TEST(Matrix, diagMatrices )
250 {
251  std::vector<Matrix> Hs;
252  Hs.push_back(Matrix::Ones(3,3));
253  Hs.push_back(Matrix::Ones(4,4)*2);
254  Hs.push_back(Matrix::Ones(2,2)*3);
255 
256  Matrix actual = diag(Hs);
257 
258  Matrix expected = (Matrix(9, 9) <<
259  1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
260  1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
261  1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
262  0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0,
263  0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0,
264  0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0,
265  0.0, 0.0, 0.0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0,
266  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 3.0,
267  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 3.0).finished();
268 
269  EXPECT(assert_equal(expected, actual));
270 }
271 
272 /* ************************************************************************* */
273 TEST(Matrix, stream_read ) {
274  Matrix expected = (Matrix(3,4) <<
275  1.1, 2.3, 4.2, 7.6,
276  -0.3, -8e-2, 5.1, 9.0,
277  1.2, 3.4, 4.5, 6.7).finished();
278 
279  string matrixAsString =
280  "1.1 2.3 4.2 7.6\n"
281  "-0.3 -8e-2 5.1 9.0\n\r" // Test extra spaces and windows newlines
282  "1.2 \t 3.4 4.5 6.7"; // Test tab as separator
283 
284  stringstream asStream(matrixAsString, ios::in);
285 
286  Matrix actual;
287  asStream >> actual;
288 
289  EXPECT(assert_equal(expected, actual));
290 }
291 
292 /* ************************************************************************* */
293 TEST(Matrix, scale_columns )
294 {
295  Matrix A(3, 4);
296  A(0, 0) = 1.;
297  A(0, 1) = 1.;
298  A(0, 2) = 1.;
299  A(0, 3) = 1.;
300  A(1, 0) = 1.;
301  A(1, 1) = 1.;
302  A(1, 2) = 1.;
303  A(1, 3) = 1.;
304  A(2, 0) = 1.;
305  A(2, 1) = 1.;
306  A(2, 2) = 1.;
307  A(2, 3) = 1.;
308 
309  Vector v = (Vector(4) << 2., 3., 4., 5.).finished();
310 
311  Matrix actual = vector_scale(A, v);
312 
313  Matrix expected(3, 4);
314  expected(0, 0) = 2.;
315  expected(0, 1) = 3.;
316  expected(0, 2) = 4.;
317  expected(0, 3) = 5.;
318  expected(1, 0) = 2.;
319  expected(1, 1) = 3.;
320  expected(1, 2) = 4.;
321  expected(1, 3) = 5.;
322  expected(2, 0) = 2.;
323  expected(2, 1) = 3.;
324  expected(2, 2) = 4.;
325  expected(2, 3) = 5.;
326 
327  EXPECT(assert_equal(actual, expected));
328 }
329 
330 /* ************************************************************************* */
331 TEST(Matrix, scale_rows )
332 {
333  Matrix A(3, 4);
334  A(0, 0) = 1.;
335  A(0, 1) = 1.;
336  A(0, 2) = 1.;
337  A(0, 3) = 1.;
338  A(1, 0) = 1.;
339  A(1, 1) = 1.;
340  A(1, 2) = 1.;
341  A(1, 3) = 1.;
342  A(2, 0) = 1.;
343  A(2, 1) = 1.;
344  A(2, 2) = 1.;
345  A(2, 3) = 1.;
346 
347  Vector v = Vector3(2., 3., 4.);
348 
349  Matrix actual = vector_scale(v, A);
350 
351  Matrix expected(3, 4);
352  expected(0, 0) = 2.;
353  expected(0, 1) = 2.;
354  expected(0, 2) = 2.;
355  expected(0, 3) = 2.;
356  expected(1, 0) = 3.;
357  expected(1, 1) = 3.;
358  expected(1, 2) = 3.;
359  expected(1, 3) = 3.;
360  expected(2, 0) = 4.;
361  expected(2, 1) = 4.;
362  expected(2, 2) = 4.;
363  expected(2, 3) = 4.;
364 
365  EXPECT(assert_equal(actual, expected));
366 }
367 
368 /* ************************************************************************* */
369 TEST(Matrix, scale_rows_mask )
370 {
371  Matrix A(3, 4);
372  A(0, 0) = 1.;
373  A(0, 1) = 1.;
374  A(0, 2) = 1.;
375  A(0, 3) = 1.;
376  A(1, 0) = 1.;
377  A(1, 1) = 1.;
378  A(1, 2) = 1.;
379  A(1, 3) = 1.;
380  A(2, 0) = 1.;
381  A(2, 1) = 1.;
382  A(2, 2) = 1.;
383  A(2, 3) = 1.;
384 
385  Vector v = (Vector(3) << 2., std::numeric_limits<double>::infinity(), 4.).finished();
386 
387  Matrix actual = vector_scale(v, A, true);
388 
389  Matrix expected(3, 4);
390  expected(0, 0) = 2.;
391  expected(0, 1) = 2.;
392  expected(0, 2) = 2.;
393  expected(0, 3) = 2.;
394  expected(1, 0) = 1.;
395  expected(1, 1) = 1.;
396  expected(1, 2) = 1.;
397  expected(1, 3) = 1.;
398  expected(2, 0) = 4.;
399  expected(2, 1) = 4.;
400  expected(2, 2) = 4.;
401  expected(2, 3) = 4.;
402 
403  EXPECT(assert_equal(actual, expected));
404 }
405 
406 /* ************************************************************************* */
407 TEST(Matrix, skewSymmetric )
408 {
409  double wx = 1, wy = 2, wz = 3;
410  Matrix3 actual = skewSymmetric(wx,wy,wz);
411 
412  Matrix expected(3,3);
413  expected << 0, -3, 2,
414  3, 0, -1,
415  -2, 1, 0;
416 
417  EXPECT(assert_equal(actual, expected));
418 
419 }
420 
421 
422 /* ************************************************************************* */
423 TEST(Matrix, equal )
424 {
425  Matrix A(4, 4);
426  A(0, 0) = -1;
427  A(0, 1) = 1;
428  A(0, 2) = 2;
429  A(0, 3) = 3;
430  A(1, 0) = 1;
431  A(1, 1) = -3;
432  A(1, 2) = 1;
433  A(1, 3) = 3;
434  A(2, 0) = 1;
435  A(2, 1) = 2;
436  A(2, 2) = -1;
437  A(2, 3) = 4;
438  A(3, 0) = 2;
439  A(3, 1) = 1;
440  A(3, 2) = 2;
441  A(3, 3) = -2;
442 
443  Matrix A2(A);
444 
445  Matrix A3(A);
446  A3(3, 3) = -2.1;
447 
448  EXPECT(A==A2);
449  EXPECT(A!=A3);
450 }
451 
452 /* ************************************************************************* */
453 TEST(Matrix, equal_nan )
454 {
455  Matrix A(4, 4);
456  A(0, 0) = -1;
457  A(0, 1) = 1;
458  A(0, 2) = 2;
459  A(0, 3) = 3;
460  A(1, 0) = 1;
461  A(1, 1) = -3;
462  A(1, 2) = 1;
463  A(1, 3) = 3;
464  A(2, 0) = 1;
465  A(2, 1) = 2;
466  A(2, 2) = -1;
467  A(2, 3) = 4;
468  A(3, 0) = 2;
469  A(3, 1) = 1;
470  A(3, 2) = 2;
471  A(3, 3) = -2;
472 
473  Matrix A2(A);
474 
475  Matrix A3(A);
476  A3(3, 3) = inf;
477 
478  EXPECT(A!=A3);
479 }
480 
481 /* ************************************************************************* */
482 TEST(Matrix, addition )
483 {
484  Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
485  Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
486  Matrix C = (Matrix(2, 2) << 5.0, 5.0, 5.0, 5.0).finished();
487  EQUALITY(A+B,C);
488 }
489 
490 /* ************************************************************************* */
491 TEST(Matrix, addition_in_place )
492 {
493  Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
494  Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
495  Matrix C = (Matrix(2, 2) << 5.0, 5.0, 5.0, 5.0).finished();
496  A += B;
497  EQUALITY(A,C);
498 }
499 
500 /* ************************************************************************* */
501 TEST(Matrix, subtraction )
502 {
503  Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
504  Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
505  Matrix C = (Matrix(2, 2) << -3.0, -1.0, 1.0, 3.0).finished();
506  EQUALITY(A-B,C);
507 }
508 
509 /* ************************************************************************* */
510 TEST(Matrix, subtraction_in_place )
511 {
512  Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
513  Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
514  Matrix C = (Matrix(2, 2) << -3.0, -1.0, 1.0, 3.0).finished();
515  A -= B;
516  EQUALITY(A,C);
517 }
518 
519 /* ************************************************************************* */
520 TEST(Matrix, multiplication )
521 {
522  Matrix A(2, 2);
523  A(0, 0) = -1;
524  A(1, 0) = 1;
525  A(0, 1) = 1;
526  A(1, 1) = -3;
527 
528  Matrix B(2, 1);
529  B(0, 0) = 1.2;
530  B(1, 0) = 3.4;
531 
532  Matrix AB(2, 1);
533  AB(0, 0) = 2.2;
534  AB(1, 0) = -9.;
535 
536  EQUALITY(A*B,AB);
537 }
538 
539 /* ************************************************************************* */
540 TEST(Matrix, scalar_matrix_multiplication )
541 {
542  Vector result(2);
543 
544  Matrix A(2, 2);
545  A(0, 0) = -1;
546  A(1, 0) = 1;
547  A(0, 1) = 1;
548  A(1, 1) = -3;
549 
550  Matrix B(2, 2);
551  B(0, 0) = -10;
552  B(1, 0) = 10;
553  B(0, 1) = 10;
554  B(1, 1) = -30;
555 
556  EQUALITY((10*A),B);
557 }
558 
559 /* ************************************************************************* */
560 TEST(Matrix, matrix_vector_multiplication )
561 {
562  Vector result(2);
563 
564  Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
565  Vector v = Vector3(1., 2., 3.);
566  Vector Av = Vector2(14., 32.);
567  Vector AtAv = Vector3(142., 188., 234.);
568 
569  EQUALITY(A*v,Av);
570  EQUALITY(A^Av,AtAv);
571 }
572 
573 /* ************************************************************************* */
574 TEST(Matrix, nrRowsAndnrCols )
575 {
576  Matrix A(3, 6);
577  LONGS_EQUAL( A.rows() , 3 );
578  LONGS_EQUAL( A.cols() , 6 );
579 }
580 
581 /* ************************************************************************* */
582 TEST(Matrix, scalar_divide )
583 {
584  Matrix A(2, 2);
585  A(0, 0) = 10;
586  A(1, 0) = 30;
587  A(0, 1) = 20;
588  A(1, 1) = 40;
589 
590  Matrix B(2, 2);
591  B(0, 0) = 1;
592  B(1, 0) = 3;
593  B(0, 1) = 2;
594  B(1, 1) = 4;
595 
596  EQUALITY(B,A/10);
597 }
598 
599 /* ************************************************************************* */
600 TEST(Matrix, zero_below_diagonal ) {
601  Matrix A1 = (Matrix(3, 4) <<
602  1.0, 2.0, 3.0, 4.0,
603  1.0, 2.0, 3.0, 4.0,
604  1.0, 2.0, 3.0, 4.0).finished();
605 
606  Matrix expected1 = (Matrix(3, 4) <<
607  1.0, 2.0, 3.0, 4.0,
608  0.0, 2.0, 3.0, 4.0,
609  0.0, 0.0, 3.0, 4.0).finished();
610  Matrix actual1r = A1;
611  zeroBelowDiagonal(actual1r);
612  EXPECT(assert_equal(expected1, actual1r, 1e-10));
613 
614  Matrix actual1c = A1;
615  zeroBelowDiagonal(actual1c);
616  EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10));
617 
618  actual1c = A1;
619  zeroBelowDiagonal(actual1c, 4);
620  EXPECT(assert_equal(Matrix(expected1), actual1c, 1e-10));
621 
622  Matrix A2 = (Matrix(5, 3) <<
623  1.0, 2.0, 3.0,
624  1.0, 2.0, 3.0,
625  1.0, 2.0, 3.0,
626  1.0, 2.0, 3.0,
627  1.0, 2.0, 3.0).finished();
628  Matrix expected2 = (Matrix(5, 3) <<
629  1.0, 2.0, 3.0,
630  0.0, 2.0, 3.0,
631  0.0, 0.0, 3.0,
632  0.0, 0.0, 0.0,
633  0.0, 0.0, 0.0).finished();
634 
635  Matrix actual2r = A2;
636  zeroBelowDiagonal(actual2r);
637  EXPECT(assert_equal(expected2, actual2r, 1e-10));
638 
639  Matrix actual2c = A2;
640  zeroBelowDiagonal(actual2c);
641  EXPECT(assert_equal(Matrix(expected2), actual2c, 1e-10));
642 
643  Matrix expected2_partial = (Matrix(5, 3) <<
644  1.0, 2.0, 3.0,
645  0.0, 2.0, 3.0,
646  0.0, 2.0, 3.0,
647  0.0, 2.0, 3.0,
648  0.0, 2.0, 3.0).finished();
649  actual2c = A2;
650  zeroBelowDiagonal(actual2c, 1);
651  EXPECT(assert_equal(Matrix(expected2_partial), actual2c, 1e-10));
652 }
653 
654 /* ************************************************************************* */
655 TEST(Matrix, inverse )
656 {
657  Matrix A(3, 3);
658  A(0, 0) = 1;
659  A(0, 1) = 2;
660  A(0, 2) = 3;
661  A(1, 0) = 0;
662  A(1, 1) = 4;
663  A(1, 2) = 5;
664  A(2, 0) = 1;
665  A(2, 1) = 0;
666  A(2, 2) = 6;
667 
668  Matrix Ainv = A.inverse();
669  EXPECT(assert_equal((Matrix) I_3x3, A*Ainv));
670  EXPECT(assert_equal((Matrix) I_3x3, Ainv*A));
671 
672  Matrix expected(3, 3);
673  expected(0, 0) = 1.0909;
674  expected(0, 1) = -0.5454;
675  expected(0, 2) = -0.0909;
676  expected(1, 0) = 0.2272;
677  expected(1, 1) = 0.1363;
678  expected(1, 2) = -0.2272;
679  expected(2, 0) = -0.1818;
680  expected(2, 1) = 0.0909;
681  expected(2, 2) = 0.1818;
682 
683  EXPECT(assert_equal(expected, Ainv, 1e-4));
684 
685  // These two matrices failed before version 2003 because we called LU incorrectly
686  Matrix lMg((Matrix(3, 3) << 0.0, 1.0, -2.0, -1.0, 0.0, 1.0, 0.0, 0.0, 1.0).finished());
687  EXPECT(assert_equal((Matrix(3, 3) <<
688  0.0, -1.0, 1.0,
689  1.0, 0.0, 2.0,
690  0.0, 0.0, 1.0).finished(),
691  lMg.inverse()));
692  Matrix gMl((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0).finished());
693  EXPECT(assert_equal((Matrix(3, 3) <<
694  0.0, 1.0,-2.0,
695  -1.0, 0.0, 1.0,
696  0.0, 0.0, 1.0).finished(),
697  gMl.inverse()));
698 }
699 
700 /* ************************************************************************* */
701 TEST(Matrix, inverse2 )
702 {
703  Matrix A(3, 3);
704  A(0, 0) = 0;
705  A(0, 1) = -1;
706  A(0, 2) = 1;
707  A(1, 0) = 1;
708  A(1, 1) = 0;
709  A(1, 2) = 2;
710  A(2, 0) = 0;
711  A(2, 1) = 0;
712  A(2, 2) = 1;
713 
714  Matrix Ainv = A.inverse();
715 
716  Matrix expected(3, 3);
717  expected(0, 0) = 0;
718  expected(0, 1) = 1;
719  expected(0, 2) = -2;
720  expected(1, 0) = -1;
721  expected(1, 1) = 0;
722  expected(1, 2) = 1;
723  expected(2, 0) = 0;
724  expected(2, 1) = 0;
725  expected(2, 2) = 1;
726 
727  EXPECT(assert_equal(expected, Ainv, 1e-4));
728 }
729 
730 /* ************************************************************************* */
731 TEST(Matrix, backsubtitution )
732 {
733  // TEST ONE 2x2 matrix U1*x=b1
734  Vector expected1 = Vector2(3.6250, -0.75);
735  Matrix U22 = (Matrix(2, 2) << 2., 3., 0., 4.).finished();
736  Vector b1 = U22 * expected1;
737  EXPECT( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001));
738 
739  // TEST TWO 3x3 matrix U2*x=b2
740  Vector expected2 = Vector3(5.5, -8.5, 5.);
741  Matrix U33 = (Matrix(3, 3) << 3., 5., 6., 0., 2., 3., 0., 0., 1.).finished();
742  Vector b2 = U33 * expected2;
743  EXPECT( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001));
744 
745  // TEST THREE Lower triangular 3x3 matrix L3*x=b3
746  Vector expected3 = Vector3(1., 1., 1.);
747  Matrix L3 = trans(U33);
748  Vector b3 = L3 * expected3;
749  EXPECT( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001));
750 
751  // TEST FOUR Try the above with transpose backSubstituteUpper
752  EXPECT( assert_equal(expected3 , backSubstituteUpper(b3,U33), 0.000001));
753 }
754 
755 /* ************************************************************************* */
756 TEST(Matrix, householder )
757 {
758  // check in-place householder, with v vectors below diagonal
759 
760  Matrix expected1 = (Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236,
761  0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565,
762  -0.618034, 0, 4.4721, 0, -4.4721, 0, 0,
763  0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894).finished();
764  Matrix A1 = (Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1,
765  00,-5, 0, 5, 0, 0, 1.5,
766  10, 0, 0, 0,-10,0, 2,
767  00, 10,0, 0, 0, -10, -1 ).finished();
768  householder_(A1, 3);
769  EXPECT(assert_equal(expected1, A1, 1e-3));
770 
771  // in-place, with zeros below diagonal
772 
773  Matrix expected = (Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803,
774  0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0,
775  0, 0, 4.4721, 0, -4.4721, 0.894).finished();
776  Matrix A2 = (Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1,
777  00,-5, 0, 5, 0, 0, 1.5,
778  10, 0, 0, 0,-10,0, 2,
779  00, 10,0, 0, 0, -10, -1).finished();
780  householder(A2, 3);
781  EXPECT(assert_equal(expected, A2, 1e-3));
782 }
783 
784 /* ************************************************************************* */
785 TEST(Matrix, householder_colMajor )
786 {
787  // check in-place householder, with v vectors below diagonal
788 
789  Matrix expected1((Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236,
790  0, 11.1803, 0, -2.2361, 0, -8.9443, -1.565,
791  -0.618034, 0, 4.4721, 0, -4.4721, 0, 0,
792  0, -0.618034, 0, 4.4721, 0, -4.4721, 0.894).finished());
793  Matrix A1((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1,
794  00,-5, 0, 5, 0, 0, 1.5,
795  10, 0, 0, 0,-10,0, 2,
796  00, 10,0, 0, 0, -10, -1).finished());
797  householder_(A1, 3);
798  EXPECT(assert_equal(expected1, A1, 1e-3));
799 
800  // in-place, with zeros below diagonal
801 
802  Matrix expected((Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803,
803  0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0,
804  0, 0, 4.4721, 0, -4.4721, 0.894).finished());
805  Matrix A2((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1,
806  00,-5, 0, 5, 0, 0, 1.5,
807  10, 0, 0, 0,-10,0, 2,
808  00, 10,0, 0, 0, -10, -1).finished());
809  householder(A2, 3);
810  EXPECT(assert_equal(expected, A2, 1e-3));
811 }
812 
813 /* ************************************************************************* */
814 TEST(Matrix, eigen_QR )
815 {
816  // use standard Eigen function to yield a non-in-place QR factorization
817 
818  // in-place, with zeros below diagonal
819 
820  Matrix expected((Matrix(4, 7) << 11.1803, 0, -2.2361, 0, -8.9443, 0, 2.236, 0, 11.1803,
821  0, -2.2361, 0, -8.9443, -1.565, 0, 0, 4.4721, 0, -4.4721, 0, 0, 0,
822  0, 0, 4.4721, 0, -4.4721, 0.894).finished());
823  Matrix A((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1,
824  00,-5, 0, 5, 0, 0, 1.5,
825  10, 0, 0, 0,-10,0, 2,
826  00, 10,0, 0, 0, -10, -1).finished());
827  Matrix actual = A.householderQr().matrixQR();
828  zeroBelowDiagonal(actual);
829 
830  EXPECT(assert_equal(expected, actual, 1e-3));
831 
832  // use shiny new in place QR inside gtsam
833  A = Matrix((Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1,
834  00,-5, 0, 5, 0, 0, 1.5,
835  10, 0, 0, 0,-10,0, 2,
836  00, 10,0, 0, 0, -10, -1).finished());
837  inplace_QR(A);
838  EXPECT(assert_equal(expected, A, 1e-3));
839 }
840 
841 /* ************************************************************************* */
842 // unit test for qr factorization (and hence householder)
843 // This behaves the same as QR in matlab: [Q,R] = qr(A), except for signs
844 /* ************************************************************************* */
845 TEST(Matrix, qr )
846 {
847 
848  Matrix A = (Matrix(6, 4) << -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00,
849  0, 0, -10, 10, 0, -10, 0).finished();
850 
851 
852  Matrix expectedQ = (Matrix(6, 6) << -0.3333, 0, 0.2981, 0, 0, -0.8944, 0000000, -0.4472, 0,
853  0.3651, -0.8165, 0, 00.6667, 0, 0.7454, 0, 0, 0, 0000000, 0.8944,
854  0, 0.1826, -0.4082, 0, 0000000, 0, 0, -0.9129, -0.4082, 0, 00.6667,
855  0, -0.5963, 0, 0, -0.4472).finished();
856 
857  Matrix expectedR = (Matrix(6, 4) << 15, 0, -8.3333, 0, 00, 11.1803, 0, -2.2361, 00, 0,
858  7.4536, 0, 00, 0, 0, 10.9545, 00, 0, 0, 0, 00, 0, 0, 0).finished();
859 
860  const auto [Q, R] = qr(A);
861  EXPECT(assert_equal(expectedQ, Q, 1e-4));
862  EXPECT(assert_equal(expectedR, R, 1e-4));
863  EXPECT(assert_equal(A, Q*R, 1e-14));
864 }
865 
866 /* ************************************************************************* */
867 TEST(Matrix, sub )
868 {
869  Matrix A = (Matrix(4, 6) << -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10,
870  0, 00, 10, 0, 0, 0, -10).finished();
871  Matrix actual = sub(A, 1, 3, 1, 5);
872 
873  Matrix expected = (Matrix(2, 4) << -5, 0, 5, 0, 00, 0, 0, -10).finished();
874 
875  EQUALITY(actual,expected);
876 }
877 
878 /* ************************************************************************* */
879 TEST(Matrix, trans )
880 {
881  Matrix A = (Matrix(2, 2) << 1.0, 3.0, 2.0, 4.0).finished();
882  Matrix B = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
883  EQUALITY(trans(A),B);
884 }
885 
886 /* ************************************************************************* */
887 TEST(Matrix, col_major_access )
888 {
889  Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
890  const double* a = &A(0, 0);
891  DOUBLES_EQUAL(2.0,a[2],1e-9);
892 }
893 
894 /* ************************************************************************* */
895 TEST(Matrix, weighted_elimination )
896 {
897  // create a matrix to eliminate
898  Matrix A = (Matrix(4, 6) << -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0.,
899  1., 0., 0., 0., -1., 0., 0., 1., 0., 0., 0., -1.).finished();
900  Vector b = (Vector(4) << -0.2, 0.3, 0.2, -0.1).finished();
901  Vector sigmas = (Vector(4) << 0.2, 0.2, 0.1, 0.1).finished();
902 
903  // expected values
904  Matrix expectedR = (Matrix(4, 6) << 1., 0., -0.2, 0., -0.8, 0., 0., 1., 0.,
905  -0.2, 0., -0.8, 0., 0., 1., 0., -1., 0., 0., 0., 0., 1., 0., -1.).finished();
906  Vector d = (Vector(4) << 0.2, -0.14, 0.0, 0.2).finished();
907  Vector newSigmas = (Vector(4) << 0.0894427, 0.0894427, 0.223607, 0.223607).finished();
908 
909  // perform elimination
910  Matrix A1 = A;
911  Vector b1 = b;
912  std::list<std::tuple<Vector, double, double> > solution =
913  weighted_eliminate(A1, b1, sigmas);
914 
915  // unpack and verify
916  size_t i = 0;
917  for (const auto& [r, di, sigma] : solution) {
918  EXPECT(assert_equal(r, expectedR.row(i))); // verify r
919  DOUBLES_EQUAL(d(i), di, 1e-8); // verify d
920  DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma
921  i += 1;
922  }
923 }
924 
925 /* ************************************************************************* */
926 TEST(Matrix, inverse_square_root )
927 {
928  Matrix measurement_covariance = (Matrix(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25,
929  0.0, 0.0, 0.0, 0.01).finished();
930  Matrix actual = inverse_square_root(measurement_covariance);
931 
932  Matrix expected = (Matrix(3, 3) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0,
933  10.0).finished();
934 
935  EQUALITY(expected,actual);
936  EQUALITY(measurement_covariance,(actual*actual).inverse());
937 
938  // Randomly generated test. This test really requires inverse to
939  // be working well; if it's not, there's the possibility of a
940  // bug in inverse masking a bug in this routine since we
941  // use the same inverse routing inside inverse_square_root()
942  // as we use here to check it.
943 
944  Matrix M = (Matrix(5, 5) <<
945  0.0785892, 0.0137923, -0.0142219, -0.0171880, 0.0028726,
946  0.0137923, 0.0908911, 0.0020775, -0.0101952, 0.0175868,
947  -0.0142219, 0.0020775, 0.0973051, 0.0054906, 0.0047064,
948  -0.0171880,-0.0101952, 0.0054906, 0.0892453, -0.0059468,
949  0.0028726, 0.0175868, 0.0047064, -0.0059468, 0.0816517).finished();
950 
951  expected = (Matrix(5, 5) <<
952  3.567126953241796, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000,
953  -0.590030436566913, 3.362022286742925, 0.000000000000000, 0.000000000000000, 0.000000000000000,
954  0.618207860252376, -0.168166020746503, 3.253086082942785, 0.000000000000000, 0.000000000000000,
955  0.683045380655496, 0.283773848115276, -0.099969232183396, 3.433537147891568, 0.000000000000000,
956  -0.006740136923185, -0.669325697387650, -0.169716689114923, 0.171493059476284, 3.583921085468937).finished();
957  EQUALITY(expected, inverse_square_root(M));
958 
959 }
960 
961 /* *********************************************************************** */
962 // M was generated as the covariance of a set of random numbers. L that
963 // we are checking against was generated via chol(M)' on octave
964 namespace cholesky {
965 Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463,
966  0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363,
967  0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463,
968  0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363,
969  -0.0021113, 0.0036415, 0.0909464).finished();
970 
971 Matrix expected = (Matrix(5, 5) <<
972  0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000,
973  -0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000,
974  0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000,
975  0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000,
976  0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247).finished();
977 }
978 TEST(Matrix, LLt )
979 {
980  EQUALITY(cholesky::expected, LLt(cholesky::M));
981 }
982 TEST(Matrix, RtR )
983 {
984  EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M));
985 }
986 
987 TEST(Matrix, cholesky_inverse )
988 {
989  EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M));
990 }
991 
992 /* ************************************************************************* */
993 TEST(Matrix, linear_dependent )
994 {
995  Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
996  Matrix B = (Matrix(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished();
997  EXPECT(linear_dependent(A, B));
998 }
999 
1000 /* ************************************************************************* */
1001 TEST(Matrix, linear_dependent2 )
1002 {
1003  Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
1004  Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished();
1005  EXPECT(linear_dependent(A, B));
1006 }
1007 
1008 /* ************************************************************************* */
1009 TEST(Matrix, linear_dependent3 )
1010 {
1011  Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
1012  Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0).finished();
1013  EXPECT(linear_independent(A, B));
1014 }
1015 
1016 /* ************************************************************************* */
1017 TEST(Matrix, svd1 )
1018 {
1019  Vector v = Vector3(2., 1., 0.);
1020  Matrix U1 = Matrix::Identity(4, 3), S1 = v.asDiagonal(), V1 = I_3x3, A = (U1 * S1)
1021  * Matrix(trans(V1));
1022  Matrix U, V;
1023  Vector s;
1024  svd(A, U, s, V);
1025  Matrix S = s.asDiagonal();
1026  EXPECT(assert_equal(U*S*Matrix(trans(V)),A));
1027  EXPECT(assert_equal(S,S1));
1028 }
1029 
1030 /* ************************************************************************* */
1032 static Matrix sampleA = (Matrix(3, 2) << 0.,-2., 0., 0., 3., 0.).finished();
1033 static Matrix sampleAt = trans(sampleA);
1034 
1035 /* ************************************************************************* */
1036 TEST(Matrix, svd2 )
1037 {
1038  Matrix U, V;
1039  Vector s;
1040 
1041  Matrix expectedU = (Matrix(3, 2) << 0.,-1.,0.,0.,1.,0.).finished();
1042  Vector expected_s = Vector2(3.,2.);
1043  Matrix expectedV = (Matrix(2, 2) << 1.,0.,0.,1.).finished();
1044 
1045  svd(sampleA, U, s, V);
1046 
1047  // take care of sign ambiguity
1048  if (U(0, 1) > 0) {
1049  U = -U;
1050  V = -V;
1051  }
1052 
1053  EXPECT(assert_equal(expectedU,U));
1054  EXPECT(assert_equal(expected_s,s,1e-9));
1055  EXPECT(assert_equal(expectedV,V));
1056 }
1057 
1058 /* ************************************************************************* */
1059 TEST(Matrix, svd3 )
1060 {
1061  Matrix U, V;
1062  Vector s;
1063 
1064  Matrix expectedU = (Matrix(2, 2) << -1.,0.,0.,-1.).finished();
1065  Vector expected_s = Vector2(3.0, 2.0);
1066  Matrix expectedV = (Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.).finished();
1067 
1068  svd(sampleAt, U, s, V);
1069 
1070  // take care of sign ambiguity
1071  if (U(0, 0) > 0) {
1072  U = -U;
1073  V = -V;
1074  }
1075 
1076  Matrix S = s.asDiagonal();
1077  Matrix t = U * S;
1078  Matrix Vt = trans(V);
1079 
1080  EXPECT(assert_equal(sampleAt, prod(t, Vt)));
1081  EXPECT(assert_equal(expectedU,U));
1082  EXPECT(assert_equal(expected_s,s,1e-9));
1083  EXPECT(assert_equal(expectedV,V));
1084 }
1085 
1086 /* ************************************************************************* */
1087 TEST(Matrix, svd4 )
1088 {
1089  Matrix U, V;
1090  Vector s;
1091 
1092  Matrix A = (Matrix(3, 2) <<
1093  0.8147, 0.9134,
1094  0.9058, 0.6324,
1095  0.1270, 0.0975).finished();
1096 
1097  Matrix expectedU = (Matrix(3, 2) <<
1098  0.7397, 0.6724,
1099  0.6659, -0.7370,
1100  0.0970, -0.0689).finished();
1101 
1102  Vector expected_s = Vector2(1.6455, 0.1910);
1103 
1104  Matrix expectedV = (Matrix(2, 2) <<
1105  0.7403, -0.6723,
1106  0.6723, 0.7403).finished();
1107 
1108  svd(A, U, s, V);
1109 
1110  // take care of sign ambiguity
1111  if (U(0, 0) < 0) {
1112  U.col(0) = -U.col(0);
1113  V.col(0) = -V.col(0);
1114  }
1115  if (U(0, 1) < 0) {
1116  U.col(1) = -U.col(1);
1117  V.col(1) = -V.col(1);
1118  }
1119 
1120  Matrix reconstructed = U * s.asDiagonal() * trans(V);
1121 
1122  EXPECT(assert_equal(A, reconstructed, 1e-4));
1123  EXPECT(assert_equal(expectedU,U, 1e-3));
1124  EXPECT(assert_equal(expected_s,s, 1e-4));
1125  EXPECT(assert_equal(expectedV,V, 1e-4));
1126 }
1127 
1128 /* ************************************************************************* */
1129 TEST(Matrix, DLT )
1130 {
1131  Matrix A = (Matrix(8, 9) <<
1132  0.21, -0.42, -10.71, 0.18, -0.36, -9.18, -0.61, 1.22, 31.11,
1133  0.44, -0.66, -15.84, 0.34, -0.51, -12.24, -1.64, 2.46, 59.04,
1134  0.69, -8.28, -12.19, -0.48, 5.76, 8.48, -1.89, 22.68, 33.39,
1135  0.96, -8.4, -17.76, -0.6, 5.25, 11.1, -3.36, 29.4, 62.16,
1136  1.25, 0.3, 2.75, -3.5, -0.84, -7.7, 16.25, 3.9, 35.75,
1137  1.56, 0.42, 4.56, -3.38, -0.91, -9.88, 22.36, 6.02, 65.36,
1138  1.89, 2.24, 3.99, 3.24, 3.84, 6.84, 18.09, 21.44, 38.19,
1139  2.24, 2.48, 6.24, 3.08, 3.41, 8.58, 24.64, 27.28, 68.64
1140  ).finished();
1141  const auto [rank,error,actual] = DLT(A);
1142  Vector expected = (Vector(9) << -0.0, 0.2357, 0.4714, -0.2357, 0.0, - 0.4714,-0.4714, 0.4714, 0.0).finished();
1143  EXPECT_LONGS_EQUAL(8,rank);
1144  EXPECT_DOUBLES_EQUAL(0,error,1e-8);
1145  EXPECT(assert_equal(expected, actual, 1e-4));
1146 }
1147 
1148 //******************************************************************************
1149 TEST(Matrix, Matrix24IsVectorSpace) {
1150  GTSAM_CONCEPT_ASSERT(IsVectorSpace<Matrix24>);
1151 }
1152 
1153 TEST(Matrix, RowMajorIsVectorSpace) {
1154 #ifdef GTSAM_USE_BOOST_FEATURES
1155  typedef Eigen::Matrix<double, 2, 3, Eigen::RowMajor> RowMajor;
1156  GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowMajor>);
1157 #endif
1158 }
1159 
1160 TEST(Matrix, MatrixIsVectorSpace) {
1161  GTSAM_CONCEPT_ASSERT(IsVectorSpace<Matrix>);
1162 }
1163 
1164 TEST(Matrix, VectorIsVectorSpace) {
1165  GTSAM_CONCEPT_ASSERT(IsVectorSpace<Vector>);
1166 }
1167 
1168 TEST(Matrix, RowVectorIsVectorSpace) {
1169 #ifdef GTSAM_USE_BOOST_FEATURES
1170  typedef Eigen::Matrix<double, 1, -1> RowVector;
1171  GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowVector>);
1172  GTSAM_CONCEPT_ASSERT(IsVectorSpace<Vector5>);
1173 #endif
1174 }
1175 
1176 //******************************************************************************
1177 TEST(Matrix, AbsoluteError) {
1178  double a = 2000, b = 1997, tol = 1e-1;
1179  bool isEqual;
1180 
1181  // Test only absolute error
1182  isEqual = fpEqual(a, b, tol, false);
1183  EXPECT(!isEqual);
1184 
1185  // Test relative error as well
1186  isEqual = fpEqual(a, b, tol);
1187  EXPECT(isEqual);
1188 }
1189 
1190 // A test to check if a matrix and an optional reference_wrapper to
1191 // a matrix are equal.
1192 TEST(Matrix, MatrixRef) {
1193  Matrix A = Matrix::Random(3, 3);
1194  Matrix B = Matrix::Random(3, 3);
1195 
1196  EXPECT(assert_equal(A, A));
1197  EXPECT(assert_equal(A, std::cref(A)));
1198  EXPECT(!assert_equal(A, std::cref(B)));
1199 }
1200 
1201 /* ************************************************************************* */
1202 int main() {
1203  TestResult tr;
1204  return TestRegistry::runAllTests(tr);
1205 }
1206 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::weighted_eliminate
list< std::tuple< Vector, double, double > > weighted_eliminate(Matrix &A, Vector &b, const Vector &sigmas)
Definition: Matrix.cpp:272
S1
static double S1[]
Definition: shichi.c:61
gtsam::DLT
std::tuple< int, double, Vector > DLT(const Matrix &A, double rank_tol)
Definition: Matrix.cpp:567
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
gtsam::column
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Definition: base/Matrix.h:210
simple_graph::b1
Vector2 b1(2, -1)
gtsam::zeroBelowDiagonal
void zeroBelowDiagonal(MATRIX &A, size_t cols=0)
Definition: base/Matrix.h:231
A3
static const double A3[]
Definition: expn.h:8
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:43
test_constructor::sigmas
Vector1 sigmas
Definition: testHybridNonlinearFactor.cpp:52
TestHarness.h
gtsam::RowVector
Eigen::RowVectorXd RowVector
Definition: LinearCost.h:25
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::inverse_square_root
Matrix inverse_square_root(const Matrix &A)
Definition: Matrix.cpp:551
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::diag
Matrix diag(const std::vector< Matrix > &Hs)
Definition: Matrix.cpp:207
cholesky::M
Matrix M
Definition: testMatrix.cpp:965
simple_graph::b2
Vector2 b2(4, -5)
trans
static char trans
Definition: blas_interface.hh:58
gtsam::householder_
void householder_(Matrix &A, size_t k, bool copy_vectors)
Definition: Matrix.cpp:326
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Eigen::RowMajor
@ RowMajor
Definition: Constants.h:321
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
testLie.h
exp
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
Definition: ArrayCwiseUnaryOps.h:97
exp2
double exp2(double x)
Definition: exp2.c:75
main
int main()
Definition: testMatrix.cpp:1202
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
result
Values result
Definition: OdometryOptimize.cpp:8
svd
cout<< "Here is the matrix m:"<< endl<< m<< endl;JacobiSVD< MatrixXf > svd(m, ComputeThinU|ComputeThinV)
Q
Quaternion Q
Definition: testQuaternion.cpp:27
screwPose2::expectedR
Rot2 expectedR
Definition: testPose2.cpp:149
gtsam::IsVectorSpace
Vector Space concept.
Definition: VectorSpace.h:470
EQUALITY
#define EQUALITY(expected, actual)
Definition: Test.h:127
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:170
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
align_3::a1
Point2 a1
Definition: testPose2.cpp:769
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
householder
void householder(const MatrixType &m)
Definition: householder.cpp:13
align_3::a3
Point2 a3
Definition: testPose2.cpp:771
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
sampleAt
static Matrix sampleAt
Definition: testMatrix.cpp:1033
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
gtsam::stack
Matrix stack(size_t nrMatrices,...)
Definition: Matrix.cpp:396
gtsam::RtR
Matrix RtR(const Matrix &A)
Definition: Matrix.cpp:529
gtsam::cholesky_inverse
Matrix cholesky_inverse(const Matrix &A)
Definition: Matrix.cpp:538
A2
static const double A2[]
Definition: expn.h:7
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::fpEqual
bool fpEqual(double a, double b, double tol, bool check_relative_also)
Definition: Vector.cpp:42
screwNavState::expectedV
Point3 expectedV(0.29552, 0.0446635, 1)
simple_graph::b3
Vector2 b3(3, -6)
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
TestResult
Definition: TestResult.h:26
sampleA
static Matrix sampleA
Sample A matrix for SVD.
Definition: testMatrix.cpp:1032
tol
static const double tol
Definition: testMatrix.cpp:32
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam::inplace_QR
void inplace_QR(Matrix &A)
Definition: Matrix.cpp:637
gtsam::backSubstituteUpper
Vector backSubstituteUpper(const Matrix &U, const Vector &b, bool unit)
Definition: Matrix.cpp:376
cholesky
Definition: testMatrix.cpp:964
qr
HouseholderQR< MatrixXf > qr(A)
ceres::MatrixRef
Eigen::Map< Matrix > MatrixRef
Definition: gtsam/3rdparty/ceres/eigen.h:44
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
gtsam
traits
Definition: SFMdata.h:40
error
static double error
Definition: testRot3.cpp:37
Eigen::DenseBase
Base class for all dense matrices, vectors, and arrays.
Definition: DenseBase.h:41
gtsam::vector_scale
Matrix vector_scale(const Vector &v, const Matrix &A, bool inf_mask)
Definition: Matrix.cpp:497
test_cases::small
std::vector< Vector3 > small
Definition: testPose3.cpp:874
align_3::a2
Point2 a2
Definition: testPose2.cpp:770
row
m row(1)
gtsam::backSubstituteLower
Vector backSubstituteLower(const Matrix &L, const Vector &b, bool unit)
Definition: Matrix.cpp:366
std
Definition: BFloat16.h:88
big
static double big
Definition: igam.c:119
A1
static const double A1[]
Definition: expn.h:6
gtsam::insertSub
void insertSub(Eigen::MatrixBase< Derived1 > &fullMatrix, const Eigen::MatrixBase< Derived2 > &subMatrix, size_t i, size_t j)
Definition: base/Matrix.h:194
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:41
U
@ U
Definition: testDecisionTree.cpp:342
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
Eigen::MatrixBase
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
gtsam::linear_independent
bool linear_independent(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:101
gtsam::collect
Matrix collect(const std::vector< const Matrix * > &matrices, size_t m, size_t n)
Definition: Matrix.cpp:442
inf
static double inf
Definition: testMatrix.cpp:31
align_3::t
Point2 t(10, 10)
prod
EIGEN_DONT_INLINE void prod(const Lhs &a, const Rhs &b, Res &c)
Definition: product_threshold.cpp:39
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
VectorSpace.h
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Definition: Testable.h:85
GTSAM_CONCEPT_ASSERT
#define GTSAM_CONCEPT_ASSERT(concept)
Definition: base/concepts.h:22
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::linear_dependent
bool linear_dependent(const Matrix &A, const Matrix &B, double tol)
Definition: Matrix.cpp:115
S
DiscreteKey S(1, 2)
sub
EIGEN_DONT_INLINE T sub(T a, T b)
Definition: svd_common.h:299
gtsam::LLt
Matrix LLt(const Matrix &A)
Definition: Matrix.cpp:522
TEST
TEST(Matrix, constructor_data)
Definition: testMatrix.cpp:35

gtsam
Author(s):
autogenerated on Sat Dec 28 2024 04:07:38