From 8d39d910e9d8a158be9ffe195030252930fb36d7 Mon Sep 17 00:00:00 2001 From: Alex Trevor Date: Wed, 29 Jan 2014 20:48:20 -0500 Subject: [PATCH] Added a couple unit tests for the plane factor. --- gtsam/geometry/OrientedPlane3.cpp | 2 +- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 124 ++++++++++++++++++ 2 files changed, 125 insertions(+), 1 deletion(-) create mode 100644 gtsam/slam/tests/testOrientedPlane3Factor.cpp diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 24ffae3ee..cfc9a183b 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -72,7 +72,7 @@ void OrientedPlane3::print(const std::string& s) const { { Vector n_error = -n_.localCoordinates (plane.n_, Sphere2::EXPMAP); double d_error = d_ - plane.d_; - return (Vector (3) << n_error (0), n_error (1), -d_error); + return (Vector (3) << n_error (0), n_error (1), d_error); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp new file mode 100644 index 000000000..0b8f0331a --- /dev/null +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testOrientedPlane3.cpp + * @date Dec 19, 2012 + * @author Alex Trevor + * @brief Tests the OrientedPlane3Factor class + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace boost::assign; +using namespace gtsam; +using namespace std; + +GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) +GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) + +TEST (OrientedPlane3, 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); + + gtsam::ISAM2 isam2; + gtsam::Values new_values; + gtsam::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 (gtsam::Vector_ (6, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001))); + 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::OrientedPlane3Factor test_meas0 (gtsam::Vector_ (4, -1.0, 0.0, 0.0, 3.0), gtsam::noiseModel::Diagonal::Sigmas (gtsam::Vector_ (3, 0.1, 0.1, 0.1)), init_sym, lm_sym); + new_graph.add (test_meas0); + gtsam::OrientedPlane3Factor test_meas1 (gtsam::Vector_ (4, -1.0, 0.0, 0.0, 1.0), gtsam::noiseModel::Diagonal::Sigmas (gtsam::Vector_ (3, 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(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); + EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark)); +} + +TEST (OrientedPlane3, 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); + + gtsam::ISAM2 isam2; + gtsam::Values new_values; + gtsam::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 (gtsam::Vector_ (6, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001))); + new_values.insert (init_sym, init_pose); + new_graph.add (pose_prior); + + // Add two landmark measurements, differing in angle + new_values.insert (lm_sym, test_lm0); + gtsam::OrientedPlane3Factor test_meas0 (gtsam::Vector_ (4, -1.0, 0.0, 0.0, 3.0), gtsam::noiseModel::Diagonal::Sigmas (gtsam::Vector_ (3, 0.1, 0.1, 0.1)), init_sym, lm_sym); + new_graph.add (test_meas0); + gtsam::OrientedPlane3Factor test_meas1 (gtsam::Vector_ (4, 0.0, -1.0, 0.0, 3.0), gtsam::noiseModel::Diagonal::Sigmas (gtsam::Vector_ (3, 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(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); + EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark)); +} + +/* ************************************************************************* */ +int main() { + srand(time(NULL)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */