From f354f8183dc07936b16eb84125f89b0588d8ce70 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 22 Oct 2010 02:53:27 +0000
Subject: [PATCH] Added examples of using NonlinearOptimizer with multifrontal
 elimination

---
 examples/PlanarSLAMSelfContained_advanced.cpp | 13 ++++++++++---
 linear/NoiseModel.cpp                         |  2 --
 slam/pose2SLAM.h                              |  5 ++++-
 3 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp
index a37467f1f..211871d98 100644
--- a/examples/PlanarSLAMSelfContained_advanced.cpp
+++ b/examples/PlanarSLAMSelfContained_advanced.cpp
@@ -38,6 +38,7 @@
 #include <gtsam/nonlinear/TupleValues-inl.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
 #include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
+#include <gtsam/linear/GaussianMultifrontalSolver.h>
 
 // Main typedefs
 typedef gtsam::TypedSymbol<gtsam::Pose2, 'x'> PoseKey;       // Key for poses, with type included
@@ -46,7 +47,8 @@ typedef gtsam::LieValues<PoseKey> PoseValues;                // config type for
 typedef gtsam::LieValues<PointKey> PointValues;              // config type for points
 typedef gtsam::TupleValues2<PoseValues, PointValues> Values; // main config with two variable classes
 typedef gtsam::NonlinearFactorGraph<Values> Graph;			 // graph structure
-typedef gtsam::NonlinearOptimizer<Graph,Values> Optimizer;   // optimization engine for this domain
+typedef gtsam::NonlinearOptimizer<Graph,Values,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential;   // optimization engine for this domain
+typedef gtsam::NonlinearOptimizer<Graph,Values,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal;   // optimization engine for this domain
 
 using namespace std;
 using namespace gtsam;
@@ -120,9 +122,14 @@ int main(int argc, char** argv) {
 	initial.print("initial estimate");
 
 	// optimize using Levenburg-Marquadt optimization with an ordering from colamd
-	Optimizer::shared_values result = Optimizer::optimizeLM(graph, initial);
 
-	result->print("final result");
+	// first using sequential elimination
+	OptimizerSeqential::shared_values resultSequential = OptimizerSeqential::optimizeLM(graph, initial);
+	resultSequential->print("final result (solved with a sequential solver)");
+
+	// then using multifrontal
+  OptimizerMultifrontal::shared_values resultMultifrontal = OptimizerMultifrontal::optimizeLM(graph, initial);
+  resultMultifrontal->print("final result (solved with a multifrontal solver)");
 
 	return 0;
 }
diff --git a/linear/NoiseModel.cpp b/linear/NoiseModel.cpp
index 20eb2fcf9..f55bede1a 100644
--- a/linear/NoiseModel.cpp
+++ b/linear/NoiseModel.cpp
@@ -220,9 +220,7 @@ SharedDiagonal Gaussian::QR(Matrix& Ab, boost::optional<std::vector<long>&> firs
 // General QR, see also special version in Constrained
 SharedDiagonal Gaussian::QRColumnWise(ublas::matrix<double, ublas::column_major>& Ab, vector<long>& firstZeroRows) const {
 	Matrix Abresult(Ab);
-	gtsam::print(Abresult, "Abresult before = ");
 	SharedDiagonal result = QR(Abresult, firstZeroRows);
-	gtsam::print(Abresult, "Abresult after = ");
 	Ab = Abresult;
 	return result;
 //  // get size(A) and maxRank
diff --git a/slam/pose2SLAM.h b/slam/pose2SLAM.h
index 18a138cc2..e7cac8552 100644
--- a/slam/pose2SLAM.h
+++ b/slam/pose2SLAM.h
@@ -26,6 +26,8 @@
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
+#include <gtsam/linear/GaussianSequentialSolver.h>
+#include <gtsam/linear/GaussianMultifrontalSolver.h>
 
 namespace gtsam {
 
@@ -60,7 +62,8 @@ namespace gtsam {
 		};
 
 		// Optimizer
-		typedef NonlinearOptimizer<Graph, Values> Optimizer;
+		typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianSequentialSolver> OptimizerSequential;
+    typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
 
 	} // pose2SLAM