All SLAM Graph classes now derive from a common base class "EasyFactorGraph", to avoid duplicating the common optimize convenience methods.

.cproject

@@ -1947,10 +1947,10 @@
 			</target>
 			<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
+				<buildArguments>-j5</buildArguments>
 				<buildTarget>check</buildTarget>
 				<stopOnError>true</stopOnError>
-				<useDefaultCommand>false</useDefaultCommand>
+				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
 			<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
@@ -2244,46 +2244,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check j5" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="wrap_gtsam_distclean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>wrap_gtsam_distclean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="wrap_gtsam_unstable_distclean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>wrap_gtsam_unstable_distclean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="wrap_gtsam_clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>wrap_gtsam_clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="wrap_gtsam_unstable_clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>wrap_gtsam_unstable_clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>

gtsam.h

@@ -997,6 +997,8 @@ class Values {
 
 class Graph {
 	Graph();
+  Graph(const gtsam::NonlinearFactorGraph& graph);
+  Graph(const pose2SLAM::Graph& graph);
 
 	// FactorGraph
 	void print(string s) const;
@@ -1017,7 +1019,7 @@ class Graph {
 	// pose2SLAM-specific
 	void addPoseConstraint(size_t key, const gtsam::Pose2& pose);
 	void addPosePrior(size_t key, const gtsam::Pose2& pose, const gtsam::noiseModel::Base* noiseModel);
-	void addRelativePose(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::noiseModel::Base* noiseModel);
+	void addRelativePose(size_t key1, size_t key2, const gtsam::Pose2& relativePoseMeasurement, const gtsam::noiseModel::Base* noiseModel);
 	pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate, size_t verbosity) const;
 	pose2SLAM::Values optimizeSPCG(const pose2SLAM::Values& initialEstimate, size_t verbosity) const;
 	gtsam::Marginals marginals(const pose2SLAM::Values& solution) const;
@@ -1048,6 +1050,8 @@ class Values {
 
 class Graph {
 	Graph();
+  Graph(const gtsam::NonlinearFactorGraph& graph);
+  Graph(const pose3SLAM::Graph& graph);
 
 	// FactorGraph
 	void print(string s) const;
@@ -1115,6 +1119,9 @@ class Values {
 
 class Graph {
 	Graph();
+  Graph(const gtsam::NonlinearFactorGraph& graph);
+  Graph(const pose2SLAM::Graph& graph);
+  Graph(const planarSLAM::Graph& graph);
 
 	// FactorGraph
 	void print(string s) const;
@@ -1135,7 +1142,7 @@ class Graph {
 	// pose2SLAM-inherited
 	void addPoseConstraint(size_t key, const gtsam::Pose2& pose);
 	void addPosePrior(size_t key, const gtsam::Pose2& pose, const gtsam::noiseModel::Base* noiseModel);
-	void addRelativePose(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::noiseModel::Base* noiseModel);
+	void addRelativePose(size_t key1, size_t key2, const gtsam::Pose2& relativePoseMeasurement, const gtsam::noiseModel::Base* noiseModel);
 	planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate, size_t verbosity) const;
 	planarSLAM::Values optimizeSPCG(const planarSLAM::Values& initialEstimate, size_t verbosity) const;
 	gtsam::Marginals marginals(const planarSLAM::Values& solution) const;
@@ -1197,6 +1204,9 @@ class Values {
 
 class Graph {
   Graph();
+  Graph(const gtsam::NonlinearFactorGraph& graph);
+  Graph(const pose3SLAM::Graph& graph);
+  Graph(const visualSLAM::Graph& graph);
 
 	// FactorGraph
 	void print(string s) const;
@@ -1258,6 +1268,7 @@ class LevenbergMarquardtOptimizer {
   double lambda() const;
   void iterate();
   visualSLAM::Values optimize();
+  visualSLAM::Values optimizeSafely();
   double error() const;
   size_t iterations() const;
   visualSLAM::Values values() const;

gtsam/nonlinear/EasyFactorGraph.cpp

@@ -0,0 +1,57 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   EasyFactorGraph.cpp
+ *  @brief  Nonlinear Factor Graph class with methods defined for safe and easy access in MATLAB
+ *  @date   June 24, 2012
+ *  @author Frank Dellaert
+ **/
+
+#include <gtsam/nonlinear/EasyFactorGraph.h>
+#include <gtsam/linear/SimpleSPCGSolver.h>
+
+namespace gtsam {
+
+	EasyFactorGraph::EasyFactorGraph() {
+	}
+
+	EasyFactorGraph::EasyFactorGraph(const NonlinearFactorGraph& graph) :
+			NonlinearFactorGraph(graph) {
+	}
+
+	LevenbergMarquardtOptimizer EasyFactorGraph::optimizer(
+			const Values& initialEstimate, const LevenbergMarquardtParams& p) const {
+		return LevenbergMarquardtOptimizer((*this), initialEstimate, p);
+	}
+
+	const Values& EasyFactorGraph::optimize(const Values& initialEstimate,
+			size_t verbosity) const {
+		LevenbergMarquardtParams p;
+		p.verbosity = (NonlinearOptimizerParams::Verbosity) verbosity;
+		return optimizer(initialEstimate, p).optimizeSafely();
+	}
+
+	const Values& EasyFactorGraph::optimizeSPCG(const Values& initialEstimate,
+			size_t verbosity) const {
+		LevenbergMarquardtParams p;
+		p.verbosity = (NonlinearOptimizerParams::Verbosity) verbosity;
+		p.linearSolverType = SuccessiveLinearizationParams::CG;
+		p.iterativeParams = boost::make_shared<SimpleSPCGSolverParameters>();
+		return optimizer(initialEstimate, p).optimizeSafely();
+	}
+
+	Marginals EasyFactorGraph::marginals(const Values& solution) const {
+		return Marginals(*this, solution);
+	}
+
+} // namespace gtsam
+

gtsam/nonlinear/EasyFactorGraph.h

@@ -0,0 +1,69 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file   EasyFactorGraph.h
+ *  @brief  Nonlinear Factor Graph class with methods defined for safe and easy access in MATLAB
+ *  @date   June 24, 2012
+ *  @author Frank Dellaert
+ **/
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+
+namespace gtsam {
+
+	/**
+	 * Nonlinear Factor Graph class with methods defined for safe and easy access in MATLAB
+	 */
+	struct EasyFactorGraph: public NonlinearFactorGraph {
+
+		/// Default constructor for a NonlinearFactorGraph
+		EasyFactorGraph();
+
+		/// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
+		EasyFactorGraph(const NonlinearFactorGraph& graph);
+
+		/**
+		 *  Setup and return a LevenbargMarquardtOptimizer
+		 *  @param initialEstimate initial estimate of all variables in the graph
+		 *  @param p optimizer's parameters
+		 *  @return a LevenbergMarquardtOptimizer object, which you can use to control the optimization process
+		 */
+		LevenbergMarquardtOptimizer optimizer(const Values& initialEstimate,
+				const LevenbergMarquardtParams& p = LevenbergMarquardtParams()) const;
+
+		/**
+		 *  Safely optimize the graph, using Levenberg-Marquardt
+		 *  @param initialEstimate initial estimate of all variables in the graph
+		 *  @param verbosity unsigned specifying verbosity level
+		 */
+		const Values& optimize(const Values& initialEstimate, size_t verbosity = 0) const;
+
+		/**
+		 *  Safely optimize using subgraph preconditioning
+		 *  @param initialEstimate initial estimate of all variables in the graph
+		 *  @param verbosity unsigned specifying verbosity level
+		 */
+		const Values& optimizeSPCG(const Values& initialEstimate, size_t verbosity = 0) const;
+
+		/**
+		 *  Return a Marginals object, so you can query marginals
+		 */
+		Marginals marginals(const Values& solution) const;
+
+	};
+
+} // namespace gtsam
+

gtsam/slam/planarSLAM.h

@@ -23,9 +23,6 @@
 #include <gtsam/slam/RangeFactor.h>
 #include <gtsam/slam/BearingFactor.h>
 #include <gtsam/slam/BearingRangeFactor.h>
-#include <gtsam/nonlinear/NonlinearEquality.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/geometry/Pose2.h>
 
 // Use planarSLAM namespace for specific SLAM instance
@@ -84,10 +81,10 @@ namespace planarSLAM {
    */
   struct Graph: public pose2SLAM::Graph {
 
-    /// Default constructor for a NonlinearFactorGraph
+    /// Default constructor
     Graph(){}
 
-    /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
+    /// Copy constructor given any other NonlinearFactorGraph
     Graph(const NonlinearFactorGraph& graph):
   		pose2SLAM::Graph(graph) {}
 

gtsam/slam/pose2SLAM.cpp

@@ -15,7 +15,6 @@
  *  @author Frank Dellaert
  **/
 
-#include <gtsam/linear/SimpleSPCGSolver.h>
 #include <gtsam/slam/pose2SLAM.h>
 
 // Use pose2SLAM namespace for specific SLAM instance
@@ -63,22 +62,5 @@ namespace pose2SLAM {
   }
 
   /* ************************************************************************* */
-  Values Graph::optimize(const Values& initialEstimate, size_t verbosity) const {
-    LevenbergMarquardtParams params;
-    params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity;
-    LevenbergMarquardtOptimizer optimizer(*this, initialEstimate,params);
-    return optimizer.optimize();
-  }
-
-  /* ************************************************************************* */
-  Values Graph::optimizeSPCG(const Values& initialEstimate, size_t verbosity) const {
-    LevenbergMarquardtParams params;
-    params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity;
-    params.linearSolverType = SuccessiveLinearizationParams::CG;
-    params.iterativeParams = boost::make_shared<SimpleSPCGSolverParameters>();
-    return LevenbergMarquardtOptimizer(*this, initialEstimate, params).optimize();
-  }
-
-  /* ************************************************************************* */
 
 } // pose2SLAM

gtsam/slam/pose2SLAM.h

@@ -19,11 +19,8 @@
 
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/EasyFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/geometry/Pose2.h>
 
 // Use pose2SLAM namespace for specific SLAM instance
@@ -77,16 +74,16 @@ namespace pose2SLAM {
    * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
    * @addtogroup SLAM
    */
-  struct Graph: public NonlinearFactorGraph {
+  struct Graph: public EasyFactorGraph {
 
 	  typedef boost::shared_ptr<Graph> shared_ptr;
 
-	  /// Default constructor for a NonlinearFactorGraph
+	  /// Default constructor
     Graph(){}
 
-    /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph
+    /// Copy constructor given any other NonlinearFactorGraph
     Graph(const NonlinearFactorGraph& graph):
-    	NonlinearFactorGraph(graph) {}
+    	EasyFactorGraph(graph) {}
 
     /// Creates a hard constraint for key i with the given Pose2 p.
     void addPoseConstraint(Key i, const Pose2& p);
@@ -96,18 +93,6 @@ namespace pose2SLAM {
 
     /// Creates an relative pose factor between poses with keys i1 and i2
     void addRelativePose(Key i1, Key i2, const Pose2& z, const SharedNoiseModel& model);
-
-    /// Optimize
-    Values optimize(const Values& initialEstimate, size_t verbosity=NonlinearOptimizerParams::SILENT) const;
-
-    /// Optimize using subgraph preconditioning
-    Values optimizeSPCG(const Values& initialEstimate, size_t verbosity=NonlinearOptimizerParams::SILENT) const;
-
-    /// Return a Marginals object
-    Marginals marginals(const Values& solution) const {
-    	return Marginals(*this,solution);
-    }
-
   };
 
 } // pose2SLAM

gtsam/slam/pose3SLAM.cpp

@@ -69,13 +69,5 @@ namespace pose3SLAM {
   }
 
   /* ************************************************************************* */
-  Values Graph::optimize(const Values& initialEstimate, size_t verbosity) const {
-    LevenbergMarquardtParams params;
-    params.verbosity = (NonlinearOptimizerParams::Verbosity)verbosity;
-    LevenbergMarquardtOptimizer optimizer(*this, initialEstimate,params);
-    return optimizer.optimize();
-  }
-
-  /* ************************************************************************* */
 
 } // pose3SLAM

gtsam/slam/pose3SLAM.h

@@ -19,10 +19,8 @@
 
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/EasyFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/geometry/Pose3.h>
 
 /// Use pose3SLAM namespace for specific SLAM instance
@@ -63,9 +61,6 @@ namespace pose3SLAM {
     /// get a pose
     Pose3 pose(Key i) const { return at<Pose3>(i); }
 
-    /// check if value with specified key exists
-    bool exists(Key i) const { return gtsam::Values::exists(i); }
-
     Matrix translations() const; ///< get all pose translations coordinates in a matrix
   };
 
@@ -73,7 +68,14 @@ namespace pose3SLAM {
    * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
    * @addtogroup SLAM
    */
-  struct Graph: public NonlinearFactorGraph {
+  struct Graph: public EasyFactorGraph {
+
+	  /// Default constructor
+    Graph(){}
+
+    /// Copy constructor given any other NonlinearFactorGraph
+    Graph(const NonlinearFactorGraph& graph):
+    	EasyFactorGraph(graph) {}
 
     /**
      *  Add a prior on a pose
@@ -98,32 +100,6 @@ namespace pose3SLAM {
      *  @param model uncertainty model of this measurement
      */
     void addRelativePose(Key x1, Key x2, const Pose3& z, const SharedNoiseModel& model);
-
-    /**
-     *  Optimize the graph
-     *  @param initialEstimate initial estimate of all variables in the graph
-     *  @param pointKey variable key of the landmark
-     *  @param range approximate range to landmark
-     *  @param model uncertainty model of this prior
-     */
-    Values optimize(const Values& initialEstimate, size_t verbosity=NonlinearOptimizerParams::SILENT) const;
-
-    /**
-     *  Setup and return a LevenbargMarquardtOptimizer
-     *  @param initialEstimate initial estimate of all variables in the graph
-     *  @param parameters optimizer's parameters
-     *  @return a LevenbergMarquardtOptimizer object, which you can use to control the optimization process
-     */
-    LevenbergMarquardtOptimizer optimizer(const Values& initialEstimate,
-				const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) const {
-			return LevenbergMarquardtOptimizer((*this), initialEstimate, parameters);
-		}
-
-    /// Return a Marginals object
-    Marginals marginals(const Values& solution) const {
-      return Marginals(*this,solution);
-    }
-
   };
 
 } // pose3SLAM

gtsam/slam/visualSLAM.h

@@ -24,7 +24,6 @@
 #include <gtsam/slam/StereoFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/nonlinear/NonlinearISAM.h>
-#include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/geometry/SimpleCamera.h>
 
 namespace visualSLAM {
@@ -76,20 +75,17 @@ namespace visualSLAM {
   /**
    * Non-linear factor graph for vanilla visual SLAM
    */
-  class Graph: public pose3SLAM::Graph {
+  struct Graph: public pose3SLAM::Graph {
 
-  public:
   	/// shared pointer to this type of graph
     typedef boost::shared_ptr<Graph> shared_graph;
 
-    /// default constructor is empty graph
+	  /// Default constructor
-    Graph() {
+    Graph(){}
-    }
 
-    /// print out graph
+    /// Copy constructor given any other NonlinearFactorGraph
-    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    Graph(const NonlinearFactorGraph& graph):
-      NonlinearFactorGraph::print(s, keyFormatter);
+    	pose3SLAM::Graph(graph) {}
-    }
 
     /// check if two graphs are equal
     bool equals(const Graph& p, double tol = 1e-9) const {