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 /* ************************************************************************* */
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:974
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:142
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
R
Rot2 R(Rot2::fromAngle(0.1))
L
MatrixXd L
Definition: LLT_example.cpp:6
std
Definition: Half.h:150
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
gtsam::symbol_shorthand::S
Key S(std::uint64_t j)
Definition: inference/Symbol.h:161
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
R1
Q R1(Eigen::AngleAxisd(1, Q_z_axis))
Eigen::Upper
Definition: Constants.h:206
Eigen::QuaternionBase::setIdentity
EIGEN_DEVICE_FUNC QuaternionBase & setIdentity()
Definition: 3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h:115
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:135
gtsam::choleskyPartial
bool choleskyPartial(Matrix &ABC, size_t nFrontal, size_t topleft)
Definition: base/cholesky.cpp:108
gtsam
traits
Definition: chartTesting.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
gtsam::svd
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
Definition: Matrix.cpp:559
gtsam::TEST
TEST(LPInitSolver, InfiniteLoopSingleVar)
Definition: testLPSolver.cpp:64
R2
Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)))
cholesky.h
Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:46:21