add a ternary sfm projection factor for (pose,point,calibration)

gtsam/slam/GeneralSFMFactor.h

@@ -22,6 +22,7 @@
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
 
@@ -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 CAMERA, class LANDMARK>
-	class GeneralSFMFactor:
-	public NoiseModelFactor2<CAMERA, LANDMARK> {
+	class GeneralSFMFactor:	public NoiseModelFactor2<CAMERA, LANDMARK> {
 	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 CALIBRATION>
+  class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
+  protected:
+    Point2 measured_;     ///< the 2D measurement
+
+  public:
+
+    typedef GeneralSFMFactor2<CALIBRATION> This;
+    typedef PinholeCamera<CALIBRATION> Camera;                  ///< typedef for camera type
+    typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> 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<This> 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>(
+          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<const This*>(&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<Matrix&> H1=boost::none,
+                         boost::optional<Matrix&> H2=boost::none,
+                         boost::optional<Matrix&> 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<class Archive>
+    void serialize(Archive & ar, const unsigned int version) {
+      ar & BOOST_SERIALIZATION_NVP(measured_);
+    }
+  };
+
+
+
 } //namespace