Updated examples for new NonlinearOptimizer

examples/CameraResectioning.cpp

@@ -22,7 +22,7 @@
 #include <gtsam/geometry/SimpleCamera.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/NonlinearOptimization.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
 using namespace gtsam;
 
@@ -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 = optimize<NonlinearFactorGraph> (graph, initial);
+  Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized();
   result.print("Final result: ");
 
   return 0;

examples/PlanarSLAMExample_easy.cpp

@@ -20,7 +20,7 @@
 
 // pull in the planar SLAM domain with all typedefs and helper functions defined
 #include <gtsam/slam/planarSLAM.h>
-#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
 using namespace std;
 using namespace gtsam;
@@ -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 = optimize(graph, initialEstimate);
+	planarSLAM::Values result = *LevenbergMarquardtOptimizer(graph, initialEstimate).optimized();
 	result.print("final result");
 
 	return 0;

examples/PlanarSLAMSelfContained_advanced.cpp

@@ -35,17 +35,13 @@
 
 // implementations for structures - needed if self-contained, and these should be included last
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/NonlinearOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
 #include <gtsam/linear/GaussianMultifrontalSolver.h>
 
 using namespace std;
 using namespace gtsam;
 
-// Main typedefs
-typedef NonlinearOptimizer<NonlinearFactorGraph,GaussianFactorGraph,GaussianSequentialSolver> OptimizerSeqential;   // optimization engine for this domain
-typedef NonlinearOptimizer<NonlinearFactorGraph,GaussianFactorGraph,GaussianMultifrontalSolver> OptimizerMultifrontal;   // optimization engine for this domain
-
 /**
  * In this version of the system we make the following assumptions:
  *  - All values are axis aligned
@@ -117,22 +113,27 @@ int main(int argc, char** argv) {
 	// optimize using Levenberg-Marquardt optimization with an ordering from colamd
 
 	// first using sequential elimination
-	OptimizerSeqential::shared_values resultSequential = OptimizerSeqential::optimizeLM(*graph, *initial);
+	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)");
 
 	// then using multifrontal, advanced interface
-	// Note how we create an optimizer, call LM, then we get access to covariances
+	// Note that we keep the original optimizer object so we can use the COLAMD
-	Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial);
+	// ordering it computes.
-  OptimizerMultifrontal optimizerMF(graph, initial, ordering);
+	LevenbergMarquardtOptimizer optimizer(graph, initial);
-  OptimizerMultifrontal resultMF = optimizerMF.levenbergMarquardt();
+	Values::const_shared_ptr resultMultifrontal = optimizer.optimized();
-  resultMF.values()->print("final result (solved with a multifrontal solver)");
+	resultMultifrontal->print("final result (solved with a multifrontal solver)");
+
+	const Ordering& ordering = *optimizer.ordering();
+	GaussianMultifrontalSolver linearSolver(*graph->linearize(*resultMultifrontal, ordering));
 
   // Print marginals covariances for all variables
-  print(resultMF.marginalCovariance(x1), "x1 covariance");
+  print(linearSolver.marginalCovariance(ordering[x1]), "x1 covariance");
-  print(resultMF.marginalCovariance(x2), "x2 covariance");
+  print(linearSolver.marginalCovariance(ordering[x2]), "x2 covariance");
-  print(resultMF.marginalCovariance(x3), "x3 covariance");
+  print(linearSolver.marginalCovariance(ordering[x3]), "x3 covariance");
-  print(resultMF.marginalCovariance(l1), "l1 covariance");
+  print(linearSolver.marginalCovariance(ordering[l1]), "l1 covariance");
-  print(resultMF.marginalCovariance(l2), "l2 covariance");
+  print(linearSolver.marginalCovariance(ordering[l2]), "l2 covariance");
 
 	return 0;
 }

examples/Pose2SLAMExample_advanced.cpp

@@ -22,7 +22,7 @@
 
 // pull in the Pose2 SLAM domain with all typedefs and helper functions defined
 #include <gtsam/slam/pose2SLAM.h>
-#include <gtsam/nonlinear/NonlinearOptimization.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
@@ -65,16 +65,16 @@ int main(int argc, char** argv) {
 	Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial);
 
 	/* 4.2.2 set up solver and optimize */
-  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
+	LevenbergMarquardtParams params;
-	Optimizer optimizer(graph, initial, ordering, params);
+	params.relativeErrorTol = 1e-15;
-	Optimizer optimizer_result = optimizer.levenbergMarquardt();
+	params.absoluteErrorTol = 1e-15;
-
+	pose2SLAM::Values result = *LevenbergMarquardtOptimizer(graph, initial, params, ordering).optimized();
-	pose2SLAM::Values result = *optimizer_result.values();
 	result.print("final result");
 
 	/* Get covariances */
-	Matrix covariance1  = optimizer_result.marginalCovariance(PoseKey(1));
+	GaussianMultifrontalSolver solver(*graph->linearize(result, *ordering));
-	Matrix covariance2  = optimizer_result.marginalCovariance(PoseKey(1));
+	Matrix covariance1 = solver.marginalCovariance(ordering->at(PoseKey(1)));
+	Matrix covariance2 = solver.marginalCovariance(ordering->at(PoseKey(1)));
 
 	print(covariance1, "Covariance1");
 	print(covariance2, "Covariance2");

examples/Pose2SLAMExample_easy.cpp

@@ -24,7 +24,7 @@
 
 // pull in the Pose2 SLAM domain with all typedefs and helper functions defined
 #include <gtsam/slam/pose2SLAM.h>
-#include <gtsam/nonlinear/NonlinearOptimization.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
 using namespace std;
 using namespace gtsam;
@@ -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 = optimize<Graph>(graph, initial);
+	pose2SLAM::Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized();
 	result.print("final result");
 
 

examples/SimpleRotation.cpp

@@ -24,7 +24,7 @@
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/NonlinearOptimization.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
 /*
  * TODO: make factors independent of RotValues
@@ -105,7 +105,7 @@ int main() {
 	 * initial estimate.  This will yield a new RotValues structure
 	 * with the final state of the optimization.
 	 */
-	Values result = optimize(graph, initial);
+	Values result = *LevenbergMarquardtOptimizer(graph, initial).optimized();
 	result.print("final result");
 
 	return 0;

examples/vSLAMexample/vSFMexample.cpp

@@ -20,7 +20,7 @@
 using namespace boost;
 
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/NonlinearOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/slam/visualSLAM.h>
 #include <gtsam/slam/PriorFactor.h>
 
@@ -143,12 +143,13 @@ int main(int argc, char* argv[]) {
 
   // Optimize the graph
   cout << "*******************************************************" << endl;
-  NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newVerbosity(Optimizer::Parameters::DAMPED);
+  LevenbergMarquardtParams params;
-  visualSLAM::Optimizer::shared_values result = visualSLAM::Optimizer::optimizeGN(graph, initialEstimates, params);
+  params.lmVerbosity = LevenbergMarquardtParams::DAMPED;
+  visualSLAM::Values result = *LevenbergMarquardtOptimizer(graph, initialEstimates, params).optimized();
 
   // Print final results
   cout << "*******************************************************" << endl;
-  result->print("FINAL RESULTS: ");
+  result.print("FINAL RESULTS: ");
 
   return 0;
 }