diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index b0592709f..0a68dcbad 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -36,89 +36,86 @@ using namespace std; GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) +using symbol_shorthand::P; //< Planes +using symbol_shorthand::X; //< Pose3 + // ************************************************************************* -TEST (OrientedPlane3Factor, lm_translation_error) { +TEST(OrientedPlane3Factor, lm_translation_error) { // Tests one pose, two measurements of the landmark that differ in range only. // Normal along -x, 3m away - Symbol lm_sym('p', 0); OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); - ISAM2 isam2; - Values new_values; - NonlinearFactorGraph new_graph; + NonlinearFactorGraph graph; - // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose - Symbol init_sym('x', 0); + // Init pose and prior. Pose Prior is needed since a single plane measurement + // does not fully constrain the pose Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); - Vector sigmas(6); + Vector6 sigmas; sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; - new_graph.addPrior( - init_sym, init_pose, noiseModel::Diagonal::Sigmas(sigmas)); - new_values.insert(init_sym, init_pose); + graph.addPrior(X(0), init_pose, noiseModel::Diagonal::Sigmas(sigmas)); // Add two landmark measurements, differing in range - new_values.insert(lm_sym, test_lm0); - Vector sigmas3(3); - sigmas3 << 0.1, 0.1, 0.1; - Vector test_meas0_mean(4); - test_meas0_mean << -1.0, 0.0, 0.0, 3.0; - OrientedPlane3Factor test_meas0(test_meas0_mean, - noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); - new_graph.add(test_meas0); - Vector test_meas1_mean(4); - test_meas1_mean << -1.0, 0.0, 0.0, 1.0; - OrientedPlane3Factor test_meas1(test_meas1_mean, - noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); - new_graph.add(test_meas1); + Vector3 sigmas3 {0.1, 0.1, 0.1}; + Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; + OrientedPlane3Factor factor0( + measurement0, noiseModel::Diagonal::Sigmas(sigmas3), X(0), P(0)); + graph.add(factor0); + Vector4 measurement1 {-1.0, 0.0, 0.0, 1.0}; + OrientedPlane3Factor factor1( + measurement1, noiseModel::Diagonal::Sigmas(sigmas3), X(0), P(0)); + graph.add(factor1); + + // Initial Estimate + Values values; + values.insert(X(0), init_pose); + values.insert(P(0), test_lm0); // Optimize - ISAM2Result result = isam2.update(new_graph, new_values); + ISAM2 isam2; + ISAM2Result result = isam2.update(graph, values); Values result_values = isam2.calculateEstimate(); - OrientedPlane3 optimized_plane_landmark = result_values.at( - lm_sym); + OrientedPlane3 optimized_plane_landmark = + result_values.at(P(0)); // Given two noisy measurements of equal weight, expect result between the two OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0); EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } -// ************************************************************************* +// // ************************************************************************* TEST (OrientedPlane3Factor, lm_rotation_error) { // Tests one pose, two measurements of the landmark that differ in angle only. // Normal along -x, 3m away - Symbol lm_sym('p', 0); OrientedPlane3 test_lm0(-1.0/sqrt(1.01), 0.1/sqrt(1.01), 0.0, 3.0); - ISAM2 isam2; - Values new_values; - NonlinearFactorGraph new_graph; + NonlinearFactorGraph graph; // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose - Symbol init_sym('x', 0); Pose3 init_pose(Rot3::Ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); - new_graph.addPrior(init_sym, init_pose, + graph.addPrior(X(0), init_pose, noiseModel::Diagonal::Sigmas( (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); - new_values.insert(init_sym, init_pose); -// // Add two landmark measurements, differing in angle - new_values.insert(lm_sym, test_lm0); - Vector test_meas0_mean(4); - test_meas0_mean << -1.0, 0.0, 0.0, 3.0; - OrientedPlane3Factor test_meas0(test_meas0_mean, - noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym); - new_graph.add(test_meas0); - Vector test_meas1_mean(4); - test_meas1_mean << 0.0, -1.0, 0.0, 3.0; - OrientedPlane3Factor test_meas1(test_meas1_mean, - noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym); - new_graph.add(test_meas1); + // Add two landmark measurements, differing in angle + Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; + OrientedPlane3Factor factor0(measurement0, + noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), X(0), P(0)); + graph.add(factor0); + Vector4 measurement1 {0.0, -1.0, 0.0, 3.0}; + OrientedPlane3Factor factor1(measurement1, + noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), X(0), P(0)); + graph.add(factor1); + + // Initial Estimate + Values values; + values.insert(X(0), init_pose); + values.insert(P(0), test_lm0); // Optimize - ISAM2Result result = isam2.update(new_graph, new_values); + ISAM2 isam2; + ISAM2Result result = isam2.update(graph, values); Values result_values = isam2.calculateEstimate(); - OrientedPlane3 optimized_plane_landmark = result_values.at( - lm_sym); + auto optimized_plane_landmark = result_values.at(P(0)); // Given two noisy measurements of equal weight, expect result between the two OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, @@ -126,6 +123,7 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } +// ************************************************************************* TEST( OrientedPlane3Factor, Derivatives ) { // Measurement OrientedPlane3 p(sqrt(2)/2, -sqrt(2)/2, 0, 5); @@ -163,7 +161,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { // If pitch and roll are zero for an aerospace frame, // that means Z is pointing down, i.e., direction of Z = (0,0,-1) - Vector planeOrientation = (Vector(4) << 0.0, 0.0, -1.0, 10.0).finished(); // all vertical planes directly facing the origin + Vector4 planeOrientation = (Vector(4) << 0.0, 0.0, -1.0, 10.0).finished(); // all vertical planes directly facing the origin // Factor Key key(1); @@ -171,9 +169,9 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { OrientedPlane3DirectionPrior factor(key, planeOrientation, model); // Create a linearization point at the zero-error point - Vector theta1 = Vector4(0.0, 0.02, -1.2, 10.0); - Vector theta2 = Vector4(0.0, 0.1, -0.8, 10.0); - Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0); + Vector4 theta1 {0.0, 0.02, -1.2, 10.0}; + Vector4 theta2 {0.0, 0.1, -0.8, 10.0}; + Vector4 theta3 {0.0, 0.2, -0.9, 10.0}; OrientedPlane3 T1(theta1); OrientedPlane3 T2(theta2); @@ -204,90 +202,18 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { EXPECT(assert_equal(expectedH3, actualH3, 1e-8)); } -/* ************************************************************************* */ -// Test by Marco Camurri to debug issue #561 -TEST(OrientedPlane3Factor, Issue561) { - // 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(0.799903913, -0.564527097, 0.203624376, 0.552537226, 0.82520195, - 0.117236322, -0.234214312, 0.0187322547, 0.972004505), - Vector3{-91.7500013, -0.47569366, -2.2}); - auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); - graph.addPrior(X(0), x0_prior, x0_noise); - -// Plane p1_prior(0.211098835, 0.214292752, 0.95368543, 26.4269514); -// auto p1_noise = -// noiseModel::Diagonal::Sigmas(Vector3{0.785398163, 0.785398163, 5}); -// graph.addPrior(P(1), p1_prior, p1_noise); - -// ADDING THIS PRIOR MAKES OPTIMIZATION FAIL -// Plane p2_prior(0.301901811, 0.151751467, 0.941183717, 33.4388229); -// auto p2_noise = -// noiseModel::Diagonal::Sigmas(Vector3{0.785398163, 0.785398163, 5}); -// graph.addPrior(P(2), p2_prior, p2_noise); - - // First plane factor - Plane p1_meas = Plane(0.0638967294, 0.0755284627, 0.995094297, 2.55956073); - const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.0451801); - graph.emplace_shared(p1_meas.planeCoefficients(), - x0_p1_noise, X(0), P(1)); - - // Second plane factor - Plane p2_meas = Plane(0.104902077, -0.0275756528, 0.994100165, 1.32765088); - const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.0322889); - graph.emplace_shared(p2_meas.planeCoefficients(), - x0_p2_noise, X(0), P(2)); - - // Optimize - try { - // Initial values - Values initialEstimate; - Plane p1(0.211098835, 0.214292752, 0.95368543, 26.4269514); - Plane p2(0.301901811, 0.151751467, 0.941183717, 33.4388229); - Pose3 x0( - Rot3(0.799903913, -0.564527097, 0.203624376, 0.552537226, 0.82520195, - 0.117236322, -0.234214312, 0.0187322547, 0.972004505), - Vector3{-91.7500013, -0.47569366, -2.2}); - initialEstimate.insert(P(1), p1); - initialEstimate.insert(P(2), p2); - initialEstimate.insert(X(0), x0); - - 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); - } catch (const IndeterminantLinearSystemException &e) { - std::cerr << "CAPTURED THE EXCEPTION: " << DefaultKeyFormatter(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)); + Pose3 x0(Rot3::identity(), Vector3(0, 0, 10)); auto x0_noise = noiseModel::Isotropic::Sigma(6, 0.01); - graph.addPrior(X(0), x0_prior, x0_noise); + graph.addPrior(X(0), x0, x0_noise); // Two horizontal planes with different heights. const Plane p1(0,0,1,1), p2(0,0,1,2); @@ -300,22 +226,29 @@ TEST(OrientedPlane3Factor, Issue561Simplified) { graph.addPrior(P(2), p2, p2_noise); // First plane factor + auto p1_measured_from_x0 = p1.transform(x0); const auto x0_p1_noise = noiseModel::Isotropic::Sigma(3, 0.05); - graph.emplace_shared(p1.planeCoefficients(), - x0_p1_noise, X(0), P(1)); + graph.emplace_shared( + p1_measured_from_x0.planeCoefficients(), x0_p1_noise, X(0), P(1)); // Second plane factor + auto p2_measured_from_x0 = p2.transform(x0); const auto x0_p2_noise = noiseModel::Isotropic::Sigma(3, 0.05); - graph.emplace_shared(p2.planeCoefficients(), - x0_p2_noise, X(0), P(2)); + graph.emplace_shared( + p2_measured_from_x0.planeCoefficients(), x0_p2_noise, X(0), P(2)); + + GTSAM_PRINT(graph); // 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))); + Pose3 x0_initial = x0.compose(Pose3(Rot3::identity(), Vector3(1,0,0))); initialEstimate.insert(P(1), p1); initialEstimate.insert(P(2), p2); - initialEstimate.insert(X(0), x0); + initialEstimate.insert(X(0), x0_initial); + + // Print Jacobian + cout << graph.linearize(initialEstimate)->augmentedJacobian() << endl << endl; // For testing only HessianFactor::shared_ptr hessianFactor = graph.linearizeToHessianFactor(initialEstimate); @@ -364,7 +297,7 @@ TEST(OrientedPlane3Factor, Issue561Simplified) { 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(x0.equals(result.at(X(0)))); EXPECT(p1.equals(result.at(P(1)))); EXPECT(p2.equals(result.at(P(2)))); } catch (const IndeterminantLinearSystemException &e) {