testCholesky.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 #include <gtsam/base/debug.h>
19 #include <gtsam/base/cholesky.h>
20 #include <CppUnitLite/TestHarness.h>
21 
22 using namespace gtsam;
23 using namespace std;
24 
25 /* ************************************************************************* */
26 TEST(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(RSL, 0);
38  EXPECT(assert_equal(ABC, RSL, 1e-9));
39 }
40 
41 /* ************************************************************************* */
42 TEST(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(RSL, 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(3, 3, 4, 4).setIdentity();
60 
61  R2.block(3, 3, 4, 4) = R2.block(3, 3, 4, 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 /* ************************************************************************* */
70 TEST(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(R, 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 /* ************************************************************************* */
85 TEST(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 /* ************************************************************************* */
101 TEST(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 /* ************************************************************************* */
141 int main() {
142  TestResult tr;
143  return TestRegistry::runAllTests(tr);
144 }
145 /* ************************************************************************* */
sampling::A1
static Matrix A1
Definition: testGaussianBayesNet.cpp:167
main
int main()
Definition: testCholesky.cpp:141
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
debug.h
Global debugging flags.
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
R
Rot2 R(Rot2::fromAngle(0.1))
L
MatrixXd L
Definition: LLT_example.cpp:6
std
Definition: BFloat16.h:88
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
S
DiscreteKey S(1, 2)
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::svd
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
Definition: Matrix.cpp:558
Eigen::Upper
Definition: Constants.h:211
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
gtsam
traits
Definition: chartTesting.h:28
gtsam::choleskyPartial
bool choleskyPartial(Matrix &ABC, size_t nFrontal, size_t topleft)
Definition: base/cholesky.cpp:107
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
cholesky.h
Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:59