From 83f656f93ded66bd0674ad92eee54b1d284f1267 Mon Sep 17 00:00:00 2001
From: Yong-Dian Jian <ydjian@gmail.com>
Date: Mon, 4 Jun 2012 20:14:41 +0000
Subject: [PATCH] add the spcg example to matlab

---
 examples/Pose2SLAMwSPCG.cpp      |  9 ++----
 examples/matlab/Pose2SLAMwSPCG.m | 54 ++++++++++++++++++++++++++++++++
 gtsam.h                          |  1 +
 gtsam/slam/pose2SLAM.cpp         |  7 +++++
 gtsam/slam/pose2SLAM.h           |  1 +
 5 files changed, 65 insertions(+), 7 deletions(-)
 create mode 100644 examples/matlab/Pose2SLAMwSPCG.m

diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp
index 46a3787f2..cec4301dc 100644
--- a/examples/Pose2SLAMwSPCG.cpp
+++ b/examples/Pose2SLAMwSPCG.cpp
@@ -63,13 +63,8 @@ int main(void) {
 
   // 4. Single Step Optimization using Levenberg-Marquardt
   // Note: Although there are many options in IterativeOptimizationParameters,
-  // the SimpleSPCGSolver doesn't actually use all of them at this moment.
-  // More detail in the next release.
-  LevenbergMarquardtParams param;
-  param.linearSolverType = SuccessiveLinearizationParams::CG;
-  param.iterativeParams = boost::make_shared<IterativeOptimizationParameters>();
-  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, param);
-  Values result = optimizer.optimize();
+  Values result = graph.optimizeSPCG(initialEstimate);
+  result.print("\nFinal result:\n");
   cout << "final error = " << graph.error(result) << endl;
 
   return 0 ;
diff --git a/examples/matlab/Pose2SLAMwSPCG.m b/examples/matlab/Pose2SLAMwSPCG.m
new file mode 100644
index 000000000..f9c0537be
--- /dev/null
+++ b/examples/matlab/Pose2SLAMwSPCG.m
@@ -0,0 +1,54 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% GTSAM Copyright 2010, Georgia Tech Research Corporation,
+% Atlanta, Georgia 30332-0415
+% All Rights Reserved
+% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+%
+% See LICENSE for the license information
+%
+% @brief Simple 2D robotics example using the SimpleSPCGSolver
+% @author Yong-Dian Jian
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% Assumptions
+%  - All values are axis aligned
+%  - Robot poses are facing along the X axis (horizontal, to the right in images)
+%  - We have full odometry for measurements
+%  - The robot is on a grid, moving 2 meters each step
+
+%% Create graph container and add factors to it
+graph = pose2SLAMGraph;
+
+%% Add prior
+% gaussian for prior
+priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
+priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
+graph.addPrior(1, priorMean, priorNoise); % add directly to graph
+
+%% Add odometry
+% general noisemodel for odometry
+odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
+graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
+graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
+graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
+graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
+
+%% Add pose constraint
+model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
+graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
+
+% print
+graph.print(sprintf('\nFactor graph:\n'));
+
+%% Initialize to noisy points
+initialEstimate = pose2SLAMValues;
+initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2 ));
+initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2 ));
+initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, pi/2));
+initialEstimate.insertPose(4, gtsamPose2(4.0, 2.0, pi  ));
+initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2));
+initialEstimate.print(sprintf('\nInitial estimate:\n'));
+
+%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
+result = graph.optimizeSPCG(initialEstimate);
+result.print(sprintf('\nFinal result:\n'));
\ No newline at end of file
diff --git a/gtsam.h b/gtsam.h
index f9062cd9e..c73b59c3c 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -532,6 +532,7 @@ class Graph {
 	void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
 	void addConstraint(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
 	pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate) const;
+	pose2SLAM::Values optimizeSPCG(const pose2SLAM::Values& initialEstimate) const;
 	gtsam::Marginals marginals(const pose2SLAM::Values& solution) const;
 };
 
diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp
index 49ab37111..110a50551 100644
--- a/gtsam/slam/pose2SLAM.cpp
+++ b/gtsam/slam/pose2SLAM.cpp
@@ -85,6 +85,13 @@ namespace pose2SLAM {
     return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
   }
 
+  Values Graph::optimizeSPCG(const Values& initialEstimate) const {
+    LevenbergMarquardtParams params;
+    params.linearSolverType = SuccessiveLinearizationParams::CG;
+    return LevenbergMarquardtOptimizer(*this, initialEstimate, params).optimize();
+  }
+
+
   /* ************************************************************************* */
 
 } // pose2SLAM
diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h
index d7ebd3323..71710f823 100644
--- a/gtsam/slam/pose2SLAM.h
+++ b/gtsam/slam/pose2SLAM.h
@@ -107,6 +107,7 @@ namespace pose2SLAM {
 
     /// Optimize
     Values optimize(const Values& initialEstimate) const;
+    Values optimizeSPCG(const Values& initialEstimate) const;
 
     /// Return a Marginals object
     Marginals marginals(const Values& solution) const {