diff --git a/cpp/visualSLAM.cpp b/cpp/visualSLAM.cpp index 093919060..93791b4ba 100644 --- a/cpp/visualSLAM.cpp +++ b/cpp/visualSLAM.cpp @@ -18,17 +18,17 @@ namespace gtsam { namespace visualSLAM { - /* ************************************************************************* */ - void ProjectionFactor::print(const std::string& s) const { - Base::print(s); - z_.print(s + ".z"); - } - - /* ************************************************************************* */ - bool ProjectionFactor::equals(const ProjectionFactor& p, double tol) const { - return Base::equals(p, tol) && z_.equals(p.z_, tol) - && K_->equals(*p.K_, tol); - } +// /* ************************************************************************* */ +// void ProjectionFactor::print(const std::string& s) const { +// Base::print(s); +// z_.print(s + ".z"); +// } +// +// /* ************************************************************************* */ +// bool ProjectionFactor::equals(const ProjectionFactor& p, double tol) const { +// return Base::equals(p, tol) && z_.equals(p.z_, tol) +// && K_->equals(*p.K_, tol); +// } // /* ************************************************************************* */ // bool compareLandmark(const std::string& key, diff --git a/cpp/visualSLAM.h b/cpp/visualSLAM.h index 5c7f616f1..119f9addf 100644 --- a/cpp/visualSLAM.h +++ b/cpp/visualSLAM.h @@ -34,7 +34,8 @@ namespace gtsam { namespace visualSLAM { * Non-linear factor for a constraint derived from a 2D measurement, * i.e. the main building block for visual SLAM. */ - class ProjectionFactor: public NonlinearFactor2, Testable { + template + class GenericProjectionFactor : public NonlinearFactor2, Testable > { private: // Keep a copy of measurement and calibration for I/O @@ -44,15 +45,15 @@ namespace gtsam { namespace visualSLAM { public: // shorthand for base class type - typedef NonlinearFactor2 Base; + typedef NonlinearFactor2 Base; // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /** * Default constructor */ - ProjectionFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} + GenericProjectionFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {} /** * Constructor @@ -62,7 +63,7 @@ namespace gtsam { namespace visualSLAM { * @param landmarkNumber is the index of the landmark * @param K the constant calibration */ - ProjectionFactor(const Point2& z, + GenericProjectionFactor(const Point2& z, const SharedGaussian& model, PoseKey j_pose, PointKey j_landmark, const shared_ptrK& K) : z_(z), K_(K), Base(model, j_pose, j_landmark) { @@ -72,12 +73,18 @@ namespace gtsam { namespace visualSLAM { * print * @param s optional string naming the factor */ - void print(const std::string& s = "ProjectionFactor") const; + void print(const std::string& s = "ProjectionFactor") const { + Base::print(s); + z_.print(s + ".z"); + } /** * equals */ - bool equals(const ProjectionFactor&, double tol = 1e-9) const; + bool equals(const GenericProjectionFactor& p, double tol = 1e-9) const { + return Base::equals(p, tol) && z_.equals(p.z_, tol) + && K_->equals(*p.K_, tol); + } // /** h(x) */ // Point2 predict(const Pose3& pose, const Point3& point) const { @@ -106,6 +113,10 @@ namespace gtsam { namespace visualSLAM { } }; + typedef GenericProjectionFactor ProjectionFactor; + + + // /** // * Non-linear factor graph for visual SLAM