linearize method for Pose2Graph works in Matlab!!

cpp/NonlinearFactorGraph-inl.h

@@ -28,14 +28,11 @@ namespace gtsam {
 	}
 	/* ************************************************************************* */
 	template<class Config>
-	GaussianFactorGraph NonlinearFactorGraph<Config>::linearize(
+	boost::shared_ptr<GaussianFactorGraph> NonlinearFactorGraph<Config>::linearize_(
 			const Config& config) const{
-		// TODO speed up the function either by returning a pointer or by
-		// returning the linearisation as a second argument and returning
-		// the reference
 
 		// create an empty linear FG
-		GaussianFactorGraph linearFG;
+		boost::shared_ptr<GaussianFactorGraph>  linearFG(new GaussianFactorGraph);
 
 		typedef typename FactorGraph<NonlinearFactor<Config> >::const_iterator
 		const_iterator;
@@ -43,12 +40,14 @@ namespace gtsam {
 		for (const_iterator factor = this->factors_.begin(); factor
 		< this->factors_.end(); factor++) {
 			boost::shared_ptr<GaussianFactor> lf = (*factor)->linearize(config);
-			linearFG.push_back(lf);
+			linearFG->push_back(lf);
 		}
-
 		return linearFG;
 	}
-
 	/* ************************************************************************* */
-
+	template<class Config>
+	GaussianFactorGraph NonlinearFactorGraph<Config>::linearize(
+			const Config& config) const {
+		return *linearize_(config);
+	}
 }

cpp/NonlinearFactorGraph.h

@@ -41,6 +41,11 @@ namespace gtsam {
 		 */
 		GaussianFactorGraph linearize(const Config& config) const;
 
+	    /**
+	     * shared pointer versions for MATLAB
+	     */
+	    boost::shared_ptr<GaussianFactorGraph> linearize_(const Config& config) const;
+
 	};
 
 } // namespace

cpp/Ordering.h

@@ -53,6 +53,7 @@ namespace gtsam {
 		 * @return bool
 		 */
 		bool equals(const Ordering &ord, double tol=0) const;
+
 	};
 
 }

cpp/Pose2Graph.cpp

@@ -12,7 +12,7 @@ namespace gtsam {
 
 // explicit instantiation so all the code is there and we can link with it
 template class FactorGraph<NonlinearFactor<gtsam::Pose2Config> > ;
-template class NonlinearFactorGraph<Pose2Factor> ;
+template class NonlinearFactorGraph<Pose2Config> ;
 //template class NonlinearOptimizer<Pose2Graph, Pose2Config> ;
 
 void Pose2Graph::print(const std::string& s) const {

cpp/gtsam.h

@@ -2,6 +2,7 @@ class Ordering {
   Ordering();
   void push_back(string s);
   void print(string s) const;
+  void unique ();
 };
 
 class SymbolicFactor{
@@ -193,6 +194,8 @@ class Pose2Graph{
 	Pose2Graph();
 	void print(string s) const;
 	bool equals(const Pose2Graph& p, double tol) const;
-	GaussianFactorGraph linearize(const Pose2Config& config) const;
+	GaussianFactorGraph* linearize_(const Pose2Config& config) const;
 	void push_back(Pose2Factor* factor);
 };
+
+

cpp/testGaussianFactorGraph.cpp

@@ -31,6 +31,7 @@ double tol=1e-4;
 TEST( GaussianFactorGraph, equals ){
 
   GaussianFactorGraph fg = createGaussianFactorGraph();
+  fg.print("fg");
   GaussianFactorGraph fg2 = createGaussianFactorGraph();
   CHECK(fg.equals(fg2));
 }