add the spcg example to matlab

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.
+  Values result = graph.optimizeSPCG(initialEstimate);
-  // More detail in the next release.
+  result.print("\nFinal result:\n");
-  LevenbergMarquardtParams param;
-  param.linearSolverType = SuccessiveLinearizationParams::CG;
-  param.iterativeParams = boost::make_shared<IterativeOptimizationParameters>();
-  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, param);
-  Values result = optimizer.optimize();
   cout << "final error = " << graph.error(result) << endl;
 
   return 0 ;

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'));

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;
 };
 

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

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 {