From c2935c5dd19ae0e1c58782a56897d97c1d64a29f Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Thu, 26 Jul 2012 13:33:40 +0000 Subject: [PATCH] Replaced calls to the namespace Circle functions with the newly added matlab function --- matlab/examples/Pose2SLAMExample_circle.m | 19 ++++++++++--------- matlab/examples/Pose3SLAMExample.m | 19 ++++++++++--------- matlab/tests/testPose3SLAMExample.m | 19 ++++++++++--------- 3 files changed, 30 insertions(+), 27 deletions(-) diff --git a/matlab/examples/Pose2SLAMExample_circle.m b/matlab/examples/Pose2SLAMExample_circle.m index e16edf9cb..3d2265d76 100644 --- a/matlab/examples/Pose2SLAMExample_circle.m +++ b/matlab/examples/Pose2SLAMExample_circle.m @@ -10,13 +10,14 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +import gtsam.* + %% Create a hexagon of poses -hexagon = pose2SLAM.Values.Circle(6,1.0); -p0 = hexagon.pose(0); -p1 = hexagon.pose(1); +hexagon = circlePose2(6,1.0); +p0 = hexagon.at(0); +p1 = hexagon.at(1); %% create a Pose graph with one equality constraint and one measurement -import gtsam.* fg = NonlinearFactorGraph; fg.add(NonlinearEqualityPose2(0, p0)); delta = p0.between(p1); @@ -31,11 +32,11 @@ fg.add(BetweenFactorPose2(5,0, delta, covariance)); %% Create initial config initial = Values; initial.insert(0, p0); -initial.insert(1, hexagon.pose(1).retract([-0.1, 0.1,-0.1]')); -initial.insert(2, hexagon.pose(2).retract([ 0.1,-0.1, 0.1]')); -initial.insert(3, hexagon.pose(3).retract([-0.1, 0.1,-0.1]')); -initial.insert(4, hexagon.pose(4).retract([ 0.1,-0.1, 0.1]')); -initial.insert(5, hexagon.pose(5).retract([-0.1, 0.1,-0.1]')); +initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]')); +initial.insert(2, hexagon.at(2).retract([ 0.1,-0.1, 0.1]')); +initial.insert(3, hexagon.at(3).retract([-0.1, 0.1,-0.1]')); +initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]')); +initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]')); %% Plot Initial Estimate cla diff --git a/matlab/examples/Pose3SLAMExample.m b/matlab/examples/Pose3SLAMExample.m index 86c5c23c8..0d2bd237f 100644 --- a/matlab/examples/Pose3SLAMExample.m +++ b/matlab/examples/Pose3SLAMExample.m @@ -10,13 +10,14 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +import gtsam.* + %% Create a hexagon of poses -hexagon = pose3SLAM.Values.Circle(6,1.0); -p0 = hexagon.pose(0); -p1 = hexagon.pose(1); +hexagon = circlePose3(6,1.0); +p0 = hexagon.at(0); +p1 = hexagon.at(1); %% create a Pose graph with one equality constraint and one measurement -import gtsam.* fg = NonlinearFactorGraph; fg.add(NonlinearEqualityPose3(0, p0)); delta = p0.between(p1); @@ -32,11 +33,11 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance)); initial = Values; s = 0.10; initial.insert(0, p0); -initial.insert(1, hexagon.pose(1).retract(s*randn(6,1))); -initial.insert(2, hexagon.pose(2).retract(s*randn(6,1))); -initial.insert(3, hexagon.pose(3).retract(s*randn(6,1))); -initial.insert(4, hexagon.pose(4).retract(s*randn(6,1))); -initial.insert(5, hexagon.pose(5).retract(s*randn(6,1))); +initial.insert(1, hexagon.at(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.at(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.at(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); %% Plot Initial Estimate cla diff --git a/matlab/tests/testPose3SLAMExample.m b/matlab/tests/testPose3SLAMExample.m index 0d47990e8..dafad4e47 100644 --- a/matlab/tests/testPose3SLAMExample.m +++ b/matlab/tests/testPose3SLAMExample.m @@ -10,13 +10,14 @@ % @author Frank Dellaert %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +import gtsam.* + %% Create a hexagon of poses -hexagon = pose3SLAM.Values.Circle(6,1.0); -p0 = hexagon.pose(0); -p1 = hexagon.pose(1); +hexagon = circlePose3(6,1.0); +p0 = hexagon.at(0); +p1 = hexagon.at(1); %% create a Pose graph with one equality constraint and one measurement -import gtsam.* fg = NonlinearFactorGraph; fg.add(NonlinearEqualityPose3(0, p0)); delta = p0.between(p1); @@ -32,11 +33,11 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance)); initial = Values; s = 0.10; initial.insert(0, p0); -initial.insert(1, hexagon.pose(1).retract(s*randn(6,1))); -initial.insert(2, hexagon.pose(2).retract(s*randn(6,1))); -initial.insert(3, hexagon.pose(3).retract(s*randn(6,1))); -initial.insert(4, hexagon.pose(4).retract(s*randn(6,1))); -initial.insert(5, hexagon.pose(5).retract(s*randn(6,1))); +initial.insert(1, hexagon.at(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.at(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.at(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.at(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.at(5).retract(s*randn(6,1))); %% optimize optimizer = LevenbergMarquardtOptimizer(fg, initial);