From eecafcebe74f6ed5554e198ca8541031fb6f40f9 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Fri, 22 Jun 2012 23:40:04 +0000
Subject: [PATCH] Fixing Pose2SLAMwSPCG and make a unittest in testPose2SLAM
 for optimizeSPCG(). Why does SPCG need a constraint between 2 poses to work?
 GaussNewton still works fine without the constraint!

---
 examples/Pose2SLAMwSPCG.cpp         |  5 +++++
 gtsam/slam/pose2SLAM.cpp            |  2 ++
 gtsam/slam/tests/testPose2SLAM.cpp  | 31 +++++++++++++++++++++++++++++
 matlab/tests/testPose2SLAMExample.m |  4 ++++
 4 files changed, 42 insertions(+)

diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp
index 53566e7e8..76f51e366 100644
--- a/examples/Pose2SLAMwSPCG.cpp
+++ b/examples/Pose2SLAMwSPCG.cpp
@@ -85,5 +85,10 @@ int main(void) {
     cout << "subgraph solver final error = " << graph.error(result) << endl;
   }
 
+  {
+  	Values result = graph.optimizeSPCG(initialEstimate);
+  	result.print("\nFinal result:\n");
+  }
+
   return 0 ;
 }
diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp
index 110a50551..c0a63e53d 100644
--- a/gtsam/slam/pose2SLAM.cpp
+++ b/gtsam/slam/pose2SLAM.cpp
@@ -15,6 +15,7 @@
  *  @author Frank Dellaert
  **/
 
+#include <gtsam/linear/SimpleSPCGSolver.h>
 #include <gtsam/slam/pose2SLAM.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
@@ -88,6 +89,7 @@ namespace pose2SLAM {
   Values Graph::optimizeSPCG(const Values& initialEstimate) const {
     LevenbergMarquardtParams params;
     params.linearSolverType = SuccessiveLinearizationParams::CG;
+    params.iterativeParams = boost::make_shared<SimpleSPCGSolverParameters>();
     return LevenbergMarquardtOptimizer(*this, initialEstimate, params).optimize();
   }
 
diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp
index 96f5dad03..923a67031 100644
--- a/gtsam/slam/tests/testPose2SLAM.cpp
+++ b/gtsam/slam/tests/testPose2SLAM.cpp
@@ -17,6 +17,7 @@
 #include <gtsam/slam/pose2SLAM.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/inference/FactorGraph.h>
 #include <gtsam/base/numericalDerivative.h>
 using namespace gtsam;
@@ -190,6 +191,36 @@ TEST_UNSAFE(Pose2SLAM, optimize) {
 	EQUALITY(expectedP1, actualP1);
 }
 
+/* ************************************************************************* */
+TEST_UNSAFE(Pose2SLAM, optimizeSPCG) {
+
+	// create a Pose graph with one equality constraint and one measurement
+  pose2SLAM::Graph fg;
+  fg.addPrior(0, Pose2(0,0,0), noiseModel::Diagonal::Sigmas(Vector_(3,3.0,3.0,1.0)));
+  fg.addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance);
+
+  // [Duy] For some unknown reason, SPCG needs this constraint to work. GaussNewton doesn't need this.
+  fg.addConstraint(0, 1, Pose2(1,2,M_PI_2), noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)));
+
+  // Create initial config
+  Values initial;
+  initial.insert(0, Pose2(0,0,0));
+  initial.insert(1, Pose2(0,0,0));
+
+  // Optimize using SPCG
+  Values actual = fg.optimizeSPCG(initial);
+
+  // Try GaussNewton without the above constraint to see if the problem is underconstrained. Still works!
+  Values actual2 = GaussNewtonOptimizer(fg, initial).optimize();
+
+  // Check with expected config
+  Values expected;
+  expected.insert(0, Pose2(0,0,0));
+  expected.insert(1, Pose2(1,2,M_PI_2));
+  CHECK(assert_equal(expected, actual));
+  CHECK(assert_equal(expected, actual2));
+}
+
 /* ************************************************************************* */
 // test optimization with 3 poses, note we use plain Keys here, not symbols
 TEST_UNSAFE(Pose2SLAM, optimizeThreePoses) {
diff --git a/matlab/tests/testPose2SLAMExample.m b/matlab/tests/testPose2SLAMExample.m
index bf4f261e7..707089fea 100644
--- a/matlab/tests/testPose2SLAMExample.m
+++ b/matlab/tests/testPose2SLAMExample.m
@@ -49,6 +49,7 @@ initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
 result = graph.optimize(initialEstimate);
+resultSPCG = graph.optimizeSPCG(initialEstimate);
 
 %% Plot Covariance Ellipses
 marginals = graph.marginals(result);
@@ -57,4 +58,7 @@ P = marginals.marginalCovariance(1);
 pose_1 = result.pose(1);
 CHECK('pose_1.equals(gtsamPose2,1e-4)',pose_1.equals(gtsamPose2,1e-4));
 
+poseSPCG_1 = resultSPCG.pose(1);
+CHECK('poseSPCG_1.equals(gtsamPose2,1e-4)',poseSPCG_1.equals(gtsamPose2,1e-4));
+