diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 9d04a2c96..d94c162af 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -31,8 +32,7 @@ namespace gtsam { * Non-linear factor for a constraint derived from a 2D measurement. The calibration is unknown here compared to GenericProjectionFactor */ template - class GeneralSFMFactor: - public NoiseModelFactor2 { + class GeneralSFMFactor: public NoiseModelFactor2 { protected: Point2 measured_; ///< the 2D measurement @@ -115,4 +115,92 @@ namespace gtsam { } }; + /** + * Non-linear factor for a constraint derived from a 2D measurement. + * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. + */ + template + class GeneralSFMFactor2: public NoiseModelFactor3 { + protected: + Point2 measured_; ///< the 2D measurement + + public: + + typedef GeneralSFMFactor2 This; + typedef PinholeCamera Camera; ///< typedef for camera type + typedef NoiseModelFactor3 Base; ///< typedef for the base class + typedef Point2 Measurement; ///< typedef for the measurement + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * Constructor + * @param z is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation of the measurements + * @param i is basically the frame number + * @param j is the index of the landmark + */ + GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) : + Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {} + + virtual ~GeneralSFMFactor2() {} ///< destructor + + /// @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 + */ + void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); + } + + /** + * equals + */ + 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) ; + } + + /** h(x)-z */ + Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib, + boost::optional H1=boost::none, + boost::optional H2=boost::none, + boost::optional H3=boost::none) const + { + try { + Camera camera(pose3,calib); + Point2 reprojError(camera.project(point, H1, H2, H3) - measured_); + return reprojError.vector(); + } + catch( CheiralityException& e) { + if (H1) *H1 = zeros(2, pose3.dim()); + if (H2) *H2 = zeros(2, point.dim()); + if (H3) *H3 = zeros(2, calib.dim()); + } + return zero(2); + } + + /** return the measured */ + inline const Point2 measured() const { + return measured_; + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(measured_); + } + }; + + + } //namespace