diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 5a871d96f..e1ea38b27 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -29,13 +29,13 @@ 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. */ - template + template class GenericProjectionFactor: public NoiseModelFactor2 { protected: // Keep a copy of measurement and calibration for I/O - Point2 measured_; ///< 2D measurement - boost::shared_ptr K_; ///< shared pointer to calibration object + Point2 measured_; ///< 2D measurement + boost::shared_ptr K_; ///< shared pointer to calibration object public: @@ -43,14 +43,13 @@ namespace gtsam { typedef NoiseModelFactor2 Base; /// shorthand for this class - typedef GenericProjectionFactor This; + typedef GenericProjectionFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; /// Default constructor - GenericProjectionFactor() : - K_(new Cal3_S2(444, 555, 666, 777, 888)) { + GenericProjectionFactor() { } /** @@ -94,7 +93,7 @@ namespace gtsam { Vector evaluateError(const Pose3& pose, const Point3& point, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { try { - SimpleCamera camera(*K_, pose); + PinholeCamera camera(*K_, pose); Point2 reprojectionError(camera.project(point, H1, H2) - measured_); return reprojectionError.vector(); } catch( CheiralityException& e) { @@ -112,7 +111,7 @@ namespace gtsam { } /** return the calibration object */ - inline const Cal3_S2::shared_ptr calibration() const { + inline const boost::shared_ptr calibration() const { return K_; } diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 0cd357c70..f344f98c9 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -18,6 +18,7 @@ #include #include +#include #include using namespace std; @@ -41,8 +42,14 @@ using symbol_shorthand::X; using symbol_shorthand::L; /* ************************************************************************* */ -TEST( ProjectionFactor, error ) +TEST( ProjectionFactor, nonStandard ) { + GenericProjectionFactor f; +} + +/* ************************************************************************* */ +TEST( ProjectionFactor, error ) + { // Create the factor with a measurement that is 3 pixels off in x Point2 z(323.,240.); int i=1, j=1;