diff --git a/matlab/examples/LocalizationExample.m b/matlab/examples/LocalizationExample.m new file mode 100644 index 000000000..0e801e4c0 --- /dev/null +++ b/matlab/examples/LocalizationExample.m @@ -0,0 +1,58 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 Example of a simple 2D localization example +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%% Assumptions +% - Robot poses are facing along the X axis (horizontal, to the right in 2D) +% - The robot moves 2 meters each step +% - The robot is on a grid, moving 2 meters each step + +%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) +graph = pose2SLAMGraph; + +%% Add two odometry factors +odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) +odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta +graph.addOdometry(1, 2, odometry, odometryNoise); +graph.addOdometry(2, 3, odometry, odometryNoise); + +%% Add three "GPS" measurements +% We use Pose2 Priors here with high variance on theta +noiseModel = gtsamSharedNoiseModel_Sigmas([0.1; 0.1; 10]); +graph.addPrior(1, gtsamPose2(0.0, 0.0, 0.0), noiseModel); +graph.addPrior(2, gtsamPose2(2.0, 0.0, 0.0), noiseModel); +graph.addPrior(3, gtsamPose2(4.0, 0.0, 0.0), noiseModel); + +%% 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, 0.1)); +initialEstimate.print(sprintf('\nInitial estimate:\n ')); + +%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd +result = graph.optimize(initialEstimate); +result.print(sprintf('\nFinal result:\n ')); + +%% Plot Covariance Ellipses +cla; +plot(result.xs(),result.ys(),'k*-'); hold on +marginals = graph.marginals(result); +for i=1:result.size() + pose_i = result.pose(i); + P{i}=marginals.marginalCovariance(i); + plotPose2(pose_i,'g',P{i}) +end +axis([-0.6 4.8 -1 1]) +axis equal diff --git a/matlab/examples/OdometryExample.m b/matlab/examples/OdometryExample.m index bb9d06c70..68bdf0380 100644 --- a/matlab/examples/OdometryExample.m +++ b/matlab/examples/OdometryExample.m @@ -52,4 +52,5 @@ for i=1:result.size() P{i}=marginals.marginalCovariance(i); plotPose2(pose_i,'g',P{i}) end +axis([-0.6 4.8 -1 1]) axis equal diff --git a/matlab/examples/gtsamExamples.fig b/matlab/examples/gtsamExamples.fig index 39370f3df..e29163e28 100644 Binary files a/matlab/examples/gtsamExamples.fig and b/matlab/examples/gtsamExamples.fig differ diff --git a/matlab/examples/gtsamExamples.m b/matlab/examples/gtsamExamples.m index 4f6cb525c..6cab86732 100644 --- a/matlab/examples/gtsamExamples.m +++ b/matlab/examples/gtsamExamples.m @@ -57,11 +57,6 @@ handles.output = hObject; % Update handles structure guidata(hObject, handles); -% This sets up the initial plot - only do when we are invisible -% so window can get raised using gtsamExamples. -if strcmp(get(hObject,'Visible'),'off') - plot(rand(5)); -end % --- Outputs from this function are returned to the command line. function varargout = gtsamExamples_OutputFcn(hObject, eventdata, handles) @@ -96,7 +91,10 @@ echo off % --- Executes on button press in Localization. function Localization_Callback(hObject, eventdata, handles) -fprintf(1,'LocalizationExample not implemented yet\n'); +axes(handles.axes3); +echo on +LocalizationExample; +echo off % --- Executes on button press in Pose2SLAM. function Pose2SLAM_Callback(hObject, eventdata, handles)