Fixed gtsam_test (except serialize)

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);
+    pose_i = result.atPose2(i);
-    CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.at(i),1e-4));
+    CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.atPose2(i),1e-4));
     P{i}=marginals.marginalCovariance(i);
 end

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));

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));

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));
 

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);
+p0 = hexagon.atPose3(0);
-p1 = hexagon.at(1);
+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(1, hexagon.atPose3(1).retract(s*randn(6,1)));
-initial.insert(2, hexagon.at(2).retract(s*randn(6,1)));
+initial.insert(2, hexagon.atPose3(2).retract(s*randn(6,1)));
-initial.insert(3, hexagon.at(3).retract(s*randn(6,1)));
+initial.insert(3, hexagon.atPose3(3).retract(s*randn(6,1)));
-initial.insert(4, hexagon.at(4).retract(s*randn(6,1)));
+initial.insert(4, hexagon.atPose3(4).retract(s*randn(6,1)));
-initial.insert(5, hexagon.at(5).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));
 
 

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

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));

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

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!'