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
12/**
13 * @file testCholesky.cpp
14 * @author Richard Roberts
15 * @date Nov 5, 2010
16 */
17
18#include <gtsam/base/debug.h>
19#include <gtsam/base/cholesky.h>
20#include <CppUnitLite/TestHarness.h>
21
22using namespace gtsam;
23using namespace std;
24
25/* ************************************************************************* */
26TEST(cholesky, choleskyPartial0) {
27
28 // choleskyPartial should only use the upper triangle, so this represents a
29 // symmetric matrix.
30 Matrix ABC(3,3);
31 ABC << 4.0375, 3.4584, 3.5735,
32 0., 4.7267, 3.8423,
33 0., 0., 5.1600;
34
35 // Test passing 0 frontals to partialCholesky
36 Matrix RSL(ABC);
37 choleskyPartial(ABC&: RSL, nFrontal: 0);
38 EXPECT(assert_equal(ABC, RSL, 1e-9));
39}
40
41/* ************************************************************************* */
42TEST(cholesky, choleskyPartial) {
43 Matrix ABC = (Matrix(7,7) <<
44 4.0375, 3.4584, 3.5735, 2.4815, 2.1471, 2.7400, 2.2063,
45 0., 4.7267, 3.8423, 2.3624, 2.8091, 2.9579, 2.5914,
46 0., 0., 5.1600, 2.0797, 3.4690, 3.2419, 2.9992,
47 0., 0., 0., 1.8786, 1.0535, 1.4250, 1.3347,
48 0., 0., 0., 0., 3.0788, 2.6283, 2.3791,
49 0., 0., 0., 0., 0., 2.9227, 2.4056,
50 0., 0., 0., 0., 0., 0., 2.5776).finished();
51
52 // Do partial Cholesky on 3 frontal scalar variables
53 Matrix RSL(ABC);
54 choleskyPartial(ABC&: RSL, nFrontal: 3);
55
56 // See the function comment for choleskyPartial, this decomposition should hold.
57 Matrix R1 = RSL.transpose();
58 Matrix R2 = RSL;
59 R1.block(startRow: 3, startCol: 3, blockRows: 4, blockCols: 4).setIdentity();
60
61 R2.block(startRow: 3, startCol: 3, blockRows: 4, blockCols: 4) = R2.block(startRow: 3, startCol: 3, blockRows: 4, blockCols: 4).selfadjointView<Eigen::Upper>();
62
63 Matrix actual = R1 * R2;
64
65 Matrix expected = ABC.selfadjointView<Eigen::Upper>();
66 EXPECT(assert_equal(expected, actual, 1e-9));
67}
68
69/* ************************************************************************* */
70TEST(cholesky, BadScalingCholesky) {
71 Matrix A = (Matrix(2,2) <<
72 1e-40, 0.0,
73 0.0, 1.0).finished();
74
75 Matrix R(A.transpose() * A);
76 choleskyPartial(ABC&: R, nFrontal: 2);
77
78 double expectedSqrtCondition = 1e-40;
79 double actualSqrtCondition = R(0,0) / R(1,1);
80
81 DOUBLES_EQUAL(expectedSqrtCondition, actualSqrtCondition, 1e-41);
82}
83
84/* ************************************************************************* */
85TEST(cholesky, BadScalingSVD) {
86 Matrix A = (Matrix(2,2) <<
87 1.0, 0.0,
88 0.0, 1e-40).finished();
89
90 Matrix U, V;
91 Vector S;
92 gtsam::svd(A, U, S, V);
93
94 double expectedCondition = 1e40;
95 double actualCondition = S(0) / S(1);
96
97 DOUBLES_EQUAL(expectedCondition, actualCondition, 1e30);
98}
99
100/* ************************************************************************* */
101TEST(cholesky, underconstrained) {
102 Matrix L(6,6); L <<
103 1, 0, 0, 0, 0, 0,
104 1.11177808157954, 1.06204809504665, 0.507342638873381, 1.34953401829486, 1, 0,
105 0.155864888199928, 1.10933048588373, 0.501255576961674, 1, 0, 0,
106 1.12108665967793, 1.01584408366945, 1, 0, 0, 0,
107 0.776164062474843, 0.117617236580373, -0.0236628691347294, 0.814118199972143, 0.694309975328922, 1,
108 0.1197220685104, 1, 0, 0, 0, 0;
109 Matrix D1(6,6); D1 <<
110 0.814723686393179, 0, 0, 0, 0, 0,
111 0, 0.811780089277421, 0, 0, 0, 0,
112 0, 0, 1.82596950680844, 0, 0, 0,
113 0, 0, 0, 0.240287537694585, 0, 0,
114 0, 0, 0, 0, 1.34342584865901, 0,
115 0, 0, 0, 0, 0, 1e-12;
116 Matrix D2(6,6); D2 <<
117 0.814723686393179, 0, 0, 0, 0, 0,
118 0, 0.811780089277421, 0, 0, 0, 0,
119 0, 0, 1.82596950680844, 0, 0, 0,
120 0, 0, 0, 0.240287537694585, 0, 0,
121 0, 0, 0, 0, 0, 0,
122 0, 0, 0, 0, 0, 0;
123 Matrix D3(6,6); D3 <<
124 0.814723686393179, 0, 0, 0, 0, 0,
125 0, 0.811780089277421, 0, 0, 0, 0,
126 0, 0, 1.82596950680844, 0, 0, 0,
127 0, 0, 0, 0.240287537694585, 0, 0,
128 0, 0, 0, 0, -0.5, 0,
129 0, 0, 0, 0, 0, -0.6;
130
131 Matrix A1 = L * D1 * L.transpose();
132 Matrix A2 = L * D2 * L.transpose();
133 Matrix A3 = L * D3 * L.transpose();
134
135 LONGS_EQUAL(long(false), long(choleskyPartial(A1, 6)));
136 LONGS_EQUAL(long(false), long(choleskyPartial(A2, 6)));
137 LONGS_EQUAL(long(false), long(choleskyPartial(A3, 6)));
138}
139
140/* ************************************************************************* */
141int main() {
142 TestResult tr;
143 return TestRegistry::runAllTests(result&: tr);
144}
145/* ************************************************************************* */
146