diff --git a/gtsam/base/tests/testCholesky.cpp b/gtsam/base/tests/testCholesky.cpp index 68b2bf660..5232fc6ba 100644 --- a/gtsam/base/tests/testCholesky.cpp +++ b/gtsam/base/tests/testCholesky.cpp @@ -160,6 +160,55 @@ TEST(cholesky, ldlPartial2) { EXPECT(assert_equal(IexpectedR, p.transpose()*I)); } +/* ************************************************************************* */ +TEST(cholesky, BadScalingCholesky) { + Matrix A = Matrix_(2,2, + 1e-40, 0.0, + 0.0, 1.0); + + Matrix R(A.transpose() * A); + choleskyPartial(R, 2); + + double expectedSqrtCondition = 1e-40; + double actualSqrtCondition = R(0,0) / R(1,1); + + DOUBLES_EQUAL(expectedSqrtCondition, actualSqrtCondition, 1e-41); +} + +/* ************************************************************************* */ +TEST(cholesky, BadScalingLDL) { + Matrix A = Matrix_(2,2, + 1.0, 0.0, + 0.0, 1e-40); + + Matrix R(A.transpose() * A); + Eigen::LDLT::TranspositionType permutation = ldlPartial(R, 2); + + EXPECT(permutation.indices()(0) == 0); + EXPECT(permutation.indices()(1) == 1); + + double expectedCondition = 1e40; + double actualCondition = R(0,0) / R(1,1); + + DOUBLES_EQUAL(expectedCondition, actualCondition, 1e-41); +} + +/* ************************************************************************* */ +TEST(cholesky, BadScalingSVD) { + Matrix A = Matrix_(2,2, + 1.0, 0.0, + 0.0, 1e-40); + + Matrix U, V; + Vector S; + gtsam::svd(A, U, S, V); + + double expectedCondition = 1e40; + double actualCondition = S(0) / S(1); + + DOUBLES_EQUAL(expectedCondition, actualCondition, 1e-41); +} + /* ************************************************************************* */ int main() { TestResult tr;