diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index 699829261..4a7b4c3fe 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -37,47 +38,46 @@ GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) TEST (OrientedPlane3Factor, lm_translation_error) { // Tests one pose, two measurements of the landmark that differ in range only. // Normal along -x, 3m away - gtsam::Symbol lm_sym('p', 0); - gtsam::OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); + Symbol lm_sym('p', 0); + OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); - gtsam::ISAM2 isam2; - gtsam::Values new_values; - gtsam::NonlinearFactorGraph new_graph; + ISAM2 isam2; + Values new_values; + NonlinearFactorGraph new_graph; // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose - gtsam::Symbol init_sym('x', 0); - gtsam::Pose3 init_pose(gtsam::Rot3::ypr(0.0, 0.0, 0.0), - gtsam::Point3(0.0, 0.0, 0.0)); - gtsam::Vector sigmas(6); + Symbol init_sym('x', 0); + Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + Vector sigmas(6); sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; - gtsam::PriorFactor pose_prior(init_sym, init_pose, - gtsam::noiseModel::Diagonal::Sigmas(sigmas)); + PriorFactor pose_prior(init_sym, init_pose, + noiseModel::Diagonal::Sigmas(sigmas)); new_values.insert(init_sym, init_pose); new_graph.add(pose_prior); // Add two landmark measurements, differing in range new_values.insert(lm_sym, test_lm0); - gtsam::Vector sigmas3(3); + Vector sigmas3(3); sigmas3 << 0.1, 0.1, 0.1; - gtsam::Vector test_meas0_mean(4); + Vector test_meas0_mean(4); test_meas0_mean << -1.0, 0.0, 0.0, 3.0; - gtsam::OrientedPlane3Factor test_meas0(test_meas0_mean, - gtsam::noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); + OrientedPlane3Factor test_meas0(test_meas0_mean, + noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); new_graph.add(test_meas0); - gtsam::Vector test_meas1_mean(4); + Vector test_meas1_mean(4); test_meas1_mean << -1.0, 0.0, 0.0, 1.0; - gtsam::OrientedPlane3Factor test_meas1(test_meas1_mean, - gtsam::noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); + OrientedPlane3Factor test_meas1(test_meas1_mean, + noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); new_graph.add(test_meas1); // Optimize - gtsam::ISAM2Result result = isam2.update(new_graph, new_values); - gtsam::Values result_values = isam2.calculateEstimate(); - gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at< - gtsam::OrientedPlane3>(lm_sym); + ISAM2Result result = isam2.update(new_graph, new_values); + Values result_values = isam2.calculateEstimate(); + OrientedPlane3 optimized_plane_landmark = result_values.at( + lm_sym); // Given two noisy measurements of equal weight, expect result between the two - gtsam::OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0); + OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0); EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } @@ -85,19 +85,18 @@ TEST (OrientedPlane3Factor, lm_translation_error) { TEST (OrientedPlane3Factor, lm_rotation_error) { // Tests one pose, two measurements of the landmark that differ in angle only. // Normal along -x, 3m away - gtsam::Symbol lm_sym('p', 0); - gtsam::OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); + Symbol lm_sym('p', 0); + OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); - gtsam::ISAM2 isam2; - gtsam::Values new_values; - gtsam::NonlinearFactorGraph new_graph; + ISAM2 isam2; + Values new_values; + NonlinearFactorGraph new_graph; // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose - gtsam::Symbol init_sym('x', 0); - gtsam::Pose3 init_pose(gtsam::Rot3::ypr(0.0, 0.0, 0.0), - gtsam::Point3(0.0, 0.0, 0.0)); - gtsam::PriorFactor pose_prior(init_sym, init_pose, - gtsam::noiseModel::Diagonal::Sigmas( + Symbol init_sym('x', 0); + Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + PriorFactor pose_prior(init_sym, 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); new_graph.add(pose_prior); @@ -106,26 +105,24 @@ TEST (OrientedPlane3Factor, lm_rotation_error) { new_values.insert(lm_sym, test_lm0); Vector test_meas0_mean(4); test_meas0_mean << -1.0, 0.0, 0.0, 3.0; - gtsam::OrientedPlane3Factor test_meas0(test_meas0_mean, - gtsam::noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, - lm_sym); + 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; - gtsam::OrientedPlane3Factor test_meas1(test_meas1_mean, - gtsam::noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, - lm_sym); + 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); // Optimize - gtsam::ISAM2Result result = isam2.update(new_graph, new_values); - gtsam::Values result_values = isam2.calculateEstimate(); - gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at< - gtsam::OrientedPlane3>(lm_sym); + ISAM2Result result = isam2.update(new_graph, new_values); + Values result_values = isam2.calculateEstimate(); + OrientedPlane3 optimized_plane_landmark = result_values.at( + lm_sym); // Given two noisy measurements of equal weight, expect result between the two - gtsam::OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, - -sqrt(2.0) / 2.0, 0.0, 3.0); + OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, + 0.0, 3.0); EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } @@ -140,8 +137,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { // Factor Key key(1); - SharedGaussian model = gtsam::noiseModel::Diagonal::Sigmas( - Vector3(0.1, 0.1, 10.0)); + SharedGaussian model = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 10.0)); OrientedPlane3DirectionPrior factor(key, planeOrientation, model); // Create a linearization point at the zero-error point