From 50768371dc7bb017befec88e38d21eb9d732be17 Mon Sep 17 00:00:00 2001 From: David Wisth Date: Tue, 19 Jan 2021 17:41:12 +0000 Subject: [PATCH 1/2] Add a simplifed version of the minimal failing example --- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 66 ++++++++++++++++++- 1 file changed, 65 insertions(+), 1 deletion(-) diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index fd240f4ba6..49c1b9c07b 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -238,8 +238,72 @@ TEST(OrientedPlane3Factor, Issue561) { GaussNewtonOptimizer optimizer(graph, initialEstimate, params); Values result = optimizer.optimize(); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); - } catch (IndeterminantLinearSystemException e) { + } catch (const IndeterminantLinearSystemException &e) { std::cerr << "CAPTURED THE EXCEPTION: " << e.nearbyVariable() << std::endl; + EXPECT(false); // fail if this happens + } +} + +/* ************************************************************************* */ +// Simplified version of the test by Marco Camurri to debug issue #561 +TEST(OrientedPlane3Factor, Issue561Simplified) { + // Typedefs + using symbol_shorthand::P; //< Planes + using symbol_shorthand::X; //< Pose3 (x,y,z,r,p,y) + using Plane = OrientedPlane3; + + NonlinearFactorGraph graph; + + // Setup prior factors + Pose3 x0_prior(Rot3::identity(), Vector3(99, 0, 0)); + auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); + graph.addPrior(X(0), x0_prior, x0_noise); + + // Two horizontal planes with different heights. + const Plane p1(0,0,1,1), p2(0,0,1,2); + + auto p1_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); + graph.addPrior(P(1), p1, p1_noise); + + // ADDING THIS PRIOR MAKES OPTIMIZATION FAIL + auto p2_noise = noiseModel::Diagonal::Sigmas(Vector3{1, 1, 5}); + graph.addPrior(P(2), p2, p2_noise); + + // First plane factor + const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05); + graph.emplace_shared(p1.planeCoefficients(), + x0_p1_noise, X(0), P(1)); + + // Second plane factor + const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05); + graph.emplace_shared(p2.planeCoefficients(), + x0_p2_noise, X(0), P(2)); + + // Initial values + // Just offset the initial pose by 1m. This is what we are trying to optimize. + Values initialEstimate; + Pose3 x0 = x0_prior.compose(Pose3(Rot3::identity(), Vector3(1,0,0))); + initialEstimate.insert(P(1), p1); + initialEstimate.insert(P(2), p2); + initialEstimate.insert(X(0), x0); + + // Optimize + try { + GaussNewtonParams params; + //GTSAM_PRINT(graph); + //Ordering ordering = list_of(P(1))(P(2))(X(0)); // make sure P1 eliminated first + //params.setOrdering(ordering); + // params.setLinearSolverType("SEQUENTIAL_QR"); // abundance of caution + params.setVerbosity("TERMINATION"); // show info about stopping conditions + GaussNewtonOptimizer optimizer(graph, initialEstimate, params); + Values result = optimizer.optimize(); + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); + EXPECT(x0_prior.equals(result.at(X(0)))); + EXPECT(p1.equals(result.at(P(1)))); + EXPECT(p2.equals(result.at(P(2)))); + } catch (const IndeterminantLinearSystemException &e) { + std::cerr << "CAPTURED THE EXCEPTION: " << e.nearbyVariable() << std::endl; + EXPECT(false); // fail if this happens } } From 62119d8076b323580a7cb92a100d5c1c03d0dc24 Mon Sep 17 00:00:00 2001 From: David Wisth Date: Tue, 19 Jan 2021 20:51:31 +0000 Subject: [PATCH 2/2] Add hessian calculation --- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 40 ++++++++++++++++++- 1 file changed, 38 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 49c1b9c07b..9f94ab3acb 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -239,7 +239,7 @@ TEST(OrientedPlane3Factor, Issue561) { Values result = optimizer.optimize(); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); } catch (const IndeterminantLinearSystemException &e) { - std::cerr << "CAPTURED THE EXCEPTION: " << e.nearbyVariable() << std::endl; + std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(e.nearbyVariable()) << std::endl; EXPECT(false); // fail if this happens } } @@ -287,6 +287,42 @@ TEST(OrientedPlane3Factor, Issue561Simplified) { initialEstimate.insert(P(2), p2); initialEstimate.insert(X(0), x0); + // For testing only + HessianFactor::shared_ptr hessianFactor = graph.linearizeToHessianFactor(initialEstimate); + const auto hessian = hessianFactor->hessianBlockDiagonal(); + + Matrix hessianP1 = hessian.at(P(1)), + hessianP2 = hessian.at(P(2)), + hessianX0 = hessian.at(X(0)); + + Eigen::JacobiSVD svdP1(hessianP1, Eigen::ComputeThinU), + svdP2(hessianP2, Eigen::ComputeThinU), + svdX0(hessianX0, Eigen::ComputeThinU); + + double conditionNumberP1 = svdP1.singularValues()[0] / svdP1.singularValues()[2], + conditionNumberP2 = svdP2.singularValues()[0] / svdP2.singularValues()[2], + conditionNumberX0 = svdX0.singularValues()[0] / svdX0.singularValues()[5]; + + std::cout << "Hessian P1:\n" << hessianP1 << "\n" + << "Condition number:\n" << conditionNumberP1 << "\n" + << "Singular values:\n" << svdP1.singularValues().transpose() << "\n" + << "SVD U:\n" << svdP1.matrixU() << "\n" << std::endl; + + std::cout << "Hessian P2:\n" << hessianP2 << "\n" + << "Condition number:\n" << conditionNumberP2 << "\n" + << "Singular values:\n" << svdP2.singularValues().transpose() << "\n" + << "SVD U:\n" << svdP2.matrixU() << "\n" << std::endl; + + std::cout << "Hessian X0:\n" << hessianX0 << "\n" + << "Condition number:\n" << conditionNumberX0 << "\n" + << "Singular values:\n" << svdX0.singularValues().transpose() << "\n" + << "SVD U:\n" << svdX0.matrixU() << "\n" << std::endl; + + // std::cout << "Hessian P2:\n" << hessianP2 << std::endl; + // std::cout << "Hessian X0:\n" << hessianX0 << std::endl; + + // For testing only + // Optimize try { GaussNewtonParams params; @@ -302,7 +338,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) { EXPECT(p1.equals(result.at(P(1)))); EXPECT(p2.equals(result.at(P(2)))); } catch (const IndeterminantLinearSystemException &e) { - std::cerr << "CAPTURED THE EXCEPTION: " << e.nearbyVariable() << std::endl; + std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(e.nearbyVariable()) << std::endl; EXPECT(false); // fail if this happens } }