UrbanFactor, UrbanGraph.cpp, template instantiations

cpp/Makefile.am

@@ -189,8 +189,7 @@ testPose3Factor_SOURCES = $(example) testPose3Factor.cpp
 testPose3Factor_LDADD   = libgtsam.la
 
 # Urban
-headers += UrbanGraph.h
+sources += UrbanFactor.cpp UrbanConfig.cpp UrbanMeasurement.cpp UrbanGraph.cpp
-sources += UrbanConfig.cpp UrbanMeasurement.cpp
 check_PROGRAMS += testUrbanConfig testUrbanMeasurement testUrbanGraph
 testUrbanConfig_SOURCES       = $(example) testUrbanConfig.cpp
 testUrbanConfig_LDADD         = libgtsam.la

cpp/UrbanConfig.h

@@ -5,14 +5,11 @@
  * @author Viorela Ila
  */
 
-#include <string>
 #include <map>
-#include <vector>
-#include <fstream>
-#include "VectorConfig.h"
-#include "Pose3.h"
-#include "Point2.h"
 #include "Testable.h"
+#include "VectorConfig.h"
+#include "Point2.h"
+#include "Pose3.h"
 
 #pragma once
 

cpp/UrbanFactor.cpp

@@ -0,0 +1,21 @@
+/*
+ * UrbanFactor.cpp
+ *
+ *  Created on: Dec 17, 2009
+ *      Author: Frank Dellaert
+ */
+
+#include "UrbanFactor.h"
+
+namespace gtsam {
+
+	UrbanFactor::UrbanFactor() {
+		// TODO Auto-generated constructor stub
+
+	}
+
+	UrbanFactor::~UrbanFactor() {
+		// TODO Auto-generated destructor stub
+	}
+
+}

cpp/UrbanFactor.h

@@ -0,0 +1,34 @@
+/*
+ * UrbanFactor.h
+ *
+ *  Created on: Dec 17, 2009
+ *      Author: Frank Dellaert
+ */
+
+#ifndef URBANFACTOR_H_
+#define URBANFACTOR_H_
+
+#include "NonlinearFactor.h"
+#include "UrbanConfig.h"
+
+namespace gtsam {
+
+	class UrbanFactor : public NonlinearFactor<UrbanConfig> {
+	public:
+		UrbanFactor();
+		virtual ~UrbanFactor();
+
+    /** Vector of errors */
+		Vector error_vector(const UrbanConfig& c) const { return zero(0); }
+
+    /** linearize to a GaussianFactor */
+    boost::shared_ptr<GaussianFactor> linearize(const UrbanConfig& c) const {
+    	boost::shared_ptr<GaussianFactor> factor(new GaussianFactor);
+    	return factor;
+    }
+
+	};
+
+}
+
+#endif /* URBANFACTOR_H_ */

cpp/UrbanGraph.cpp

@@ -0,0 +1,54 @@
+/**
+ * @file   UrbanGraph.cpp
+ * @brief  A factor graph for the Urban problem
+ * @author Frank Dellaert
+ * @author Viorela Ila
+ */
+
+#include "FactorGraph-inl.h"
+#include "NonlinearOptimizer-inl.h"
+#include "UrbanGraph.h"
+
+namespace gtsam {
+
+	// explicit instantiation so all the code is there and we can link with it
+	template class FactorGraph<UrbanFactor>;
+	template class NonlinearFactorGraph<UrbanConfig>;
+	template class NonlinearOptimizer<UrbanGraph,UrbanConfig>;
+
+	/* ************************************************************************* */
+	UrbanGraph::UrbanGraph() {
+	}
+
+	/* ************************************************************************* */
+	void UrbanGraph::print(const std::string& s) const {
+		gtsam::NonlinearFactorGraph<UrbanConfig>::print(s);
+		// TODO
+	}
+
+	/* ************************************************************************* */
+	bool UrbanGraph::equals(const UrbanGraph& p, double tol) const {
+		return gtsam::NonlinearFactorGraph<UrbanConfig>::equals(p, tol);
+		// TODO
+	}
+
+	/* ************************************************************************* */
+	void UrbanGraph::addMeasurement(double x, double y, double sigma, int p1,
+			int p2) {
+		// TODO
+	}
+	;
+
+	/* ************************************************************************* */
+	void UrbanGraph::addOdometry(double dx, double yaw, double sigmadx,
+			double sigmayaw, int p) {
+		// TODO
+	}
+
+	/* ************************************************************************* */
+	void UrbanGraph::addOriginConstraint(int p) {
+		// TODO
+	}
+
+/* ************************************************************************* */
+} // namespace gtsam

cpp/UrbanGraph.h

@@ -4,66 +4,56 @@
  * @author Frank Dellaert
  * @author Viorela Ila
  */
+
 #pragma once
 
-#include <vector>
-#include <map>
-#include <set>
-#include <fstream>
-
 #include "NonlinearFactorGraph.h"
-#include "FactorGraph-inl.h"
+#include "UrbanFactor.h"
-#include "UrbanMeasurement.h"
-#include "Pose3Factor.h"
-#include "UrbanConfig.h"
-#include "Testable.h"
 
-namespace gtsam{
+namespace gtsam {
 
-/**
+	/**
- * Non-linear factor graph for visual SLAM
+	 * Non-linear factor graph for visual SLAM
- */
+	 */
-class UrbanGraph : public gtsam::NonlinearFactorGraph<UrbanConfig>{
+	class UrbanGraph: public gtsam::NonlinearFactorGraph<UrbanConfig> {
 
-public:
+	public:
 
-  /** default constructor is empty graph */
+		/** default constructor is empty graph */
-  UrbanGraph() {}
+		UrbanGraph();
 
-  /**
+		/**
-   * print out graph
+		 * print out graph
-   */
+		 */
-  void print(const std::string& s = "") const {
+		void print(const std::string& s = "") const;
-    gtsam::NonlinearFactorGraph<UrbanConfig>::print(s);
-  }
 
-  /**
+		/**
-   * equals
+		 * equals
-   */
+		 */
-  bool equals(const UrbanGraph& p, double tol=1e-9) const {
+		bool equals(const UrbanGraph& p, double tol = 1e-9) const;
-	  return gtsam::NonlinearFactorGraph<UrbanConfig>::equals(p, tol);
-  }
-  // TODO implement addMeasurenment, addOdometry and addOriginalConstraint (out of the class)
-  /**
-   *  Add a landmark constraint
-   */
-  void addMeasurement(double x, double y, double sigma, int p1, int p2) {};
 
-  /**
+		/**
-   *  Add an odometry constraint
+		 *  Add a landmark constraint
-   */
+		 */
-  void addOdometry(double dx, double yaw, double sigmadx, double sigmayaw, int p  ) {};
+		void addMeasurement(double x, double y, double sigma, int p1, int p2);
 
-  /**
+		/**
-   *  Add an initial constraint on the first pose
+		 *  Add an odometry constraint
-   */
+		 */
-  void addOriginConstraint(int p){};
+		void addOdometry(double dx, double yaw, double sigmadx, double sigmayaw,
+				int p);
 
-private:
+		/**
-	/** Serialization function */
+		 *  Add an initial constraint on the first pose
-	friend class boost::serialization::access;
+		 */
-	template<class Archive>
+		void addOriginConstraint(int p);
-	void serialize(Archive & ar, const unsigned int version) {}
+
-};
+	private:
+		/** Serialization function */
+		friend class boost::serialization::access;
+		template<class Archive>
+		void serialize(Archive & ar, const unsigned int version) {
+		}
+	};
 
 } // namespace gtsam

cpp/VSLAMGraph.cpp

@@ -23,9 +23,6 @@ template class FactorGraph<VSLAMFactor>;
 template class NonlinearFactorGraph<VSLAMConfig>;
 template class NonlinearOptimizer<VSLAMGraph,VSLAMConfig>;
 
-/* ************************************************************************* */
-
-
 /* ************************************************************************* */
 bool compareLandmark(const std::string& key,
 					const VSLAMConfig& feasible,

cpp/testUrbanGraph.cpp

@@ -13,9 +13,8 @@ using namespace boost::assign;
 #include "UrbanGraph.h"
 #include "NonlinearFactorGraph-inl.h"
 #include "NonlinearOptimizer-inl.h"
-#include "Pose3.h"
-#include "Point2.h"
 #include "Ordering.h"
+
 using namespace std;
 using namespace gtsam;
 typedef NonlinearOptimizer<UrbanGraph,UrbanConfig> Optimizer;