From c4df80df55f0bda4bd115ca0244474f803872171 Mon Sep 17 00:00:00 2001
From: Yong-Dian Jian <ydjian@gmail.com>
Date: Thu, 21 Oct 2010 01:51:23 +0000
Subject: [PATCH] sample for new optimization interface

---
 examples/Pose2SLAMwSPCG.cpp           | 30 +++++++++++++++------------
 nonlinear/NonlinearOptimization-inl.h | 20 +++++++++++++++---
 2 files changed, 34 insertions(+), 16 deletions(-)

diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp
index 4bd026b0d..7ad7d38a2 100644
--- a/examples/Pose2SLAMwSPCG.cpp
+++ b/examples/Pose2SLAMwSPCG.cpp
@@ -24,6 +24,7 @@
 #include <gtsam/inference/FactorGraph-inl.h>
 #include <gtsam/linear/SubgraphSolver-inl.h>
 #include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
+#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
 #include <gtsam/slam/pose2SLAM.h>
 
 using namespace std;
@@ -45,25 +46,28 @@ void generateData() ;
 /* ************************************************************************* */
 int main(void) {
 
+	const bool bNewInterface = true ;
 	const SharedGaussian sigma(noiseModel::Unit::Create(0.1));
 
 	// generate measurement and initial configuration
 	generateData() ;
 
-	cout << "Initialize .... " << endl;
-	SPCGOptimizer::shared_solver solver(new SPCGOptimizer::solver(*G, initial)) ;
-	sharedValue SV(new Values(initial)) ;
-	SPCGOptimizer optimizer(G, SV, solver) ;
+	if ( bNewInterface == true ) {
+		result = optimizeSPCG(*G, initial, NonlinearOptimizationParameters());
+	}
+	else {
+		cout << "Initialize .... " << endl;
+		SPCGOptimizer::shared_solver solver(new SPCGOptimizer::solver(*G, initial)) ;
+		sharedValue SV(new Values(initial)) ;
+		SPCGOptimizer optimizer(G, SV, solver) ;
+		cout << "before optimization, sum of error is " << optimizer.error() << endl;
+		cout << "Optimize .... " << endl;
+		NonlinearOptimizationParameters parameter;
+		SPCGOptimizer optimizer2 = optimizer.levenbergMarquardt(parameter);
+		cout << "after optimization, sum of error is " << optimizer2.error() << endl;
+		result = *optimizer2.config() ;
+	}
 
-	cout << "before optimization, sum of error is " << optimizer.error() << endl;
-
-	cout << "Optimize .... " << endl;
-	NonlinearOptimizationParameters parameter;
-	SPCGOptimizer optimizer2 = optimizer.levenbergMarquardt(parameter);
-
-	cout << "after optimization, sum of error is " << optimizer2.error() << endl;
-
-	result = *optimizer2.config() ;
 	result.print("result") ;
 
 	return 0 ;
diff --git a/nonlinear/NonlinearOptimization-inl.h b/nonlinear/NonlinearOptimization-inl.h
index 660dfdbc3..f95ced91e 100644
--- a/nonlinear/NonlinearOptimization-inl.h
+++ b/nonlinear/NonlinearOptimization-inl.h
@@ -20,6 +20,7 @@
 #pragma once
 
 #include <gtsam/linear/Factorization.h>
+#include <gtsam/linear/SubgraphSolver-inl.h>
 #include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
 #include <gtsam/nonlinear/NonlinearOptimization.h>
 
@@ -60,7 +61,18 @@ namespace gtsam {
 	 */
 	template<class G, class T>
 	T	optimizeSPCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters) {
-		throw runtime_error("optimizeSPCG: not implemented");
+
+		// initial optimization state is the same in both cases tested
+		typedef NonlinearOptimizer<G, T, SubgraphPreconditioner, SubgraphSolver<G,T> > SPCGOptimizer;
+		typename SPCGOptimizer::shared_solver solver(new SubgraphSolver<G,T>(graph, initialEstimate));
+		SPCGOptimizer optimizer(
+				boost::make_shared<const G>(graph),
+				boost::make_shared<const T>(initialEstimate),
+				solver);
+
+		// Levenberg-Marquardt
+		SPCGOptimizer result = optimizer.levenbergMarquardt(parameters);
+		return *result.config();
 	}
 
 	/**
@@ -75,9 +87,11 @@ namespace gtsam {
 		case MULTIFRONTAL:
 			return optimizeMultiFrontal<G,T>(graph, initialEstimate, parameters);
 		case SPCG:
-			return optimizeSPCG<G,T>(graph, initialEstimate, parameters) ;
+			throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint");
+//			return optimizeSPCG<G,T>(graph, initialEstimate, parameters) ;
+			break;
 		}
-		throw runtime_error("optimizeSPCG: undefined solver");
+		throw runtime_error("optimize: undefined solver");
 	}
 
 } //namespace gtsam