Plot marginals, sample

examples/matlab/PlanarSLAMExample.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
@@ -20,56 +20,75 @@
 %  - Landmarks are 2 meters away from the robot trajectory
 
 %% Create keys for variables
-x1 = symbol('x',1); x2 = symbol('x',2); x3 = symbol('x',3);
+i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
-l1 = symbol('l',1); l2 = symbol('l',2);
+j1 = symbol('l',1); j2 = symbol('l',2);
 
 %% Create graph container and add factors to it
 graph = planarSLAMGraph;
 
 %% 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
-graph.addPrior(x1, priorMean, priorNoise); % add directly to graph
+priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
+graph.addPrior(i1, priorMean, priorNoise); % add directly to graph
 
 %% Add odometry
-% general noisemodel for odometry
+odometry = gtsamPose2(2.0, 0.0, 0.0);
 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(i1, i2, odometry, odometryNoise);
-graph.addOdometry(x1, x2, odometry, odometryNoise);
+graph.addOdometry(i2, i3, odometry, odometryNoise);
-graph.addOdometry(x2, x3, odometry, odometryNoise);
 
-%% Add measurements
+%% Add bearing/range measurement factors
-% general noisemodel for measurements
-meas_model = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]);
-
-% create the measurement values - indices are (pose id, landmark id)
 degrees = pi/180;
-bearing11 = gtsamRot2(45*degrees);
+noiseModel = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]);
-bearing21 = gtsamRot2(90*degrees);
+if 1
-bearing32 = gtsamRot2(90*degrees);
+    graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel);
-range11 = sqrt(4+4);
+    graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel);
-range21 = 2.0;
+else
-range32 = 2.0;
+    bearingModel = gtsamSharedNoiseModel_Sigmas(0.1);    
-
+    graph.addBearing(i1, j1, gtsamRot2(45*degrees), bearingModel);
-% % create bearing/range factors and add them
+    graph.addBearing(i2, j1, gtsamRot2(90*degrees), bearingModel);
-graph.addBearingRange(x1, l1, bearing11, range11, meas_model);
+end
-graph.addBearingRange(x2, l1, bearing21, range21, meas_model);
+graph.addBearingRange(i3, j2, gtsamRot2(90*degrees), 2, noiseModel);
-graph.addBearingRange(x3, l2, bearing32, range32, meas_model);
 
 % print
-graph.print('full graph');
+graph.print(sprintf('\nFull graph:\n'));
 
 %% Initialize to noisy points
 initialEstimate = planarSLAMValues;
-initialEstimate.insertPose(x1, gtsamPose2(0.5, 0.0, 0.2));
+initialEstimate.insertPose(i1, gtsamPose2(0.5, 0.0, 0.2));
-initialEstimate.insertPose(x2, gtsamPose2(2.3, 0.1,-0.2));
+initialEstimate.insertPose(i2, gtsamPose2(2.3, 0.1,-0.2));
-initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1));
+initialEstimate.insertPose(i3, gtsamPose2(4.1, 0.1, 0.1));
-initialEstimate.insertPoint(l1, gtsamPoint2(1.8, 2.1));
+initialEstimate.insertPoint(j1, gtsamPoint2(1.8, 2.1));
-initialEstimate.insertPoint(l2, gtsamPoint2(4.1, 1.8));
+initialEstimate.insertPoint(j2, gtsamPoint2(4.1, 1.8));
 
-initialEstimate.print('initial estimate');
+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'));
+
+%% Plot Covariance Ellipses
+figure(1);clf;hold on
+marginals = graph.marginals(result);
+for i=1:3
+    key = symbol('x',i);
+    pose{i} = result.pose(key);
+    P{i}=marginals.marginalCovariance(key);
+    if i>1
+        plot([pose{i-1}.x;pose{i}.x],[pose{i-1}.y;pose{i}.y],'r-');
+    end
+end
+for i=1:3
+    plotPose2(pose{i},'g',P{i})
+end
+for j=1:2
+    key = symbol('l',j);
+    point{j} = result.point(key);
+    Q{j}=marginals.marginalCovariance(key);
+    plotPoint2(point{j},'b',Q{j})
+end
+plot([pose{1}.x;point{1}.x],[pose{1}.y;point{1}.y],'c-');
+plot([pose{2}.x;point{1}.x],[pose{2}.y;point{1}.y],'c-');
+plot([pose{3}.x;point{2}.x],[pose{3}.y;point{2}.y],'c-');
+axis equal
+

examples/matlab/PlanarSLAMExample_sampling.m

@@ -0,0 +1,79 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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
+% @author Alex Cunningham
+% @author Frank Dellaert
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% Create the same factor graph as in PlanarSLAMExample
+i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
+graph = planarSLAMGraph;
+priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
+priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
+graph.addPrior(i1, priorMean, priorNoise); % add directly to graph
+odometry = gtsamPose2(2.0, 0.0, 0.0);
+odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
+graph.addOdometry(i1, i2, odometry, odometryNoise);
+graph.addOdometry(i2, i3, odometry, odometryNoise);
+
+%% Except, for measurements we offer a choice
+j1 = symbol('l',1); j2 = symbol('l',2);
+degrees = pi/180;
+noiseModel = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]);
+if 1
+    graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel);
+    graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel);
+else
+    bearingModel = gtsamSharedNoiseModel_Sigmas(0.1);    
+    graph.addBearing(i1, j1, gtsamRot2(45*degrees), bearingModel);
+    graph.addBearing(i2, j1, gtsamRot2(90*degrees), bearingModel);
+end
+graph.addBearingRange(i3, j2, gtsamRot2(90*degrees), 2, noiseModel);
+
+%% Initialize MCMC sampler with ground truth
+sample = planarSLAMValues;
+sample.insertPose(i1, gtsamPose2(0,0,0));
+sample.insertPose(i2, gtsamPose2(2,0,0));
+sample.insertPose(i3, gtsamPose2(4,0,0));
+sample.insertPoint(j1, gtsamPoint2(2,2));
+sample.insertPoint(j2, gtsamPoint2(4,2));
+
+%% Calculate and plot Covariance Ellipses
+figure(1);clf;hold on
+marginals = graph.marginals(sample);
+for i=1:3
+    key = symbol('x',i);
+    pose{i} = sample.pose(key);
+    P{i}=marginals.marginalCovariance(key);
+    if i>1
+        plot([pose{i-1}.x;pose{i}.x],[pose{i-1}.y;pose{i}.y],'r-');
+    end
+end
+for i=1:3
+    plotPose2(pose{i},'g',P{i})
+end
+for j=1:2
+    key = symbol('l',j);
+    point{j} = sample.point(key);
+    Q{j}=marginals.marginalCovariance(key);
+    S{j}=chol(Q{j}); % for sampling
+    plotPoint2(point{j},'b',Q{j})
+end
+plot([pose{1}.x;point{1}.x],[pose{1}.y;point{1}.y],'c-');
+plot([pose{2}.x;point{1}.x],[pose{2}.y;point{1}.y],'c-');
+plot([pose{3}.x;point{2}.x],[pose{3}.y;point{2}.y],'c-');
+axis equal
+
+%% Do Sampling on point 2
+N=1000;
+for s=1:N
+    delta = S{2}*randn(2,1);
+    proposedPoint = gtsamPoint2(point{2}.x+delta(1),point{2}.y+delta(2));
+    plotPoint2(proposedPoint,'k.')
+end

examples/matlab/plotPoint2.m

@@ -0,0 +1,11 @@
+function plotPoint2(p,color,P)
+% plotPose2: show a Pose2, possibly with covariance matrix
+if size(color,2)==1
+    plot(p.x,p.y,[color '*']);
+else
+    plot(p.x,p.y,color);
+end
+if nargin>2
+    pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame
+    covarianceEllipse([p.x;p.y],pPp,color(1));
+end

examples/matlab/plotPose2.m

@@ -1,6 +1,6 @@
 function plotPose2(p,color,P)
 % plotPose2: show a Pose2, possibly with covariance matrix
-plot(p.x,p.y,[color '.']);
+plot(p.x,p.y,[color '*']);
 c = cos(p.theta);
 s = sin(p.theta);
 quiver(p.x,p.y,c,s,0.1,color);