diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 0bd553a40..f986cca1d 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -262,25 +262,6 @@ TEST( triangulation, twoIdenticalPoses) { } */ -//****************************************************************************** -TEST( triangulation, TriangulationFactor ) { - // Create the factor with a measurement that is 3 pixels off in x - Key pointKey(1); - SharedNoiseModel model; - typedef TriangulationFactor<> Factor; - Factor factor(camera1, z1, model, pointKey); - - // Use the factor to calculate the Jacobians - Matrix HActual; - factor.evaluateError(landmark, HActual); - - Matrix HExpected = numericalDerivative11( - boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); - - // Verify the Jacobians are correct - CHECK(assert_equal(HExpected, HActual, 1e-3)); -} - //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 798a7e9dc..ce83f780b 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -19,12 +19,10 @@ #pragma once -#include +#include +#include #include #include -#include - -#include namespace gtsam { diff --git a/gtsam/geometry/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h similarity index 98% rename from gtsam/geometry/TriangulationFactor.h rename to gtsam/slam/TriangulationFactor.h index fc8d546d3..b7f6a20dc 100644 --- a/gtsam/geometry/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -10,14 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * testTriangulationFactor.h + * triangulationFactor.h * @date March 2, 2014 * @author Frank Dellaert */ #include #include -#include #include #include diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp new file mode 100644 index 000000000..6b79171df --- /dev/null +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -0,0 +1,70 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * testTriangulationFactor.cpp + * + * Created on: July 30th, 2013 + * Author: cbeall3 + */ + +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +// Some common constants +static const boost::shared_ptr sharedCal = // + boost::make_shared(1500, 1200, 0, 640, 480); + +// Looking along X-axis, 1 meter above ground plane (x-y) +static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2); +static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); +PinholeCamera camera1(pose1, *sharedCal); + +// landmark ~5 meters infront of camera +static const Point3 landmark(5, 0.5, 1.2); + +// 1. Project two landmarks into two cameras and triangulate +Point2 z1 = camera1.project(landmark); + +//****************************************************************************** +TEST( triangulation, TriangulationFactor ) { + // Create the factor with a measurement that is 3 pixels off in x + Key pointKey(1); + SharedNoiseModel model; + typedef TriangulationFactor<> Factor; + Factor factor(camera1, z1, model, pointKey); + + // Use the factor to calculate the Jacobians + Matrix HActual; + factor.evaluateError(landmark, HActual); + + Matrix HExpected = numericalDerivative11( + boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); + + // Verify the Jacobians are correct + CHECK(assert_equal(HExpected, HActual, 1e-3)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//******************************************************************************