Updated examples and namespaces for the new NonlinearOptimizer interface

examples/CameraResectioning.cpp

@@ -95,7 +95,7 @@ int main(int argc, char* argv[]) {
                                0.,0.,-1.), Point3(0.,0.,2.0)));
 
   /* 4. Optimize the graph using Levenberg-Marquardt*/
-  Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized();
+  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
   result.print("Final result: ");
 
   return 0;

examples/PlanarSLAMExample_easy.cpp

@@ -83,7 +83,7 @@ int main(int argc, char** argv) {
 	initialEstimate.print("initial estimate");
 
 	// optimize using Levenberg-Marquardt optimization with an ordering from colamd
-	planarSLAM::Values result = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimized();
+	planarSLAM::Values result = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
 	result.print("final result");
 
 	return 0;

examples/PlanarSLAMSelfContained_advanced.cpp

@@ -57,14 +57,14 @@ int main(int argc, char** argv) {
 	Symbol l1('l',1), l2('l',2);
 
 	// create graph container and add factors to it
-	NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
+	NonlinearFactorGraph graph;
 
 	/* add prior  */
 	// gaussian for prior
 	SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
 	Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
 	PriorFactor<Pose2> posePrior(x1, prior_measurement, prior_model); // create the factor
-	graph->add(posePrior);  // add the factor to the graph
+	graph.add(posePrior);  // add the factor to the graph
 
 	/* add odometry */
 	// general noisemodel for odometry
@@ -73,8 +73,8 @@ int main(int argc, char** argv) {
 	// create between factors to represent odometry
 	BetweenFactor<Pose2> odom12(x1, x2, odom_measurement, odom_model);
 	BetweenFactor<Pose2> odom23(x2, x3, odom_measurement, odom_model);
-	graph->add(odom12); // add both to graph
-	graph->add(odom23);
+	graph.add(odom12); // add both to graph
+	graph.add(odom23);
 
 	/* add measurements */
 	// general noisemodel for measurements
@@ -94,39 +94,39 @@ int main(int argc, char** argv) {
 	BearingRangeFactor<Pose2, Point2> meas32(x3, l2, bearing32, range32, meas_model);
 
 	// add the factors
-	graph->add(meas11);
-	graph->add(meas21);
-	graph->add(meas32);
+	graph.add(meas11);
+	graph.add(meas21);
+	graph.add(meas32);
 
-	graph->print("Full Graph");
+	graph.print("Full Graph");
 
 	// initialize to noisy points
-	boost::shared_ptr<Values> initial(new Values);
-	initial->insert(x1, Pose2(0.5, 0.0, 0.2));
-	initial->insert(x2, Pose2(2.3, 0.1,-0.2));
-	initial->insert(x3, Pose2(4.1, 0.1, 0.1));
-	initial->insert(l1, Point2(1.8, 2.1));
-	initial->insert(l2, Point2(4.1, 1.8));
+	Values initial;
+	initial.insert(x1, Pose2(0.5, 0.0, 0.2));
+	initial.insert(x2, Pose2(2.3, 0.1,-0.2));
+	initial.insert(x3, Pose2(4.1, 0.1, 0.1));
+	initial.insert(l1, Point2(1.8, 2.1));
+	initial.insert(l2, Point2(4.1, 1.8));
 
-	initial->print("initial estimate");
+	initial.print("initial estimate");
 
 	// optimize using Levenberg-Marquardt optimization with an ordering from colamd
 
 	// first using sequential elimination
 	LevenbergMarquardtParams lmParams;
 	lmParams.elimination = LevenbergMarquardtParams::SEQUENTIAL;
-	Values::const_shared_ptr resultSequential = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimized();
-	resultSequential->print("final result (solved with a sequential solver)");
+	Values resultSequential = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
+	resultSequential.print("final result (solved with a sequential solver)");
 
 	// then using multifrontal, advanced interface
 	// Note that we keep the original optimizer object so we can use the COLAMD
 	// ordering it computes.
 	LevenbergMarquardtOptimizer optimizer(graph, initial);
-	Values::const_shared_ptr resultMultifrontal = optimizer.optimized();
-	resultMultifrontal->print("final result (solved with a multifrontal solver)");
+	Values resultMultifrontal = optimizer.optimize();
+	resultMultifrontal.print("final result (solved with a multifrontal solver)");
 
-	const Ordering& ordering = *optimizer.ordering();
-	GaussianMultifrontalSolver linearSolver(*graph->linearize(*resultMultifrontal, ordering));
+	const Ordering& ordering = *optimizer.params().ordering;
+	GaussianMultifrontalSolver linearSolver(*graph.linearize(resultMultifrontal, ordering));
 
   // Print marginals covariances for all variables
   print(linearSolver.marginalCovariance(ordering[x1]), "x1 covariance");

examples/Pose2SLAMExample_advanced.cpp

@@ -34,13 +34,13 @@ using namespace pose2SLAM;
 
 int main(int argc, char** argv) {
 	/* 1. create graph container and add factors to it */
-	shared_ptr<Graph> graph(new Graph);
+	Graph graph;
 
 	/* 2.a add prior  */
 	// gaussian for prior
 	SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
 	Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
-	graph->addPrior(1, prior_measurement, prior_model); // add directly to graph
+	graph.addPrior(1, prior_measurement, prior_model); // add directly to graph
 
 	/* 2.b add odometry */
 	// general noisemodel for odometry
@@ -48,36 +48,38 @@ int main(int argc, char** argv) {
 
 	/* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/
 	Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
-	graph->addOdometry(1, 2, odom_measurement, odom_model);
-	graph->addOdometry(2, 3, odom_measurement, odom_model);
-	graph->print("full graph");
+	graph.addOdometry(1, 2, odom_measurement, odom_model);
+	graph.addOdometry(2, 3, odom_measurement, odom_model);
+	graph.print("full graph");
 
   /* 3. Create the data structure to hold the initial estimate to the solution
    * initialize to noisy points */
-	shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values);
-	initial->insertPose(1, Pose2(0.5, 0.0, 0.2));
-	initial->insertPose(2, Pose2(2.3, 0.1,-0.2));
-	initial->insertPose(3, Pose2(4.1, 0.1, 0.1));
-	initial->print("initial estimate");
+	pose2SLAM::Values initial;
+	initial.insertPose(1, Pose2(0.5, 0.0, 0.2));
+	initial.insertPose(2, Pose2(2.3, 0.1,-0.2));
+	initial.insertPose(3, Pose2(4.1, 0.1, 0.1));
+	initial.print("initial estimate");
 
 	/* 4.2.1 Alternatively, you can go through the process step by step
 	 * Choose an ordering */
-	Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial);
+	Ordering ordering = *graph.orderingCOLAMD(initial);
 
 	/* 4.2.2 set up solver and optimize */
-  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
-	Optimizer optimizer(graph, initial, ordering, params);
-	Optimizer optimizer_result = optimizer.levenbergMarquardt();
+	LevenbergMarquardtParams params;
+	params.absoluteErrorTol = 1e-15;
+	params.relativeErrorTol = 1e-15;
+	params.ordering = ordering;
+  LevenbergMarquardtOptimizer	optimizer(graph, initial, params);
 
-	pose2SLAM::Values result = *optimizer_result.values();
+	pose2SLAM::Values result = optimizer.optimize();
 	result.print("final result");
 
-	/* Get covariances */
-	Matrix covariance1  = optimizer_result.marginalCovariance(PoseKey(1));
-	Matrix covariance2  = optimizer_result.marginalCovariance(PoseKey(2));
-
-	print(covariance1, "Covariance1");
-	print(covariance2, "Covariance2");
+//	/* Get covariances */
+//	Matrix covariance1  = optimizer_result.marginalCovariance(PoseKey(1));
+//	Matrix covariance2  = optimizer_result.marginalCovariance(PoseKey(2));
+//
+//	print(covariance1, "Covariance1");
+//	print(covariance2, "Covariance2");
 
 	return 0;
 }

examples/Pose2SLAMExample_easy.cpp

@@ -61,7 +61,7 @@ int main(int argc, char** argv) {
 
 	/* 4 Single Step Optimization
 	* optimize using Levenberg-Marquardt optimization with an ordering from colamd */
-	pose2SLAM::Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized();
+	pose2SLAM::Values result = graph.optimize(initial);
 	result.print("final result");
 
 

examples/SimpleRotation.cpp

@@ -105,7 +105,7 @@ int main() {
 	 * initial estimate.  This will yield a new RotValues structure
 	 * with the final state of the optimization.
 	 */
-	Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized();
+	Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
 	result.print("final result");
 
 	return 0;

examples/vSLAMexample/vSFMexample.cpp

@@ -129,23 +129,23 @@ int main(int argc, char* argv[]) {
   readAllData();
 
   // Create a graph using the 2D measurements (features) and the calibration data
-  boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
+  Graph graph(setupGraph(g_measurements, measurementSigma, g_calib));
 
   // Create an initial Values structure using groundtruth values as the initial estimates
-  boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses)));
+  Values initialEstimates(initialize(g_landmarks, g_poses));
   cout << "*******************************************************" << endl;
-  initialEstimates->print("INITIAL ESTIMATES: ");
+  initialEstimates.print("INITIAL ESTIMATES: ");
 
   // Add prior factor for all poses in the graph
   map<int, Pose3>::iterator poseit = g_poses.begin();
   for (; poseit != g_poses.end(); poseit++)
-    graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1));
+    graph.addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1));
 
   // Optimize the graph
   cout << "*******************************************************" << endl;
   LevenbergMarquardtParams params;
   params.lmVerbosity = LevenbergMarquardtParams::DAMPED;
-  visualSLAM::Values result = *LevenbergMarquardtOptimizer(graph, initialEstimates, params).optimized();
+  visualSLAM::Values result = LevenbergMarquardtOptimizer(graph, initialEstimates, params).optimize();
 
   // Print final results
   cout << "*******************************************************" << endl;

gtsam/slam/planarSLAM.h

@@ -110,7 +110,7 @@ namespace planarSLAM {
 
     /// Optimize
     Values optimize(const Values& initialEstimate) {
-      return LevenbergMarquardtOptimizer(*this).optimized(initialEstimate);
+      return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
     }
   };