diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index ccf68a4a6..ac87bc121 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -746,13 +746,13 @@ virtual class OdometryFactorBase : gtsam::NoiseModelFactor { }; #include -#include +#include template -virtual class TransformProjectionFactor : gtsam::NoiseModelFactor { - TransformProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, +virtual class ProjectionFactorPPP : gtsam::NoiseModelFactor { + ProjectionFactorPPP(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, + ProjectionFactorPPP(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; @@ -763,8 +763,8 @@ virtual class TransformProjectionFactor : gtsam::NoiseModelFactor { // enabling serialization functionality void serialize() const; }; -typedef gtsam::TransformProjectionFactor TransformProjectionFactorCal3_S2; -typedef gtsam::TransformProjectionFactor TransformProjectionFactorCal3DS2; +typedef gtsam::ProjectionFactorPPP ProjectionFactorPPPCal3_S2; +typedef gtsam::ProjectionFactorPPP ProjectionFactorPPPCal3DS2; #include diff --git a/gtsam_unstable/slam/TransformProjectionFactor.h b/gtsam_unstable/slam/ProjectionFactorPPP.h similarity index 89% rename from gtsam_unstable/slam/TransformProjectionFactor.h rename to gtsam_unstable/slam/ProjectionFactorPPP.h index b76968ff4..e7060dcd4 100644 --- a/gtsam_unstable/slam/TransformProjectionFactor.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -10,18 +10,17 @@ * -------------------------------------------------------------------------- */ /** - * @file TransformProjectionFactor.h - * @brief Basic bearing factor from 2D measurement + * @file ProjectionFactorPPP.h + * @brief Derived from ProjectionFactor, but estimates body-camera transform + * in addition to body pose and 3D landmark * @author Chris Beall - * @author Richard Roberts - * @author Frank Dellaert - * @author Alex Cunningham */ #pragma once #include -#include +#include +#include #include namespace gtsam { @@ -32,7 +31,7 @@ namespace gtsam { * @addtogroup SLAM */ template - class TransformProjectionFactor: public NoiseModelFactor3 { + class ProjectionFactorPPP: public NoiseModelFactor3 { protected: // Keep a copy of measurement and calibration for I/O @@ -49,13 +48,13 @@ namespace gtsam { typedef NoiseModelFactor3 Base; /// shorthand for this class - typedef TransformProjectionFactor This; + typedef ProjectionFactorPPP This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; /// Default constructor - TransformProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {} + ProjectionFactorPPP() : throwCheirality_(false), verboseCheirality_(false) {} /** * Constructor @@ -63,10 +62,11 @@ namespace gtsam { * @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 transformKey is the index of the body-camera transform * @param pointKey is the index of the landmark * @param K shared pointer to the constant calibration */ - TransformProjectionFactor(const Point2& measured, const SharedNoiseModel& model, + ProjectionFactorPPP(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), @@ -83,7 +83,7 @@ namespace gtsam { * @param throwCheirality determines whether Cheirality exceptions are rethrown * @param verboseCheirality determines whether exceptions are printed for Cheirality */ - TransformProjectionFactor(const Point2& measured, const SharedNoiseModel& model, + ProjectionFactorPPP(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key transformKey, Key pointKey, const boost::shared_ptr& K, bool throwCheirality, bool verboseCheirality) : @@ -91,7 +91,7 @@ namespace gtsam { throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} /** Virtual destructor */ - virtual ~TransformProjectionFactor() {} + virtual ~ProjectionFactorPPP() {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -104,7 +104,7 @@ namespace gtsam { * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "TransformProjectionFactor, z = "; + std::cout << s << "ProjectionFactorPPP, z = "; measured_.print(); Base::print("", keyFormatter); } diff --git a/gtsam_unstable/slam/tests/testTransformProjectionFactor.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp similarity index 96% rename from gtsam_unstable/slam/tests/testTransformProjectionFactor.cpp rename to gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 2dd44dfb5..3a7bf0cd0 100644 --- a/gtsam_unstable/slam/tests/testTransformProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -11,14 +11,14 @@ /** * @file testProjectionFactor.cpp - * @brief Unit tests for ProjectionFactor Class - * @author Frank Dellaert - * @date Nov 2009 + * @brief Unit tests for ProjectionFactorPPP Class + * @author Chris Beall + * @date July 2014 */ #include #include -#include +#include #include #include #include @@ -46,11 +46,11 @@ using symbol_shorthand::X; using symbol_shorthand::L; using symbol_shorthand::T; -typedef TransformProjectionFactor TestProjectionFactor; +typedef ProjectionFactorPPP TestProjectionFactor; /* ************************************************************************* */ TEST( ProjectionFactor, nonStandard ) { - TransformProjectionFactor f; + ProjectionFactorPPP f; } /* ************************************************************************* */