move GeneralSFMFactor from vSLAM to library. Move GenericProjectionFactor to namespace gtsam so that it can be used in other slam namespaces.

gtsam/slam/GeneralSFMFactor.h

@@ -0,0 +1,83 @@
+/*
+ * GeneralSFMFactor.h
+ *
+ *   Created on: Dec 15, 2010
+ *       Author: nikai
+ *  Description: a general SFM factor with an unknown calibration
+ */
+
+#pragma once
+
+
+namespace gtsam {
+
+	/**
+	 * Non-linear factor for a constraint derived from a 2D measurement. The calibration is unknown here compared to GenericProjectionFactor
+	 */
+	template <class Cfg, class CamK, class LmK>
+	class GeneralSFMFactor:
+	public NonlinearFactor2<Cfg, CamK, LmK> ,
+		   Testable<GeneralSFMFactor<Cfg, CamK, LmK> > {
+	protected:
+		Point2 z_;
+
+	public:
+
+		typedef typename CamK::Value Cam;
+		typedef GeneralSFMFactor<Cfg, CamK, LmK> Self ;
+		typedef NonlinearFactor2<Cfg, CamK, LmK> Base;
+
+		// shorthand for a smart pointer to a factor
+		typedef boost::shared_ptr<GeneralSFMFactor<Cfg, LmK, CamK> > shared_ptr;
+
+		/**
+		 * Constructor
+		 * @param z is the 2 dimensional location of point in image (the measurement)
+		 * @param sigma is the standard deviation
+		 * @param cameraFrameNumber is basically the frame number
+		 * @param landmarkNumber is the index of the landmark
+		 * @param K the constant calibration
+		 */
+
+		GeneralSFMFactor():z_(0.0,0.0) {}
+		GeneralSFMFactor(const Point2 & p):z_(p) {}
+		GeneralSFMFactor(double x, double y):z_(x,y) {}
+		GeneralSFMFactor(const Point2& z, const SharedGaussian& model, const CamK& i, const LmK& j) : Base(model, i, j), z_(z) {}
+
+		/**
+		 * print
+		 * @param s optional string naming the factor
+		 */
+		void print(const std::string& s = "SFMFactor") const {
+			Base::print(s);
+			z_.print(s + ".z");
+		}
+
+		/**
+		 * equals
+		 */
+		bool equals(const GeneralSFMFactor<Cfg, CamK, LmK> &p, double tol = 1e-9) const	{
+			return Base::equals(p, tol) && this->z_.equals(p.z_, tol) ;
+		}
+
+		/** h(x)-z */
+		Vector evaluateError(
+				const Cam& camera,
+				const Point3& point,
+				boost::optional<Matrix&> H1=boost::none,
+				boost::optional<Matrix&> H2=boost::none) const {
+
+			Point2 q(camera.project(point,H1,H2) - z_);
+			return q.vector() ;
+		}
+
+	private:
+		/** Serialization function */
+		friend class boost::serialization::access;
+		template<class Archive>
+		void serialize(Archive & ar, const unsigned int version) {
+			ar & BOOST_SERIALIZATION_NVP(z_);
+		}
+	};
+
+} //namespace

gtsam/slam/Makefile.am

@@ -46,6 +46,7 @@ sources += pose3SLAM.cpp
 check_PROGRAMS += tests/testPose3Factor tests/testPose3Values tests/testPose3SLAM 
 
 # Visual SLAM
+headers += GeneralSFMFactor.h
 sources += visualSLAM.cpp
 check_PROGRAMS += tests/testVSLAMFactor tests/testVSLAMGraph tests/testVSLAMValues
 

gtsam/slam/visualSLAM.h

@@ -28,31 +28,13 @@
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/slam/PriorFactor.h>
 
-namespace gtsam { namespace visualSLAM {
+namespace gtsam {
 
 	/**
-   * Typedefs that make up the visualSLAM namespace.
+	 * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
-   */
-  typedef TypedSymbol<Pose3,'x'> PoseKey;
-  typedef TypedSymbol<Point3,'l'> PointKey;
-  typedef LieValues<PoseKey> PoseValues;
-  typedef LieValues<PointKey> PointValues;
-  typedef TupleValues2<PoseValues, PointValues> Values;
-  typedef boost::shared_ptr<Values> shared_values;
-
-  typedef NonlinearEquality<Values, PoseKey> PoseConstraint;
-  typedef NonlinearEquality<Values, PointKey> PointConstraint;
-  typedef PriorFactor<Values, PoseKey> PosePrior;
-  typedef PriorFactor<Values, PointKey> PointPrior;
-
-
-
-
-  /**
-   * Non-linear factor for a constraint derived from a 2D measurement,
 	 * i.e. the main building block for visual SLAM.
 	 */
-  template <class CFG=Values, class LMK=PointKey, class POSK=PoseKey>
+	template <class CFG, class LMK, class POSK>
 	class GenericProjectionFactor : public NonlinearFactor2<CFG, POSK, LMK>, Testable<GenericProjectionFactor<CFG, LMK, POSK> > {
 	protected:
 
@@ -124,6 +106,24 @@ namespace gtsam { namespace visualSLAM {
 		}
 	};
 
+
+	namespace visualSLAM {
+
+  /**
+   * Typedefs that make up the visualSLAM namespace.
+   */
+  typedef TypedSymbol<Pose3,'x'> PoseKey;
+  typedef TypedSymbol<Point3,'l'> PointKey;
+  typedef LieValues<PoseKey> PoseValues;
+  typedef LieValues<PointKey> PointValues;
+  typedef TupleValues2<PoseValues, PointValues> Values;
+  typedef boost::shared_ptr<Values> shared_values;
+
+  typedef NonlinearEquality<Values, PoseKey> PoseConstraint;
+  typedef NonlinearEquality<Values, PointKey> PointConstraint;
+  typedef PriorFactor<Values, PoseKey> PosePrior;
+  typedef PriorFactor<Values, PointKey> PointPrior;
+  
   // Typedef for general use
   typedef GenericProjectionFactor<Values, PointKey, PoseKey> ProjectionFactor;