Synchronized C++ and MATLAB

examples/Pose2SLAMExample.cpp

@@ -47,18 +47,18 @@ int main(int argc, char** argv) {
 	// print
 	graph.print("\nFactor graph:\n");
 
-  // 3. Create the data structure to hold the initialEstimate estinmate to the solution
+  // 3. Create the data structure to hold the initialEstimate estimate to the solution
 	pose2SLAM::Values initialEstimate;
 	Pose2 x1(0.5, 0.0, 0.2   ); initialEstimate.insertPose(1, x1);
 	Pose2 x2(2.3, 0.1,-0.2   ); initialEstimate.insertPose(2, x2);
 	Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insertPose(3, x3);
 	Pose2 x4(4.0, 2.0, M_PI  ); initialEstimate.insertPose(4, x4);
 	Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insertPose(5, x5);
-	initialEstimate.print("\nInitial estimate:\n  ");
+	initialEstimate.print("\nInitial estimate:\n");
 
 	// 4. Single Step Optimization using Levenberg-Marquardt
 	pose2SLAM::Values result = graph.optimize(initialEstimate);
-	result.print("\nFinal result:\n  ");
+	result.print("\nFinal result:\n");
 
 	return 0;
 }

examples/matlab/Pose2SLAMExample_easy.m

@@ -1,9 +1,9 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-% GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+% 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 robotics example using the pre-built planar SLAM domain
@@ -23,32 +23,34 @@ graph = pose2SLAMGraph;
 
 %% Add prior
 % gaussian for prior
-priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
 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]);
-odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
+graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
-graph.addOdometry(1, 2, odometry, odometryNoise);
+graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
-graph.addOdometry(2, 3, odometry, 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 measurements
+%% Add pose constraint
-% general noisemodel for measurements
+constraintUncertainty = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
-meas_model = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]);
+graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), constraintUncertainty);
 
 % print
-graph.print('full graph');
+graph.print(sprintf('/nFactor graph:\n'));
 
 %% Initialize to noisy points
 initialEstimate = pose2SLAMValues;
-initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2));
+x1 = gtsamPose2(0.5, 0.0, 0.2 ); initialEstimate.insertPose(1, x1);
-initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2));
+x2 = gtsamPose2(2.3, 0.1,-0.2 ); initialEstimate.insertPose(2, x2);
-initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
+x3 = gtsamPose2(4.1, 0.1, pi/2); initialEstimate.insertPose(3, x3);
-
+x4 = gtsamPose2(4.0, 2.0, pi  ); initialEstimate.insertPose(4, x4);
-initialEstimate.print('initial estimate');
+x5 = gtsamPose2(2.1, 2.1,-pi/2); initialEstimate.insertPose(5, x5);
+initialEstimate.print(sprintf('\nInitial estimate:\n'));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
 result = graph.optimize(initialEstimate);
-result.print('final result');
+result.print(sprintf('\nFinal result:\n'));

examples/matlab/Pose2SLAMExample_graph.m

@@ -13,7 +13,7 @@
 %% Initialize graph, initial estimate, and odometry noise
 model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 5*pi/180]);
 [graph,initial]=load2D('../Data/w100-odom.graph',model);
-initial.print('Initial estimate:\n');
+initial.print(sprintf('Initial estimate:\n'));
 
 %% Add a Gaussian prior on pose x_1
 priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin
@@ -27,7 +27,7 @@ plot(initial.xs(),initial.ys(),'g-*'); axis equal
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
 result = graph.optimize(initial);
 hold on; plot(result.xs(),result.ys(),'b-*')
-result.print('\nFinal result:\n');
+result.print(sprintf('\nFinal result:\n'));
 
 %% Plot Covariance Ellipses
 marginals = graph.marginals(result);