Fixed many utilities and examples

matlab/+gtsam/VisualISAMPlot.m

@@ -16,7 +16,7 @@ gtsam.plot3DPoints(result, [], marginals);
 M = 1;
 while result.exists(symbol('x',M))
     ii = symbol('x',M);
-    pose_i = result.at(ii);
+    pose_i = result.atPose3(ii);
     if options.hardConstraint && (M==1)
         gtsam.plotPose3(pose_i,[],10);
     else

matlab/+gtsam/VisualISAMStep.m

@@ -27,7 +27,7 @@ for k=1:length(data.Z{nextPoseIndex})
 end
 
 %% Initial estimates for the new pose.
-prevPose = result.at(symbol('x',nextPoseIndex-1));
+prevPose = result.atPose3(symbol('x',nextPoseIndex-1));
 initialEstimates.insert(symbol('x',nextPoseIndex), prevPose.compose(odometry));
 
 %% Update ISAM

matlab/+gtsam/load3D.m

@@ -46,9 +46,9 @@ for i=1:n
                 graph.add(BetweenFactorPose3(i1, i2, dpose, model));
                 if successive
                     if i2>i1
-                        initial.insert(i2,initial.at(i1).compose(dpose));
+                        initial.insert(i2,initial.atPose3(i1).compose(dpose));
                     else
-                        initial.insert(i1,initial.at(i2).compose(dpose.inverse));
+                        initial.insert(i1,initial.atPose3(i2).compose(dpose.inverse));
                     end
                 end
             end

matlab/+gtsam/plot2DPoints.m

@@ -18,15 +18,18 @@ hold on
 % Plot points and covariance matrices
 for i = 0:keys.size-1
     key = keys.at(i);
-    p = values.at(key);
+    try
-    if isa(p, 'gtsam.Point2')
+        p = values.atPoint2(key);
         if haveMarginals
             P = marginals.marginalCovariance(key);
             gtsam.plotPoint2(p, linespec, P);
         else
             gtsam.plotPoint2(p, linespec);
         end
+    catch err
+        % I guess it's not a Point2
     end
+    
 end
 
 if ~holdstate

matlab/+gtsam/plot2DTrajectory.m

@@ -33,10 +33,12 @@ else
     keys = KeyVector(values.keys);
     for i = 0:keys.size-1
         key = keys.at(i);
-    x = values.at(key);
+        try
-    if isa(x, 'gtsam.Pose2')
+            x = values.atPose2(key);
             P = marginals.marginalCovariance(key);
             gtsam.plotPose2(x,linespec(1), P);
+        catch err
+            % I guess it's not a Pose2
         end
     end
 end

matlab/+gtsam/plot3DPoints.m

@@ -18,14 +18,16 @@ hold on
 % Plot points and covariance matrices
 for i = 0:keys.size-1
     key = keys.at(i);
-    p = values.at(key);
+    try
-    if isa(p, 'gtsam.Point3')
+        p = values.atPoint3(key);
         if haveMarginals
             P = marginals.marginalCovariance(key);
             gtsam.plotPoint3(p, linespec, P);
         else
             gtsam.plotPoint3(p, linespec);
         end
+    catch
+        % I guess it's not a Point3
     end
 end
 

matlab/+gtsam/plot3DTrajectory.m

@@ -17,13 +17,14 @@ hold on
 lastIndex = [];
 for i = 0:keys.size-1
     key = keys.at(i);
-    x = values.at(key);
+    try
-    if isa(x, 'gtsam.Pose3')
+        x = values.atPose3(key);
         if ~isempty(lastIndex)
             % Draw line from last pose then covariance ellipse on top of
             % last pose.
             lastKey = keys.at(lastIndex);
-            lastPose = values.at(lastKey);
+            try
+                lastPose = values.atPose3(lastKey);
                 plot3([ x.x; lastPose.x ], [ x.y; lastPose.y ], [ x.z; lastPose.z ], linespec);
                 if haveMarginals
                     P = marginals.marginalCovariance(lastKey);
@@ -31,21 +32,29 @@ for i = 0:keys.size-1
                     P = [];
                 end
                 gtsam.plotPose3(lastPose, P, scale);
+            catch err
+                warning(['no Pose3 at ' lastKey]);
             end
             lastIndex = i;
         end
+    catch
+        warning(['no Pose3 at ' key]);
     end
     
     % Draw final pose
     if ~isempty(lastIndex)
         lastKey = keys.at(lastIndex);
-    lastPose = values.at(lastKey);
+        try
+            lastPose = values.atPose3(lastKey);
             if haveMarginals
                 P = marginals.marginalCovariance(lastKey);
             else
                 P = [];
             end
             gtsam.plotPose3(lastPose, P, scale);
+        catch
+            warning(['no Pose3 at ' lastIndex]);
+        end
     end
     
     if ~holdstate

matlab/gtsam_examples/PlanarSLAMExample.m

@@ -71,9 +71,9 @@ marginals = Marginals(graph, result);
 plot2DTrajectory(result, [], marginals);
 plot2DPoints(result, 'b', marginals);
 
-plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-');
+plot([result.atPose2(i1).x; result.atPoint2(j1).x],[result.atPose2(i1).y; result.atPoint2(j1).y], 'c-');
-plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-');
+plot([result.atPose2(i2).x; result.atPoint2(j1).x],[result.atPose2(i2).y; result.atPoint2(j1).y], 'c-');
-plot([result.at(i3).x; result.at(j2).x],[result.at(i3).y; result.at(j2).y], 'c-');
+plot([result.atPose2(i3).x; result.atPoint2(j2).x],[result.atPose2(i3).y; result.atPoint2(j2).y], 'c-');
 axis([-0.6 4.8 -1 1])
 axis equal
 view(2)

matlab/gtsam_examples/PlanarSLAMExample_graph.m

@@ -22,7 +22,7 @@ model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]);
 [graph,initial] = load2D(datafile, model);
 
 %% Add a Gaussian prior on a pose in the middle
-priorMean = initial.at(40);
+priorMean = initial.atPose2(40);
 priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 2*pi/180]);
 graph.add(PriorFactorPose2(40, priorMean, priorNoise)); % add directly to graph
 

matlab/gtsam_examples/PlanarSLAMExample_sampling.m

@@ -55,14 +55,14 @@ plot2DPoints(sample, [], marginals);
 
 for j=1:2
     key = symbol('l',j);
-    point{j} = sample.at(key);
+    point{j} = sample.atPoint2(key);
     Q{j}=marginals.marginalCovariance(key);
     S{j}=chol(Q{j}); % for sampling
 end
 
-plot([sample.at(i1).x; sample.at(j1).x],[sample.at(i1).y; sample.at(j1).y], 'c-');
+plot([sample.atPose2(i1).x; sample.atPoint2(j1).x],[sample.atPose2(i1).y; sample.atPoint2(j1).y], 'c-');
-plot([sample.at(i2).x; sample.at(j1).x],[sample.at(i2).y; sample.at(j1).y], 'c-');
+plot([sample.atPose2(i2).x; sample.atPoint2(j1).x],[sample.atPose2(i2).y; sample.atPoint2(j1).y], 'c-');
-plot([sample.at(i3).x; sample.at(j2).x],[sample.at(i3).y; sample.at(j2).y], 'c-');
+plot([sample.atPose2(i3).x; sample.atPoint2(j2).x],[sample.atPose2(i3).y; sample.atPoint2(j2).y], 'c-');
 view(2); axis auto; axis equal
 
 %% Do Sampling on point 2

matlab/gtsam_examples/Pose2SLAMExample.m

@@ -57,7 +57,7 @@ result.print(sprintf('\nFinal result:\n'));
 %% Plot Covariance Ellipses
 cla;
 hold on
-plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-');
+plot([result.atPose2(5).x;result.atPose2(2).x],[result.atPose2(5).y;result.atPose2(2).y],'r-');
 marginals = Marginals(graph, result);
 
 plot2DTrajectory(result, [], marginals);

matlab/gtsam_examples/Pose2SLAMExample_circle.m

@@ -14,8 +14,8 @@ import gtsam.*
 
 %% Create a hexagon of poses
 hexagon = circlePose2(6,1.0);
-p0 = hexagon.at(0);
+p0 = hexagon.atPose2(0);
-p1 = hexagon.at(1);
+p1 = hexagon.atPose2(1);
 
 %% create a Pose graph with one equality constraint and one measurement
 fg = NonlinearFactorGraph;
@@ -32,11 +32,11 @@ fg.add(BetweenFactorPose2(5,0, delta, covariance));
 %% Create initial config
 initial = Values;
 initial.insert(0, p0);
-initial.insert(1, hexagon.at(1).retract([-0.1, 0.1,-0.1]'));
+initial.insert(1, hexagon.atPose2(1).retract([-0.1, 0.1,-0.1]'));
-initial.insert(2, hexagon.at(2).retract([ 0.1,-0.1, 0.1]'));
+initial.insert(2, hexagon.atPose2(2).retract([ 0.1,-0.1, 0.1]'));
-initial.insert(3, hexagon.at(3).retract([-0.1, 0.1,-0.1]'));
+initial.insert(3, hexagon.atPose2(3).retract([-0.1, 0.1,-0.1]'));
-initial.insert(4, hexagon.at(4).retract([ 0.1,-0.1, 0.1]'));
+initial.insert(4, hexagon.atPose2(4).retract([ 0.1,-0.1, 0.1]'));
-initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]'));
+initial.insert(5, hexagon.atPose2(5).retract([-0.1, 0.1,-0.1]'));
 
 %% Plot Initial Estimate
 cla

matlab/gtsam_examples/Pose2SLAMExample_graph.m

@@ -41,7 +41,7 @@ marginals = Marginals(graph, result);
 toc
 P={};
 for i=1:result.size()-1
-    pose_i = result.at(i);
+    pose_i = result.atPose2(i);
     P{i}=marginals.marginalCovariance(i);
     plotPose2(pose_i,'b',P{i})
 end

matlab/gtsam_examples/Pose3SLAMExample.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,11 +33,11 @@ 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)));
 
 %% Plot Initial Estimate
 cla

matlab/gtsam_examples/Pose3SLAMExample_graph.m

@@ -28,7 +28,7 @@ model = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.
 
 %% Plot Initial Estimate
 cla
-first = initial.at(0);
+first = initial.atPose3(0);
 plot3(first.x(),first.y(),first.z(),'r*'); hold on
 plot3DTrajectory(initial,'g-',false);
 drawnow;

matlab/gtsam_examples/StereoVOExample_large.m

@@ -45,7 +45,7 @@ for i=1:size(measurements,1)
     if ~initial.exists(symbol('l',sf(2)))
         % 3D landmarks are stored in camera coordinates: transform
         % to world coordinates using the respective initial pose
-        pose = initial.at(symbol('x', sf(1)));
+        pose = initial.atPose3(symbol('x', sf(1)));
         world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8)));
         initial.insert(symbol('l',sf(2)), world_point);
     end
@@ -54,7 +54,7 @@ toc
 
 %% add a constraint on the starting pose
 key = symbol('x',1);
-first_pose = initial.at(key);
+first_pose = initial.atPose3(key);
 graph.add(NonlinearEqualityPose3(key, first_pose));
 
 %% optimize

wrap/StaticMethod.h

@@ -42,7 +42,7 @@ struct StaticMethod: public Function, public SignatureOverloads {
 
   // emit a list of comments, one for each overload
   void comment_fragment(FileWriter& proxyFile) const {
-    SignatureOverloads::comment_fragment(proxyFile, name_);
+    SignatureOverloads::comment_fragment(proxyFile, matlabName());
   }
 
   void verifyArguments(const std::vector<std::string>& validArgs) const {