diff --git a/examples/matlab/PlanarSLAMExample.m b/examples/matlab/PlanarSLAMExample.m index 88a988755..4addd23dc 100644 --- a/examples/matlab/PlanarSLAMExample.m +++ b/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); -l1 = symbol('l',1); l2 = symbol('l',2); +i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); +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(x1, x2, odometry, odometryNoise); -graph.addOdometry(x2, x3, odometry, odometryNoise); +graph.addOdometry(i1, i2, odometry, odometryNoise); +graph.addOdometry(i2, i3, odometry, odometryNoise); -%% Add measurements -% general noisemodel for measurements -meas_model = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]); - -% create the measurement values - indices are (pose id, landmark id) +%% Add bearing/range measurement factors degrees = pi/180; -bearing11 = gtsamRot2(45*degrees); -bearing21 = gtsamRot2(90*degrees); -bearing32 = gtsamRot2(90*degrees); -range11 = sqrt(4+4); -range21 = 2.0; -range32 = 2.0; - -% % create bearing/range factors and add them -graph.addBearingRange(x1, l1, bearing11, range11, meas_model); -graph.addBearingRange(x2, l1, bearing21, range21, meas_model); -graph.addBearingRange(x3, l2, bearing32, range32, meas_model); +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); % 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(x2, gtsamPose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1)); -initialEstimate.insertPoint(l1, gtsamPoint2(1.8, 2.1)); -initialEstimate.insertPoint(l2, gtsamPoint2(4.1, 1.8)); +initialEstimate.insertPose(i1, gtsamPose2(0.5, 0.0, 0.2)); +initialEstimate.insertPose(i2, gtsamPose2(2.3, 0.1,-0.2)); +initialEstimate.insertPose(i3, gtsamPose2(4.1, 0.1, 0.1)); +initialEstimate.insertPoint(j1, gtsamPoint2(1.8, 2.1)); +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 + diff --git a/examples/matlab/PlanarSLAMExample_sampling.m b/examples/matlab/PlanarSLAMExample_sampling.m new file mode 100644 index 000000000..178e48a42 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/matlab/plotPoint2.m b/examples/matlab/plotPoint2.m new file mode 100644 index 000000000..ae45455f6 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/examples/matlab/plotPose2.m b/examples/matlab/plotPose2.m index 7dbdebf5a..17959ea59 100644 --- a/examples/matlab/plotPose2.m +++ b/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);