diff --git a/matlab/gtsam_tests/testLocalizationExample.m b/matlab/gtsam_tests/testLocalizationExample.m index 5f1d89c99..ed091ea71 100644 --- a/matlab/gtsam_tests/testLocalizationExample.m +++ b/matlab/gtsam_tests/testLocalizationExample.m @@ -29,7 +29,7 @@ groundTruth.insert(2, Pose2(2.0, 0.0, 0.0)); groundTruth.insert(3, Pose2(4.0, 0.0, 0.0)); model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]); for i=1:3 - graph.add(PriorFactorPose2(i, groundTruth.at(i), model)); + graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model)); end %% Initialize to noisy points @@ -46,7 +46,7 @@ result = optimizer.optimizeSafely(); marginals = Marginals(graph, result); P={}; for i=1:result.size() - pose_i = result.at(i); - CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.at(i),1e-4)); + pose_i = result.atPose2(i); + CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.atPose2(i),1e-4)); P{i}=marginals.marginalCovariance(i); end diff --git a/matlab/gtsam_tests/testOdometryExample.m b/matlab/gtsam_tests/testOdometryExample.m index 442b36801..9bd4762a7 100644 --- a/matlab/gtsam_tests/testOdometryExample.m +++ b/matlab/gtsam_tests/testOdometryExample.m @@ -39,5 +39,5 @@ marginals = Marginals(graph, result); marginals.marginalCovariance(1); %% Check first pose equality -pose_1 = result.at(1); +pose_1 = result.atPose2(1); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); diff --git a/matlab/gtsam_tests/testPlanarSLAMExample.m b/matlab/gtsam_tests/testPlanarSLAMExample.m index 7694a1a0b..d3cab5d1f 100644 --- a/matlab/gtsam_tests/testPlanarSLAMExample.m +++ b/matlab/gtsam_tests/testPlanarSLAMExample.m @@ -60,10 +60,10 @@ result = optimizer.optimizeSafely(); marginals = Marginals(graph, result); %% Check first pose and point equality -pose_1 = result.at(symbol('x',1)); +pose_1 = result.atPose2(symbol('x',1)); marginals.marginalCovariance(symbol('x',1)); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); -point_1 = result.at(symbol('l',1)); +point_1 = result.atPoint2(symbol('l',1)); marginals.marginalCovariance(symbol('l',1)); CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4)); diff --git a/matlab/gtsam_tests/testPose2SLAMExample.m b/matlab/gtsam_tests/testPose2SLAMExample.m index 8c70c27e7..72e5331f2 100644 --- a/matlab/gtsam_tests/testPose2SLAMExample.m +++ b/matlab/gtsam_tests/testPose2SLAMExample.m @@ -57,6 +57,6 @@ result = optimizer.optimizeSafely(); marginals = Marginals(graph, result); P = marginals.marginalCovariance(1); -pose_1 = result.at(1); +pose_1 = result.atPose2(1); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); diff --git a/matlab/gtsam_tests/testPose3SLAMExample.m b/matlab/gtsam_tests/testPose3SLAMExample.m index dafad4e47..51ba69240 100644 --- a/matlab/gtsam_tests/testPose3SLAMExample.m +++ b/matlab/gtsam_tests/testPose3SLAMExample.m @@ -14,8 +14,8 @@ import gtsam.* %% Create a hexagon of poses hexagon = circlePose3(6,1.0); -p0 = hexagon.at(0); -p1 = hexagon.at(1); +p0 = hexagon.atPose3(0); +p1 = hexagon.atPose3(1); %% create a Pose graph with one equality constraint and one measurement fg = NonlinearFactorGraph; @@ -33,17 +33,17 @@ fg.add(BetweenFactorPose3(5,0, delta, covariance)); initial = Values; s = 0.10; initial.insert(0, p0); -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))); +initial.insert(1, hexagon.atPose3(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.atPose3(5).retract(s*randn(6,1))); %% optimize optimizer = LevenbergMarquardtOptimizer(fg, initial); result = optimizer.optimizeSafely; -pose_1 = result.at(1); +pose_1 = result.atPose3(1); CHECK('pose_1.equals(Pose3,1e-4)',pose_1.equals(p1,1e-4)); diff --git a/matlab/gtsam_tests/testSFMExample.m b/matlab/gtsam_tests/testSFMExample.m index 2b04be8f1..985cbdb2c 100644 --- a/matlab/gtsam_tests/testSFMExample.m +++ b/matlab/gtsam_tests/testSFMExample.m @@ -63,11 +63,11 @@ marginals.marginalCovariance(symbol('x',1)); %% Check optimized results, should be equal to ground truth for i=1:size(truth.cameras,2) - pose_i = result.at(symbol('x',i)); + pose_i = result.atPose3(symbol('x',i)); CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5)) end for j=1:size(truth.points,2) - point_j = result.at(symbol('p',j)); + point_j = result.atPoint3(symbol('p',j)); CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5)) end diff --git a/matlab/gtsam_tests/testStereoVOExample.m b/matlab/gtsam_tests/testStereoVOExample.m index e2d6f2479..218d0ace1 100644 --- a/matlab/gtsam_tests/testStereoVOExample.m +++ b/matlab/gtsam_tests/testStereoVOExample.m @@ -61,8 +61,8 @@ optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); result = optimizer.optimize(); %% check equality for the first pose and point -pose_x1 = result.at(x1); +pose_x1 = result.atPose3(x1); CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4)); -point_l1 = result.at(l1); +point_l1 = result.atPoint3(l1); CHECK('point_1.equals(expected_l1,1e-4)',point_l1.equals(expected_l1,1e-4)); \ No newline at end of file diff --git a/matlab/gtsam_tests/testVisualISAMExample.m b/matlab/gtsam_tests/testVisualISAMExample.m index 40aca458e..223e823a6 100644 --- a/matlab/gtsam_tests/testVisualISAMExample.m +++ b/matlab/gtsam_tests/testVisualISAMExample.m @@ -45,11 +45,11 @@ for frame_i=3:options.nrCameras end for i=1:size(truth.cameras,2) - pose_i = result.at(symbol('x',i)); + pose_i = result.atPose3(symbol('x',i)); CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5)) end for j=1:size(truth.points,2) - point_j = result.at(symbol('l',j)); + point_j = result.atPoint3(symbol('l',j)); CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5)) end diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index c379179c5..e3705948d 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -30,11 +30,11 @@ testStereoVOExample display 'Starting: testVisualISAMExample' testVisualISAMExample -display 'Starting: testSerialization' -testSerialization - display 'Starting: testUtilities' testUtilities +display 'Starting: testSerialization' +testSerialization + % end of tests display 'Tests complete!'