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

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