diff --git a/gtsam.h b/gtsam.h index b7178d534..da26eb73d 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2181,6 +2181,27 @@ typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; +#include +template +virtual class TransformProjectionFactor : gtsam::NoiseModelFactor { + TransformProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t transformKey, size_t pointKey, const CALIBRATION* k); + + TransformProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t transformKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); + + gtsam::Point2 measured() const; + CALIBRATION* calibration() const; + bool verboseCheirality() const; + bool throwCheirality() const; + + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::TransformProjectionFactor TransformProjectionFactorCal3_S2; +typedef gtsam::TransformProjectionFactor TransformProjectionFactorCal3DS2; + + #include template virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { diff --git a/gtsam/slam/TransformProjectionFactor.h b/gtsam/slam/TransformProjectionFactor.h new file mode 100644 index 000000000..b76968ff4 --- /dev/null +++ b/gtsam/slam/TransformProjectionFactor.h @@ -0,0 +1,181 @@ +/* ---------------------------------------------------------------------------- + + * 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 TransformProjectionFactor.h + * @brief Basic bearing factor from 2D measurement + * @author Chris Beall + * @author Richard Roberts + * @author Frank Dellaert + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** + * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. + * i.e. the main building block for visual SLAM. + * @addtogroup SLAM + */ + template + class TransformProjectionFactor: public NoiseModelFactor3 { + protected: + + // Keep a copy of measurement and calibration for I/O + Point2 measured_; ///< 2D measurement + boost::shared_ptr K_; ///< shared pointer to calibration object + + // verbosity handling for Cheirality Exceptions + bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + + public: + + /// shorthand for base class type + typedef NoiseModelFactor3 Base; + + /// shorthand for this class + typedef TransformProjectionFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + TransformProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {} + + /** + * Constructor + * TODO: Mark argument order standard (keys, measurement, parameters) + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation + * @param poseKey is the index of the camera + * @param pointKey is the index of the landmark + * @param K shared pointer to the constant calibration + */ + TransformProjectionFactor(const Point2& measured, const SharedNoiseModel& model, + Key poseKey, Key transformKey, Key pointKey, + const boost::shared_ptr& K) : + Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K), + throwCheirality_(false), verboseCheirality_(false) {} + + /** + * Constructor with exception-handling flags + * TODO: Mark argument order standard (keys, measurement, parameters) + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation + * @param poseKey is the index of the camera + * @param pointKey is the index of the landmark + * @param K shared pointer to the constant calibration + * @param throwCheirality determines whether Cheirality exceptions are rethrown + * @param verboseCheirality determines whether exceptions are printed for Cheirality + */ + TransformProjectionFactor(const Point2& measured, const SharedNoiseModel& model, + Key poseKey, Key transformKey, Key pointKey, + const boost::shared_ptr& K, + bool throwCheirality, bool verboseCheirality) : + Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K), + throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} + + /** Virtual destructor */ + virtual ~TransformProjectionFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "TransformProjectionFactor, z = "; + measured_.print(); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e + && Base::equals(p, tol) + && this->measured_.equals(e->measured_, tol) + && this->K_->equals(*e->K_, tol); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const { + try { + if(H1 || H2 || H3) { + gtsam::Matrix H0, H02; + PinholeCamera camera(pose.compose(transform, H0, H02), *K_); + Point2 reprojectionError(camera.project(point, H1, H3) - measured_); + *H2 = *H1 * H02; + *H1 = *H1 * H0; + return reprojectionError.vector(); + } else { + PinholeCamera camera(pose.compose(transform), *K_); + Point2 reprojectionError(camera.project(point, H1, H3) - measured_); + return reprojectionError.vector(); + } + } catch( CheiralityException& e) { + if (H1) *H1 = zeros(2,6); + if (H2) *H2 = zeros(2,6); + if (H3) *H3 = zeros(2,3); + if (verboseCheirality_) + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << + " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + if (throwCheirality_) + throw e; + } + return ones(2) * 2.0 * K_->fx(); + } + + /** return the measurement */ + const Point2& measured() const { + return measured_; + } + + /** return the calibration object */ + inline const boost::shared_ptr calibration() const { + return K_; + } + + /** return verbosity */ + inline bool verboseCheirality() const { return verboseCheirality_; } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { return throwCheirality_; } + + private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(K_); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } + }; +} // \ namespace gtsam diff --git a/gtsam/slam/tests/testTransformProjectionFactor.cpp b/gtsam/slam/tests/testTransformProjectionFactor.cpp new file mode 100644 index 000000000..d38376cb6 --- /dev/null +++ b/gtsam/slam/tests/testTransformProjectionFactor.cpp @@ -0,0 +1,221 @@ +/* ---------------------------------------------------------------------------- + + * 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 testProjectionFactor.cpp + * @brief Unit tests for ProjectionFactor Class + * @author Frank Dellaert + * @date Nov 2009 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +using namespace std; +using namespace gtsam; + +// make a realistic calibration matrix +static double fov = 60; // degrees +static size_t w=640,h=480; +static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); + +// Create a noise model for the pixel error +static SharedNoiseModel model(noiseModel::Unit::Create(2)); + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; +using symbol_shorthand::T; + +typedef TransformProjectionFactor TestProjectionFactor; + +/* ************************************************************************* */ +TEST( ProjectionFactor, nonStandard ) { + TransformProjectionFactor f; +} + +/* ************************************************************************* */ +TEST( ProjectionFactor, Constructor) { + Key poseKey(X(1)); + Key transformKey(T(1)); + Key pointKey(L(1)); + + Point2 measurement(323.0, 240.0); + + TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +} + +/* ************************************************************************* */ +TEST( ProjectionFactor, ConstructorWithTransform) { + Key poseKey(X(1)); + Key transformKey(T(1)); + Key pointKey(L(1)); + + Point2 measurement(323.0, 240.0); + TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +} + +/* ************************************************************************* */ +TEST( ProjectionFactor, Equals ) { + // Create two identical factors and make sure they're equal + Point2 measurement(323.0, 240.0); + + TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K); + TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K); + + CHECK(assert_equal(factor1, factor2)); +} + +/* ************************************************************************* */ +TEST( ProjectionFactor, EqualsWithTransform ) { + // Create two identical factors and make sure they're equal + Point2 measurement(323.0, 240.0); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + + TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K); + TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K); + + CHECK(assert_equal(factor1, factor2)); +} + +/* ************************************************************************* */ +TEST( ProjectionFactor, Error ) { + // Create the factor with a measurement that is 3 pixels off in x + Key poseKey(X(1)); + Key transformKey(T(1)); + Key pointKey(L(1)); + Point2 measurement(323.0, 240.0); + TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); + + // Set the linearization point + Pose3 pose(Rot3(), Point3(0,0,-6)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose, Pose3(), point)); + + // The expected error is (-3.0, 0.0) pixels / UnitCovariance + Vector expectedError = (Vector(2) << -3.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); +} + +/* ************************************************************************* */ +TEST( ProjectionFactor, ErrorWithTransform ) { + // Create the factor with a measurement that is 3 pixels off in x + Key poseKey(X(1)); + Key transformKey(T(1)); + Key pointKey(L(1)); + Point2 measurement(323.0, 240.0); + Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + TestProjectionFactor factor(measurement, model, poseKey,transformKey, pointKey, K); + + // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) + Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose, transform, point)); + + // The expected error is (-3.0, 0.0) pixels / UnitCovariance + Vector expectedError = (Vector(2) << -3.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); +} + +/* ************************************************************************* */ +TEST( ProjectionFactor, Jacobian ) { + // Create the factor with a measurement that is 3 pixels off in x + Key poseKey(X(1)); + Key transformKey(T(1)); + Key pointKey(L(1)); + Point2 measurement(323.0, 240.0); + TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); + + // Set the linearization point + Pose3 pose(Rot3(), Point3(0,0,-6)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual; + factor.evaluateError(pose, Pose3(), point, H1Actual, H2Actual, H3Actual); + + // The expected Jacobians + Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); + Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.); + + // Verify the Jacobians are correct + CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); + CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); + + // Verify H2 with numerical derivative + Matrix H2Expected = numericalDerivative32( + boost::function( + boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, + boost::none, boost::none, boost::none)), pose, Pose3(), point); + + CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); +} + +/* ************************************************************************* */ +TEST( ProjectionFactor, JacobianWithTransform ) { + // Create the factor with a measurement that is 3 pixels off in x + Key poseKey(X(1)); + Key transformKey(T(1)); + Key pointKey(L(1)); + Point2 measurement(323.0, 240.0); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); + + // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) + Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual; + factor.evaluateError(pose, body_P_sensor, point, H1Actual, H2Actual, H3Actual); + + // The expected Jacobians + Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376); + Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376); + + // Verify the Jacobians are correct + CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); + CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); + + // Verify H2 with numerical derivative + Matrix H2Expected = numericalDerivative32( + boost::function( + boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3, + boost::none, boost::none, boost::none)), pose, body_P_sensor, point); + + CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); + + +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ +