NonlinearOptimizer constructor now requires shared_ptr to prevent storing pointers to stack and temporary variables. Code that uses it will need to be modified, but CitySLAM is already done.

cpp/NonlinearOptimizer-inl.h

@@ -44,9 +44,9 @@ namespace gtsam {
 	// Constructors
 	/* ************************************************************************* */
 	template<class G, class C>
-	NonlinearOptimizer<G, C>::NonlinearOptimizer(const G& graph,
+	NonlinearOptimizer<G, C>::NonlinearOptimizer(shared_graph graph,
-			const Ordering& ordering, shared_config config, double lambda) :
+			shared_ordering ordering, shared_config config, double lambda) :
-		graph_(&graph), ordering_(&ordering), config_(config), error_(graph.error(
+		graph_(graph), ordering_(ordering), config_(config), error_(graph->error(
 				*config)), lambda_(lambda) {
 	}
 
@@ -88,7 +88,7 @@ namespace gtsam {
 		if (verbosity >= CONFIG)
 			newConfig->print("newConfig");
 
-		return NonlinearOptimizer(*graph_, *ordering_, newConfig);
+		return NonlinearOptimizer(graph_, ordering_, newConfig);
 	}
 
 	/* ************************************************************************* */
@@ -139,14 +139,14 @@ namespace gtsam {
 			newConfig->print("config");
 
 		// create new optimization state with more adventurous lambda
-		NonlinearOptimizer next(*graph_, *ordering_, newConfig, lambda_ / factor);
+		NonlinearOptimizer next(graph_, ordering_, newConfig, lambda_ / factor);
 
 		// if error decreased, return the new state
 		if (next.error_ <= error_)
 			return next;
 		else {
 			// TODO: can we avoid copying the config ?
-			NonlinearOptimizer cautious(*graph_, *ordering_, config_, lambda_ * factor);
+			NonlinearOptimizer cautious(graph_, ordering_, config_, lambda_ * factor);
 			return cautious.try_lambda(linear, verbosity, factor);
 		}
 	}

cpp/NonlinearOptimizer.h

@@ -31,6 +31,8 @@ namespace gtsam {
 
 		// For performance reasons in recursion, we store configs in a shared_ptr
 		typedef boost::shared_ptr<const Config> shared_config;
+		typedef boost::shared_ptr<const FactorGraph> shared_graph;
+		typedef boost::shared_ptr<const Ordering> shared_ordering;
 
 		enum verbosityLevel {
 			SILENT,
@@ -49,17 +51,17 @@ namespace gtsam {
 
 		// keep a reference to const versions of the graph and ordering
 		// These normally do not change
-		const FactorGraph* graph_;
+		const shared_graph graph_;
-		const Ordering* ordering_;
+		const shared_ordering ordering_;
 
 		// keep a configuration and its error
 		// These typically change once per iteration (in a functional way)
-		shared_config config_;
+		const shared_config config_;
-		double error_;
+		const double error_;
 
 		// keep current lambda for use within LM only
 		// TODO: red flag, should we have an LM class ?
-		double lambda_;
+		const double lambda_;
 
 		// Recursively try to do tempered Gauss-Newton steps until we succeed
 		NonlinearOptimizer try_lambda(const GaussianFactorGraph& linear,
@@ -70,9 +72,16 @@ namespace gtsam {
 		/**
 		 * Constructor
 		 */
-		NonlinearOptimizer(const FactorGraph& graph, const Ordering& ordering,
+		NonlinearOptimizer(shared_graph graph, shared_ordering ordering,
 				shared_config config, double lambda = 1e-5);
 
+		/**
+		 * Copy constructor
+		 */
+		NonlinearOptimizer(const NonlinearOptimizer<FactorGraph, Config> &optimizer) :
+		  graph_(optimizer.graph_), ordering_(optimizer.ordering_), config_(optimizer.config_),
+		  error_(optimizer.error_), lambda_(optimizer.lambda_) {}
+
 		/**
 		 * Return current error
 		 */

cpp/testNonlinearOptimizer.cpp

@@ -12,6 +12,9 @@ using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <boost/shared_ptr.hpp>
+using namespace boost;
+
 #include "Matrix.h"
 #include "Ordering.h"
 #include "smallExample.h"
@@ -27,7 +30,7 @@ typedef NonlinearOptimizer<ExampleNonlinearFactorGraph,VectorConfig> Optimizer;
 /* ************************************************************************* */
 TEST( NonlinearOptimizer, delta )
 {
-	ExampleNonlinearFactorGraph fg = createNonlinearFactorGraph();
+	shared_ptr<ExampleNonlinearFactorGraph> fg(new ExampleNonlinearFactorGraph(createNonlinearFactorGraph()));
 	Optimizer::shared_config initial = sharedNoisyConfig();
 
 	// Expected configuration is the difference between the noisy config
@@ -47,22 +50,22 @@ TEST( NonlinearOptimizer, delta )
 	expected.insert("x2", dx2);
 
 	// Check one ordering
-	Ordering ord1;
+	shared_ptr<Ordering> ord1(new Ordering());
-	ord1 += "x2","l1","x1";
+	*ord1 += "x2","l1","x1";
 	Optimizer optimizer1(fg, ord1, initial);
 	VectorConfig actual1 = optimizer1.linearizeAndOptimizeForDelta();
 	CHECK(assert_equal(actual1,expected));
 
 	// Check another
-	Ordering ord2;
+	shared_ptr<Ordering> ord2(new Ordering());
-	ord2 += "x1","x2","l1";
+	*ord2 += "x1","x2","l1";
 	Optimizer optimizer2(fg, ord2, initial);
 	VectorConfig actual2 = optimizer2.linearizeAndOptimizeForDelta();
 	CHECK(assert_equal(actual2,expected));
 
 	// And yet another...
-	Ordering ord3;
+	shared_ptr<Ordering> ord3(new Ordering());
-	ord3 += "l1","x1","x2";
+	*ord3 += "l1","x1","x2";
 	Optimizer optimizer3(fg, ord3, initial);
 	VectorConfig actual3 = optimizer3.linearizeAndOptimizeForDelta();
 	CHECK(assert_equal(actual3,expected));
@@ -72,7 +75,7 @@ TEST( NonlinearOptimizer, delta )
 TEST( NonlinearOptimizer, iterateLM )
 {
 	// really non-linear factor graph
-	ExampleNonlinearFactorGraph fg = createReallyNonlinearFactorGraph();
+  shared_ptr<ExampleNonlinearFactorGraph> fg(new ExampleNonlinearFactorGraph(createReallyNonlinearFactorGraph()));
 
 	// config far from minimum
 	Vector x0 = Vector_(1, 3.0);
@@ -80,8 +83,8 @@ TEST( NonlinearOptimizer, iterateLM )
 	config->insert("x", x0);
 
 	// ordering
-	Ordering ord;
+	shared_ptr<Ordering> ord(new Ordering());
-	ord.push_back("x");
+	ord->push_back("x");
 
 	// create initial optimization state, with lambda=0
 	Optimizer optimizer(fg, ord, config, 0);
@@ -107,23 +110,23 @@ TEST( NonlinearOptimizer, iterateLM )
 /* ************************************************************************* */
 TEST( NonlinearOptimizer, optimize )
 {
-	ExampleNonlinearFactorGraph fg = createReallyNonlinearFactorGraph();
+  shared_ptr<ExampleNonlinearFactorGraph> fg(new ExampleNonlinearFactorGraph(createReallyNonlinearFactorGraph()));
 
 	// test error at minimum
 	Vector xstar = Vector_(1, 0.0);
 	VectorConfig cstar;
 	cstar.insert("x", xstar);
-	DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);
+	DOUBLES_EQUAL(0.0,fg->error(cstar),0.0);
 
 	// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
 	Vector x0 = Vector_(1, 3.0);
 	boost::shared_ptr<VectorConfig> c0(new VectorConfig);
 	c0->insert("x", x0);
-	DOUBLES_EQUAL(199.0,fg.error(*c0),1e-3);
+	DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);
 
 	// optimize parameters
-	Ordering ord;
+	shared_ptr<Ordering> ord(new Ordering());
-	ord.push_back("x");
+	ord->push_back("x");
 	double relativeThreshold = 1e-5;
 	double absoluteThreshold = 1e-5;
 

cpp/testPose2Graph.cpp

@@ -6,6 +6,7 @@
 #include <iostream>
 #include <boost/shared_ptr.hpp>
 #include <boost/assign/std/list.hpp>
+using namespace boost;
 using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
@@ -84,9 +85,9 @@ TEST( Pose2Graph, linerization )
 TEST(Pose2Graph, optimize) {
 
 	// create a Pose graph with one equality constraint and one measurement
-  Pose2Graph fg;
+  shared_ptr<Pose2Graph> fg(new Pose2Graph);
-  fg.addConstraint("p0", Pose2(0,0,0));
+  fg->addConstraint("p0", Pose2(0,0,0));
-  fg.add("p0", "p1", Pose2(1,2,M_PI_2), covariance);
+  fg->add("p0", "p1", Pose2(1,2,M_PI_2), covariance);
 
   // Create initial config
   boost::shared_ptr<Pose2Config> initial(new Pose2Config());
@@ -94,13 +95,13 @@ TEST(Pose2Graph, optimize) {
   initial->insert("p1", Pose2(0,0,0));
 
   // Choose an ordering and optimize
-  Ordering ordering;
+  shared_ptr<Ordering> ordering(new Ordering);
-  ordering += "p0","p1";
+  *ordering += "p0","p1";
   typedef NonlinearOptimizer<Pose2Graph, Pose2Config> Optimizer;
-  Optimizer optimizer(fg, ordering, initial);
+  Optimizer optimizer0(fg, ordering, initial);
   Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
   //Optimizer::verbosityLevel verbosity = Optimizer::ERROR;
-  optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity);
+  Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity);
 
   // Check with expected config
   Pose2Config expected;
@@ -118,15 +119,15 @@ TEST(Pose2Graph, optimizeCircle) {
   Pose2 p0 = hexagon["p0"], p1 = hexagon["p1"];
 
 	// create a Pose graph with one equality constraint and one measurement
-  Pose2Graph fg;
+  shared_ptr<Pose2Graph> fg(new Pose2Graph);
-  fg.addConstraint("p0", p0);
+  fg->addConstraint("p0", p0);
   Pose2 delta = between(p0,p1);
-  fg.add("p0", "p1", delta, covariance);
+  fg->add("p0", "p1", delta, covariance);
-  fg.add("p1", "p2", delta, covariance);
+  fg->add("p1", "p2", delta, covariance);
-  fg.add("p2", "p3", delta, covariance);
+  fg->add("p2", "p3", delta, covariance);
-  fg.add("p3", "p4", delta, covariance);
+  fg->add("p3", "p4", delta, covariance);
-  fg.add("p4", "p5", delta, covariance);
+  fg->add("p4", "p5", delta, covariance);
-  fg.add("p5", "p0", delta, covariance);
+  fg->add("p5", "p0", delta, covariance);
 
   // Create initial config
   boost::shared_ptr<Pose2Config> initial(new Pose2Config());
@@ -138,13 +139,13 @@ TEST(Pose2Graph, optimizeCircle) {
   initial->insert("p5", expmap(hexagon["p5"],Vector_(3,-0.1, 0.1,-0.1)));
 
   // Choose an ordering and optimize
-  Ordering ordering;
+  shared_ptr<Ordering> ordering(new Ordering);
-  ordering += "p0","p1","p2","p3","p4","p5";
+  *ordering += "p0","p1","p2","p3","p4","p5";
   typedef NonlinearOptimizer<Pose2Graph, Pose2Config> Optimizer;
-  Optimizer optimizer(fg, ordering, initial);
+  Optimizer optimizer0(fg, ordering, initial);
   Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
 //  Optimizer::verbosityLevel verbosity = Optimizer::ERROR;
-  optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity);
+  Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity);
 
   Pose2Config actual = *optimizer.config();
 

cpp/testPose3Graph.cpp

@@ -8,6 +8,7 @@
 #include <iostream>
 #include <boost/shared_ptr.hpp>
 #include <boost/assign/std/list.hpp>
+using namespace boost;
 using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
@@ -32,15 +33,15 @@ TEST(Pose3Graph, optimizeCircle) {
   Pose3 p0 = hexagon["p0"], p1 = hexagon["p1"];
 
 	// create a Pose graph with one equality constraint and one measurement
-  Pose3Graph fg;
+  shared_ptr<Pose3Graph> fg(new Pose3Graph);
-  fg.addConstraint("p0", p0);
+  fg->addConstraint("p0", p0);
   Pose3 delta = between(p0,p1);
-  fg.add("p0", "p1", delta, covariance);
+  fg->add("p0", "p1", delta, covariance);
-  fg.add("p1", "p2", delta, covariance);
+  fg->add("p1", "p2", delta, covariance);
-  fg.add("p2", "p3", delta, covariance);
+  fg->add("p2", "p3", delta, covariance);
-  fg.add("p3", "p4", delta, covariance);
+  fg->add("p3", "p4", delta, covariance);
-  fg.add("p4", "p5", delta, covariance);
+  fg->add("p4", "p5", delta, covariance);
-  fg.add("p5", "p0", delta, covariance);
+  fg->add("p5", "p0", delta, covariance);
 
   // Create initial config
   boost::shared_ptr<Pose3Config> initial(new Pose3Config());
@@ -52,13 +53,13 @@ TEST(Pose3Graph, optimizeCircle) {
   initial->insert("p5", expmap(hexagon["p5"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
 
   // Choose an ordering and optimize
-  Ordering ordering;
+  shared_ptr<Ordering> ordering(new Ordering);
-  ordering += "p0","p1","p2","p3","p4","p5";
+  *ordering += "p0","p1","p2","p3","p4","p5";
   typedef NonlinearOptimizer<Pose3Graph, Pose3Config> Optimizer;
-  Optimizer optimizer(fg, ordering, initial);
+  Optimizer optimizer0(fg, ordering, initial);
   Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
 //  Optimizer::verbosityLevel verbosity = Optimizer::ERROR;
-  optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity);
+  Optimizer optimizer = optimizer0.levenbergMarquardt(1e-15, 1e-15, verbosity);
 
   Pose3Config actual = *optimizer.config();
 

cpp/testSQP.cpp

@@ -9,6 +9,7 @@
 #include <boost/assign/std/list.hpp> // for operator +=
 #include <boost/assign/std/map.hpp> // for insert
 #include <boost/foreach.hpp>
+#include <boost/shared_ptr.hpp>
 #include <CppUnitLite/TestHarness.h>
 #include <GaussianFactorGraph.h>
 #include <NonlinearFactor.h>
@@ -32,6 +33,7 @@
 
 using namespace std;
 using namespace gtsam;
+using namespace boost;
 using namespace boost::assign;
 
 // trick from some reading group
@@ -286,11 +288,11 @@ TEST (SQP, two_pose_truth ) {
 	shared f2(new Simulated2DMeasurement(z2, sigma2, "x2", "l1"));
 
 	// construct the graph
-	NLGraph graph;
+	shared_ptr<NLGraph> graph(new NLGraph());
-	graph.push_back(ef1);
+	graph->push_back(ef1);
-	graph.push_back(ef2);
+	graph->push_back(ef2);
-	graph.push_back(f1);
+	graph->push_back(f1);
-	graph.push_back(f2);
+	graph->push_back(f2);
 
 	// create an initial estimate
 	boost::shared_ptr<VectorConfig> initialEstimate(new VectorConfig(feas)); // must start with feasible set
@@ -298,8 +300,8 @@ TEST (SQP, two_pose_truth ) {
 	//initialEstimate->insert("l1", Vector_(2, 1.2, 5.6)); // with small error
 
 	// optimize the graph
-	Ordering ordering;
+	shared_ptr<Ordering> ordering(new Ordering());
-	ordering += "x1", "x2", "l1";
+	*ordering += "x1", "x2", "l1";
 	Optimizer optimizer(graph, ordering, initialEstimate, 1e-5);
 
 	// display solution
@@ -498,27 +500,27 @@ TEST (SQP, stereo_truth ) {
 	truthConfig->addLandmarkPoint(1, landmark);
 
 	// create graph
-	VSLAMGraph graph;
+	shared_ptr<VSLAMGraph> graph(new VSLAMGraph());
 
 	// create equality constraints for poses
-	graph.addCameraConstraint(1, camera1.pose());
+	graph->addCameraConstraint(1, camera1.pose());
-	graph.addCameraConstraint(2, camera2.pose());
+	graph->addCameraConstraint(2, camera2.pose());
 
 	// create VSLAM factors
 	Point2 z1 = camera1.project(landmark);
 	if (verbose) z1.print("z1");
 	shared_vf vf1(new VSLAMFactor(z1, 1.0, 1, 1, shK));
-	graph.push_back(vf1);
+	graph->push_back(vf1);
 	Point2 z2 = camera2.project(landmark);
 	if (verbose) z2.print("z2");
 	shared_vf vf2(new VSLAMFactor(z2, 1.0, 2, 1, shK));
-	graph.push_back(vf2);
+	graph->push_back(vf2);
 
-	if (verbose) graph.print("Graph after construction");
+	if (verbose) graph->print("Graph after construction");
 
 	// create ordering
-	Ordering ord;
+	shared_ptr<Ordering> ord(new Ordering());
-	ord += "x1", "x2", "l1";
+	*ord += "x1", "x2", "l1";
 
 	// create optimizer
 	VOptimizer optimizer(graph, ord, truthConfig, 1e-5);
@@ -571,40 +573,40 @@ TEST (SQP, stereo_truth_noisy ) {
 	noisyConfig->addLandmarkPoint(1, landmarkNoisy);
 
 	// create graph
-	VSLAMGraph graph;
+	shared_ptr<VSLAMGraph> graph(new VSLAMGraph());
 
 	// create equality constraints for poses
-	graph.addCameraConstraint(1, camera1.pose());
+	graph->addCameraConstraint(1, camera1.pose());
-	graph.addCameraConstraint(2, camera2.pose());
+	graph->addCameraConstraint(2, camera2.pose());
 
 	// create VSLAM factors
 	Point2 z1 = camera1.project(landmark);
 	if (verbose) z1.print("z1");
 	shared_vf vf1(new VSLAMFactor(z1, 1.0, 1, 1, shK));
-	graph.push_back(vf1);
+	graph->push_back(vf1);
 	Point2 z2 = camera2.project(landmark);
 	if (verbose) z2.print("z2");
 	shared_vf vf2(new VSLAMFactor(z2, 1.0, 2, 1, shK));
-	graph.push_back(vf2);
+	graph->push_back(vf2);
 
 	if (verbose)  {
-		graph.print("Graph after construction");
+		graph->print("Graph after construction");
 		noisyConfig->print("Initial config");
 	}
 
 	// create ordering
-	Ordering ord;
+	shared_ptr<Ordering> ord(new Ordering());
-	ord += "x1", "x2", "l1";
+	*ord += "x1", "x2", "l1";
 
 	// create optimizer
-	VOptimizer optimizer(graph, ord, noisyConfig, 1e-5);
+	VOptimizer optimizer0(graph, ord, noisyConfig, 1e-5);
 
 	if (verbose)
-		cout << "Initial Error: " << optimizer.error() << endl;
+		cout << "Initial Error: " << optimizer0.error() << endl;
 
 	// use Levenberg-Marquardt optimization
 	double relThresh = 1e-5, absThresh = 1e-5;
-	optimizer = optimizer.levenbergMarquardt(relThresh, absThresh, VOptimizer::SILENT);
+	VOptimizer optimizer(optimizer0.levenbergMarquardt(relThresh, absThresh, VOptimizer::SILENT));
 
 	// verify
 	DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);

cpp/testVSLAMGraph.cpp

@@ -8,6 +8,8 @@
  */
 
 #include <CppUnitLite/TestHarness.h>
+#include <boost/shared_ptr.hpp>
+using namespace boost;
 
 #include "VSLAMGraph.h"
 #include "NonlinearFactorGraph-inl.h"
@@ -66,11 +68,11 @@ VSLAMGraph testGraph() {
 TEST( VSLAMGraph, optimizeLM)
 {
   // build a graph
-  VSLAMGraph graph = testGraph();
+  shared_ptr<VSLAMGraph> graph(new VSLAMGraph(testGraph()));
 	// add 3 landmark constraints
-  graph.addLandmarkConstraint(1, landmark1);
+  graph->addLandmarkConstraint(1, landmark1);
-  graph.addLandmarkConstraint(2, landmark2);
+  graph->addLandmarkConstraint(2, landmark2);
-  graph.addLandmarkConstraint(3, landmark3);
+  graph->addLandmarkConstraint(3, landmark3);
 
   // Create an initial configuration corresponding to the ground truth
   boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
@@ -89,7 +91,7 @@ TEST( VSLAMGraph, optimizeLM)
   keys.push_back("l4");
   keys.push_back("x1");
   keys.push_back("x2");
-  Ordering ordering(keys);
+  shared_ptr<Ordering> ordering(new Ordering(keys));
 
   // Create an optimizer and check its error
   // We expect the initial to be zero because config is the ground truth
@@ -110,10 +112,10 @@ TEST( VSLAMGraph, optimizeLM)
 TEST( VSLAMGraph, optimizeLM2)
 {
   // build a graph
-  VSLAMGraph graph = testGraph();
+  shared_ptr<VSLAMGraph> graph(new VSLAMGraph(testGraph()));
 	// add 2 camera constraints
-  graph.addCameraConstraint(1, camera1);
+  graph->addCameraConstraint(1, camera1);
-  graph.addCameraConstraint(2, camera2);
+  graph->addCameraConstraint(2, camera2);
 
   // Create an initial configuration corresponding to the ground truth
   boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
@@ -133,7 +135,7 @@ TEST( VSLAMGraph, optimizeLM2)
   keys.push_back("l4");
   keys.push_back("x1");
   keys.push_back("x2");
-  Ordering ordering(keys);
+  shared_ptr<Ordering> ordering(new Ordering(keys));
 
   // Create an optimizer and check its error
   // We expect the initial to be zero because config is the ground truth
@@ -154,10 +156,10 @@ TEST( VSLAMGraph, optimizeLM2)
 TEST( VSLAMGraph, CHECK_ORDERING)
 {
   // build a graph
-  VSLAMGraph graph = testGraph();
+  shared_ptr<VSLAMGraph> graph(new VSLAMGraph(testGraph()));
   // add 2 camera constraints
-  graph.addCameraConstraint(1, camera1);
+  graph->addCameraConstraint(1, camera1);
-  graph.addCameraConstraint(2, camera2);
+  graph->addCameraConstraint(2, camera2);
 
   // Create an initial configuration corresponding to the ground truth
   boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
@@ -168,7 +170,7 @@ TEST( VSLAMGraph, CHECK_ORDERING)
   initialEstimate->addLandmarkPoint(3, landmark3);
   initialEstimate->addLandmarkPoint(4, landmark4);
 
-  Ordering ordering = graph.getOrdering();
+  shared_ptr<Ordering> ordering(new Ordering(graph->getOrdering()));
 
   // Create an optimizer and check its error
   // We expect the initial to be zero because config is the ground truth