From 9e278b394a0177a79da5070197af76ca5cb221e7 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 22 Jul 2012 00:57:54 +0000 Subject: [PATCH] Converted first 2 matlab examples to not use slam namespaces --- matlab/examples/LocalizationExample.m | 38 ++++++++++++--------------- matlab/examples/OdometryExample.m | 34 +++++++++++------------- 2 files changed, 32 insertions(+), 40 deletions(-) diff --git a/matlab/examples/LocalizationExample.m b/matlab/examples/LocalizationExample.m index 0a54111b0..1c38bc979 100644 --- a/matlab/examples/LocalizationExample.m +++ b/matlab/examples/LocalizationExample.m @@ -16,51 +16,47 @@ % - The robot is on a grid, moving 2 meters each step %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) -graph = pose2SLAM.Graph; +graph = gtsam.NonlinearFactorGraph; %% Add two odometry factors import gtsam.* odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta -graph.addRelativePose(1, 2, odometry, odometryNoise); -graph.addRelativePose(2, 3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); %% Add three "GPS" measurements import gtsam.* % We use Pose2 Priors here with high variance on theta priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]); -graph.addPosePrior(1, Pose2(0.0, 0.0, 0.0), priorNoise); -graph.addPosePrior(2, Pose2(2.0, 0.0, 0.0), priorNoise); -graph.addPosePrior(3, Pose2(4.0, 0.0, 0.0), priorNoise); +graph.add(PriorFactorPose2(1, Pose2(0.0, 0.0, 0.0), priorNoise)); +graph.add(PriorFactorPose2(2, Pose2(2.0, 0.0, 0.0), priorNoise)); +graph.add(PriorFactorPose2(3, Pose2(4.0, 0.0, 0.0), priorNoise)); %% print graph.print(sprintf('\nFactor graph:\n')); %% Initialize to noisy points import gtsam.* -initialEstimate = pose2SLAM.Values; -initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); +initialEstimate = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); initialEstimate.print(sprintf('\nInitial estimate:\n ')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd import gtsam.* -result = graph.optimize(initialEstimate,1); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n ')); -%% Plot Covariance Ellipses +%% Plot trajectory and covariance ellipses import gtsam.* cla; -X=result.poses(); -plot(X(:,1),X(:,2),'k*-'); hold on -marginals = graph.marginals(result); -P={}; -for i=1:result.size() - pose_i = result.pose(i); - P{i}=marginals.marginalCovariance(i); - plotPose2(pose_i,'g',P{i}) -end +hold on; + +plot2DTrajectory(result, Marginals(graph, result)); + axis([-0.6 4.8 -1 1]) axis equal view(2) diff --git a/matlab/examples/OdometryExample.m b/matlab/examples/OdometryExample.m index 297702b51..16323a616 100644 --- a/matlab/examples/OdometryExample.m +++ b/matlab/examples/OdometryExample.m @@ -16,48 +16,44 @@ % - The robot is on a grid, moving 2 meters each step %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) -graph = pose2SLAM.Graph; +graph = gtsam.NonlinearFactorGraph; %% Add a Gaussian prior on pose x_1 import gtsam.* priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta -graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph +graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add two odometry factors import gtsam.* odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta -graph.addRelativePose(1, 2, odometry, odometryNoise); -graph.addRelativePose(2, 3, odometry, odometryNoise); +graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); %% print graph.print(sprintf('\nFactor graph:\n')); %% Initialize to noisy points import gtsam.* -initialEstimate = pose2SLAM.Values; -initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); +initialEstimate = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insert(3, Pose2(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,1); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); result.print(sprintf('\nFinal result:\n ')); -%% Plot Covariance Ellipses +%% Plot trajectory and covariance ellipses import gtsam.* cla; -X=result.poses(); -plot(X(:,1),X(:,2),'k*-'); hold on -marginals = graph.marginals(result); -P={}; -for i=1:result.size() - pose_i = result.pose(i); - P{i}=marginals.marginalCovariance(i); - plotPose2(pose_i,'g',P{i}) -end +hold on; + +plot2DTrajectory(result, Marginals(graph, result)); + axis([-0.6 4.8 -1 1]) axis equal view(2)