diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index cedcc80fd..ccf68a4a6 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -767,13 +767,13 @@ typedef gtsam::TransformProjectionFactor TransformProjectionFactorCal3DS2; -#include +#include template -virtual class TransformCalProjectionFactor : gtsam::NoiseModelFactor { - TransformCalProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, +virtual class ProjectionFactorPPPC : gtsam::NoiseModelFactor { + ProjectionFactorPPPC(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, size_t poseKey, size_t transformKey, size_t pointKey, size_t calibKey); - TransformCalProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + ProjectionFactorPPPC(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, size_t poseKey, size_t transformKey, size_t pointKey, size_t calibKey, bool throwCheirality, bool verboseCheirality); gtsam::Point2 measured() const; @@ -783,7 +783,7 @@ virtual class TransformCalProjectionFactor : gtsam::NoiseModelFactor { // enabling serialization functionality void serialize() const; }; -typedef gtsam::TransformCalProjectionFactor TransformCalProjectionFactorCal3_S2; -typedef gtsam::TransformCalProjectionFactor TransformCalProjectionFactorCal3DS2; +typedef gtsam::ProjectionFactorPPPC ProjectionFactorPPPCCal3_S2; +typedef gtsam::ProjectionFactorPPPC ProjectionFactorPPPCCal3DS2; } //\namespace gtsam diff --git a/gtsam_unstable/slam/TransformCalProjectionFactor.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h similarity index 86% rename from gtsam_unstable/slam/TransformCalProjectionFactor.h rename to gtsam_unstable/slam/ProjectionFactorPPPC.h index ac796655e..d3bfbbb7d 100644 --- a/gtsam_unstable/slam/TransformCalProjectionFactor.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -10,32 +10,30 @@ * -------------------------------------------------------------------------- */ /** - * @file TransformCalProjectionFactor.h - * @brief Basic bearing factor from 2D measurement + * @file ProjectionFactorPPPC.h + * @brief Derived from ProjectionFactor, but estimates body-camera transform + * and calibration 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 { /** - * 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. + * Non-linear factor for a constraint derived from a 2D measurement. This factor + * estimates the body pose, body-camera transform, 3D landmark, and calibration. * @addtogroup SLAM */ template - class TransformCalProjectionFactor: public NoiseModelFactor4 { + class ProjectionFactorPPPC: public NoiseModelFactor4 { protected: - // Keep a copy of measurement and calibration for I/O Point2 measured_; ///< 2D measurement // verbosity handling for Cheirality Exceptions @@ -48,13 +46,13 @@ namespace gtsam { typedef NoiseModelFactor4 Base; /// shorthand for this class - typedef TransformCalProjectionFactor This; + typedef ProjectionFactorPPPC This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; /// Default constructor - TransformCalProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {} + ProjectionFactorPPPC() : throwCheirality_(false), verboseCheirality_(false) {} /** * Constructor @@ -65,7 +63,7 @@ namespace gtsam { * @param pointKey is the index of the landmark * @param K shared pointer to the constant calibration */ - TransformCalProjectionFactor(const Point2& measured, const SharedNoiseModel& model, + ProjectionFactorPPPC(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key transformKey, Key pointKey, Key calibKey) : Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured), throwCheirality_(false), verboseCheirality_(false) {} @@ -81,14 +79,14 @@ namespace gtsam { * @param throwCheirality determines whether Cheirality exceptions are rethrown * @param verboseCheirality determines whether exceptions are printed for Cheirality */ - TransformCalProjectionFactor(const Point2& measured, const SharedNoiseModel& model, + ProjectionFactorPPPC(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key transformKey, Key pointKey, Key calibKey, bool throwCheirality, bool verboseCheirality) : Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} /** Virtual destructor */ - virtual ~TransformCalProjectionFactor() {} + virtual ~ProjectionFactorPPPC() {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -101,7 +99,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 << "TransformCalProjectionFactor, z = "; + std::cout << s << "ProjectionFactorPPPC, z = "; measured_.print(); Base::print("", keyFormatter); } diff --git a/gtsam_unstable/slam/tests/testTransformCalProjectionFactor.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp similarity index 96% rename from gtsam_unstable/slam/tests/testTransformCalProjectionFactor.cpp rename to gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp index 6e46e3e84..5b572fb69 100644 --- a/gtsam_unstable/slam/tests/testTransformCalProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testTransformCalProjectionFactor.cpp + * @file testProjectionFactorPPPC.cpp * @brief Unit tests for Pose+Transform+Calibration ProjectionFactor Class * @author Chris Beall * @date Jul 29, 2014 @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include @@ -47,11 +47,11 @@ using symbol_shorthand::L; using symbol_shorthand::T; using symbol_shorthand::K; -typedef TransformCalProjectionFactor TestProjectionFactor; +typedef ProjectionFactorPPPC TestProjectionFactor; /* ************************************************************************* */ TEST( ProjectionFactor, nonStandard ) { - TransformCalProjectionFactor f; + ProjectionFactorPPPC f; } /* ************************************************************************* */