Moved all in gtsam_utils to gtsam namespace

matlab/+gtsam/VisualISAMPlot.m

@@ -9,7 +9,7 @@ hold on;
 %% Plot points
 % Can't use data because current frame might not see all points
 marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); % TODO - this is slow
-gtsam_utils.plot3DPoints(result, [], marginals);
+gtsam.plot3DPoints(result, [], marginals);
 
 %% Plot cameras
 import gtsam.*
@@ -18,13 +18,13 @@ while result.exists(symbol('x',M))
     ii = symbol('x',M);
     pose_i = result.at(ii);
     if options.hardConstraint && (M==1)
-        gtsam_utils.plotPose3(pose_i,[],10);
+        gtsam.plotPose3(pose_i,[],10);
     else
         P = marginals.marginalCovariance(ii);
-        gtsam_utils.plotPose3(pose_i,P,10);
+        gtsam.plotPose3(pose_i,P,10);
     end
     if options.drawTruePoses % show ground truth
-        gtsam_utils.plotPose3(truth.cameras{M}.pose,[],10);
+        gtsam.plotPose3(truth.cameras{M}.pose,[],10);
     end
     
     M = M + options.cameraInterval;

matlab/+gtsam/plot2DPoints.m

@@ -22,9 +22,9 @@ for i = 0:keys.size-1
     if isa(p, 'gtsam.Point2')
         if haveMarginals
             P = marginals.marginalCovariance(key);
-            gtsam_utils.plotPoint2(p, linespec, P);
+            gtsam.plotPoint2(p, linespec, P);
         else
-            gtsam_utils.plotPoint2(p, linespec);
+            gtsam.plotPoint2(p, linespec);
         end
     end
 end

matlab/+gtsam/plot2DTrajectory.m

@@ -31,7 +31,7 @@ for i = 0:keys.size-1
             plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec);
             if haveMarginals
                 P = marginals.marginalCovariance(lastKey);
-                gtsam_utils.plotPose2(lastPose, 'g', P);
+                gtsam.plotPose2(lastPose, 'g', P);
             end
         end
         lastIndex = i;
@@ -43,7 +43,7 @@ if ~isempty(lastIndex) && haveMarginals
     lastKey = keys.at(lastIndex);
     lastPose = values.at(lastKey);
     P = marginals.marginalCovariance(lastKey);
-    gtsam_utils.plotPose2(lastPose, 'g', P);
+    gtsam.plotPose2(lastPose, 'g', P);
 end
 
 if ~holdstate

matlab/+gtsam/plot3DPoints.m

@@ -22,9 +22,9 @@ for i = 0:keys.size-1
     if isa(p, 'gtsam.Point3')
         if haveMarginals
             P = marginals.marginalCovariance(key);
-            gtsam_utils.plotPoint3(p, linespec, P);
+            gtsam.plotPoint3(p, linespec, P);
         else
-            gtsam_utils.plotPoint3(p, linespec);
+            gtsam.plotPoint3(p, linespec);
         end
     end
 end

matlab/+gtsam/plot3DTrajectory.m

@@ -28,7 +28,7 @@ for i = 0:keys.size-1
             else
                 P = [];
             end
-            gtsam_utils.plotPose3(lastPose, P, scale);
+            gtsam.plotPose3(lastPose, P, scale);
         end
         lastIndex = i;
     end
@@ -43,7 +43,7 @@ if ~isempty(lastIndex)
     else
         P = [];
     end
-    gtsam_utils.plotPose3(lastPose, P, scale);
+    gtsam.plotPose3(lastPose, P, scale);
 end
 
 if ~holdstate

matlab/+gtsam/plotPoint2.m

@@ -6,5 +6,5 @@ else
     plot(p.x,p.y,color);
 end
 if exist('P', 'var')
-    gtsam_utils.covarianceEllipse([p.x;p.y],P,color(1));
+    gtsam.covarianceEllipse([p.x;p.y],P,color(1));
 end

matlab/+gtsam/plotPoint3.m

@@ -6,7 +6,7 @@ else
     plot3(p.x,p.y,p.z,color);
 end
 if exist('P', 'var')
-    gtsam_utils.covarianceEllipse3D([p.x;p.y;p.z],P);
+    gtsam.covarianceEllipse3D([p.x;p.y;p.z],P);
 end
 
 end

matlab/+gtsam/plotPose2.m

@@ -9,5 +9,5 @@ quiver(pose.x,pose.y,c,s,axisLength,color);
 if nargin>2
     pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame    
     gRp = [c -s;s c]; % rotation from pose to global
-    gtsam_utils.covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color);
+    gtsam.covarianceEllipse([pose.x;pose.y],gRp*pPp*gRp',color);
 end

matlab/+gtsam/plotPose3.m

@@ -25,7 +25,7 @@ end
 if (nargin>2) && (~isempty(P))
     pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame    
     gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
-    gtsam_utils.covarianceEllipse3D(C,gPp);  
+    gtsam.covarianceEllipse3D(C,gPp);  
 end
     
 end

matlab/+gtsam_utils/load2D.m

@@ -1,31 +0,0 @@
-function [graph,initial] = load2D(filename,model)
-% load2D: read TORO pose graph
-% cannot read noise model from file yet, uses specified model
-
-import gtsam.*
-
-fid = fopen(filename);
-if fid < 0
-    error(['load2D: Cannot open file ' filename]);
-end
-
-% scan all lines into a cell array
-columns=textscan(fid,'%s','delimiter','\n');
-fclose(fid);
-lines=columns{1};
-
-% loop over lines and add vertices
-graph = NonlinearFactorGraph;
-initial = Values;
-n=size(lines,1);
-for i=1:n
-    line_i=lines{i};
-    if strcmp('VERTEX2',line_i(1:7))
-        v = textscan(line_i,'%s %d %f %f %f',1);
-        initial.insert(v{2}, Pose2(v{3}, v{4}, v{5}));
-    elseif strcmp('EDGE2',line_i(1:5))
-        e = textscan(line_i,'%s %d %d %f %f %f',1);
-        graph.add(BetweenFactorPose2(e{2}, e{3}, Pose2(e{4}, e{5}, e{6}), model));
-    end
-end
-

matlab/CMakeLists.txt

@@ -20,7 +20,7 @@ install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "
 
 # Utilities
 message(STATUS "Installing Matlab Toolbox Utilities")
-install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam_utils" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m")
+install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m")
 
 message(STATUS "Installing Matlab Toolbox Examples (Data)")
 # Data files: *.graph and *.txt

matlab/gtsam_examples/LocalizationExample.m

@@ -55,7 +55,7 @@ import gtsam.*
 cla;
 hold on;
 
-gtsam_utils.plot2DTrajectory(result, [], Marginals(graph, result));
+gtsam.plot2DTrajectory(result, [], Marginals(graph, result));
 
 axis([-0.6 4.8 -1 1])
 axis equal

matlab/gtsam_examples/OdometryExample.m

@@ -52,7 +52,7 @@ import gtsam.*
 cla;
 hold on;
 
-gtsam_utils.plot2DTrajectory(result, [], Marginals(graph, result));
+gtsam.plot2DTrajectory(result, [], Marginals(graph, result));
 
 axis([-0.6 4.8 -1 1])
 axis equal

matlab/gtsam_examples/PlanarSLAMExample.m

@@ -72,8 +72,8 @@ import gtsam.*
 cla;hold on
 
 marginals = Marginals(graph, result);
-gtsam_utils.plot2DTrajectory(result, [], marginals);
+gtsam.plot2DTrajectory(result, [], marginals);
-gtsam_utils.plot2DPoints(result, [], marginals);
+gtsam.plot2DPoints(result, [], marginals);
 
 plot([result.at(i1).x; result.at(j1).x],[result.at(i1).y; result.at(j1).y], 'c-');
 plot([result.at(i2).x; result.at(j1).x],[result.at(i2).y; result.at(j1).y], 'c-');

matlab/gtsam_examples/PlanarSLAMExample_sampling.m

@@ -52,8 +52,8 @@ import gtsam.*
 cla;hold on
 marginals = Marginals(graph, sample);
 
-gtsam_utils.plot2DTrajectory(sample, [], marginals);
+gtsam.plot2DTrajectory(sample, [], marginals);
-gtsam_utils.plot2DPoints(sample, [], marginals);
+gtsam.plot2DPoints(sample, [], marginals);
 
 for j=1:2
     key = symbol('l',j);
@@ -73,5 +73,5 @@ N=1000;
 for s=1:N
     delta = S{2}*randn(2,1);
     proposedPoint = Point2(point{2}.x+delta(1),point{2}.y+delta(2));
-    gtsam_utils.plotPoint2(proposedPoint,'k.')
+    gtsam.plotPoint2(proposedPoint,'k.')
 end

matlab/gtsam_examples/Pose2SLAMExample.m

@@ -68,7 +68,7 @@ hold on
 plot([result.at(5).x;result.at(2).x],[result.at(5).y;result.at(2).y],'r-','LineWidth',2);
 marginals = Marginals(graph, result);
 
-gtsam_utils.plot2DTrajectory(result, [], marginals);
+gtsam.plot2DTrajectory(result, [], marginals);
 
 axis([-0.6 4.8 -1 1])
 axis equal

matlab/gtsam_examples/Pose2SLAMExample_circle.m

@@ -12,7 +12,7 @@
 
 %% Create a hexagon of poses
 import gtsam.*
-hexagon = gtsam_utils.circlePose2(6,1.0);
+hexagon = gtsam.circlePose2(6,1.0);
 p0 = hexagon.at(0);
 p1 = hexagon.at(1);
 
@@ -42,7 +42,7 @@ initial.insert(5, hexagon.at(5).retract([-0.1, 0.1,-0.1]'));
 %% Plot Initial Estimate
 import gtsam.*
 cla
-gtsam_utils.plot2DTrajectory(initial, 'g*-'); axis equal
+gtsam.plot2DTrajectory(initial, 'g*-'); axis equal
 
 %% optimize
 import gtsam.*
@@ -51,7 +51,7 @@ result = optimizer.optimizeSafely;
 
 %% Show Result
 import gtsam.*
-hold on; gtsam_utils.plot2DTrajectory(result, 'b*-');
+hold on; gtsam.plot2DTrajectory(result, 'b*-');
 view(2);
 axis([-1.5 1.5 -1.5 1.5]);
 result.print(sprintf('\nFinal result:\n'));

matlab/gtsam_examples/Pose2SLAMExample_graph.m

@@ -11,7 +11,7 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Find data file
-datafile = gtsam_utils.findExampleDataFile('w100-odom.graph');
+datafile = gtsam.findExampleDataFile('w100-odom.graph');
 
 %% Initialize graph, initial estimate, and odometry noise
 import gtsam.*
@@ -31,13 +31,13 @@ graph.add(PriorFactorPose2(0, priorMean, priorNoise)); % add directly to graph
 %% Plot Initial Estimate
 import gtsam.*
 cla
-gtsam_utils.plot2DTrajectory(initial, 'g-*'); axis equal
+gtsam.plot2DTrajectory(initial, 'g-*'); axis equal
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
 import gtsam.*
 optimizer = LevenbergMarquardtOptimizer(graph, initial);
 result = optimizer.optimizeSafely;
-hold on; gtsam_utils.plot2DTrajectory(result, 'b-*');
+hold on; gtsam.plot2DTrajectory(result, 'b-*');
 result.print(sprintf('\nFinal result:\n'));
 
 %% Plot Covariance Ellipses
@@ -47,7 +47,7 @@ P={};
 for i=1:result.size()-1
     pose_i = result.at(i);
     P{i}=marginals.marginalCovariance(i);
-    gtsam_utils.plotPose2(pose_i,'b',P{i})
+    gtsam.plotPose2(pose_i,'b',P{i})
 end
 view(2)
 axis([-15 10 -15 10]); axis equal;

matlab/gtsam_examples/Pose3SLAMExample.m

@@ -12,7 +12,7 @@
 
 %% Create a hexagon of poses
 import gtsam.*
-hexagon = gtsam_utils.circlePose3(6,1.0);
+hexagon = gtsam.circlePose3(6,1.0);
 p0 = hexagon.at(0);
 p1 = hexagon.at(1);
 
@@ -43,7 +43,7 @@ initial.insert(5, hexagon.at(5).retract(s*randn(6,1)));
 %% Plot Initial Estimate
 import gtsam.*
 cla
-gtsam_utils.plot3DTrajectory(initial, 'g-*');
+gtsam.plot3DTrajectory(initial, 'g-*');
 
 %% optimize
 import gtsam.*
@@ -52,7 +52,7 @@ result = optimizer.optimizeSafely();
 
 %% Show Result
 import gtsam.*
-hold on; gtsam_utils.plot3DTrajectory(result, 'b-*', true, 0.3);
+hold on; gtsam.plot3DTrajectory(result, 'b-*', true, 0.3);
 axis([-2 2 -2 2 -1 1]);
 axis equal
 view(-37,40)

matlab/gtsam_examples/Pose3SLAMExample_graph.m

@@ -16,27 +16,27 @@ N = 2500;
 % dataset = 'sphere2500_groundtruth.txt';
 dataset = 'sphere2500.txt';
 
-datafile = gtsam_utils.findExampleDataFile(dataset);
+datafile = gtsam.findExampleDataFile(dataset);
 
 %% Initialize graph, initial estimate, and odometry noise
 import gtsam.*
 model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
-[graph,initial]=gtsam_utils.load3D(datafile,model,true,N);
+[graph,initial]=gtsam.load3D(datafile,model,true,N);
 
 %% Plot Initial Estimate
 import gtsam.*
 cla
 first = initial.at(0);
 plot3(first.x(),first.y(),first.z(),'r*'); hold on
-gtsam_utils.plot3DTrajectory(initial,'g-',false);
+gtsam.plot3DTrajectory(initial,'g-',false);
 drawnow;
 
 %% Read again, now with all constraints, and optimize
 import gtsam.*
-graph = gtsam_utils.load3D(datafile, model, false, N);
+graph = gtsam.load3D(datafile, model, false, N);
 graph.add(NonlinearEqualityPose3(0, first));
 optimizer = LevenbergMarquardtOptimizer(graph, initial);
 result = optimizer.optimizeSafely();
-gtsam_utils.plot3DTrajectory(result, 'r-', false); axis equal;
+gtsam.plot3DTrajectory(result, 'r-', false); axis equal;
 
 view(3); axis equal;

matlab/gtsam_examples/SBAExample.m

@@ -22,7 +22,7 @@ options.nrCameras = 10;
 options.showImages = false;
 
 %% Generate data
-[data,truth] = gtsam_utils.VisualISAMGenerateData(options);
+[data,truth] = gtsam.VisualISAMGenerateData(options);
 
 measurementNoiseSigma = 1.0;
 pointNoiseSigma = 0.1;

matlab/gtsam_examples/SFMExample.m

@@ -22,7 +22,7 @@ options.nrCameras = 10;
 options.showImages = false;
 
 %% Generate data
-[data,truth] = gtsam_utils.VisualISAMGenerateData(options);
+[data,truth] = gtsam.VisualISAMGenerateData(options);
 
 measurementNoiseSigma = 1.0;
 pointNoiseSigma = 0.1;
@@ -86,8 +86,8 @@ marginals = Marginals(graph, result);
 cla
 hold on;
 
-gtsam_utils.plot3DPoints(result, [], marginals);
+gtsam.plot3DPoints(result, [], marginals);
-gtsam_utils.plot3DTrajectory(result, '*', 1, 8, marginals);
+gtsam.plot3DTrajectory(result, '*', 1, 8, marginals);
 
 axis([-40 40 -40 40 -10 20]);axis equal
 view(3)

matlab/gtsam_examples/StereoVOExample.m

@@ -75,6 +75,6 @@ axis equal
 view(-38,12)
 camup([0;1;0]);
 
-gtsam_utils.plot3DTrajectory(initialEstimate, 'r', 1, 0.3);
+gtsam.plot3DTrajectory(initialEstimate, 'r', 1, 0.3);
-gtsam_utils.plot3DTrajectory(result, 'g', 1, 0.3);
+gtsam.plot3DTrajectory(result, 'g', 1, 0.3);
-gtsam_utils.plot3DPoints(result);
+gtsam.plot3DPoints(result);

matlab/gtsam_examples/StereoVOExample_large.m

@@ -13,7 +13,7 @@
 %% Load calibration
 import gtsam.*
 % format: fx fy skew cx cy baseline
-calib = dlmread(gtsam_utils.findExampleDataFile('VO_calibration.txt'));
+calib = dlmread(gtsam.findExampleDataFile('VO_calibration.txt'));
 K = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6));
 stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
 
@@ -26,7 +26,7 @@ initial = Values;
 % row format: camera_id 4x4 pose (row, major)
 import gtsam.*
 fprintf(1,'Reading data\n');
-cameras = dlmread(gtsam_utils.findExampleDataFile('VO_camera_poses_large.txt'));
+cameras = dlmread(gtsam.findExampleDataFile('VO_camera_poses_large.txt'));
 for i=1:size(cameras,1)
     pose = Pose3(reshape(cameras(i,2:17),4,4)');
     initial.insert(symbol('x',cameras(i,1)),pose);
@@ -35,7 +35,7 @@ end
 %% load stereo measurements and initialize landmarks
 % camera_id landmark_id uL uR v X Y Z
 import gtsam.*
-measurements = dlmread(gtsam_utils.findExampleDataFile('VO_stereo_factors_large.txt'));
+measurements = dlmread(gtsam.findExampleDataFile('VO_stereo_factors_large.txt'));
 
 fprintf(1,'Creating Graph\n'); tic
 for i=1:size(measurements,1)
@@ -69,9 +69,9 @@ toc
 %% visualize initial trajectory, final trajectory, and final points
 cla; hold on;
 
-gtsam_utils.plot3DTrajectory(initial, 'r', 1, 0.5);
+gtsam.plot3DTrajectory(initial, 'r', 1, 0.5);
-gtsam_utils.plot3DTrajectory(result, 'g', 1, 0.5);
+gtsam.plot3DTrajectory(result, 'g', 1, 0.5);
-gtsam_utils.plot3DPoints(result);
+gtsam.plot3DPoints(result);
 
 axis([-5 20 -20 20 0 100]);
 axis equal

matlab/gtsam_examples/VisualISAMExample.m

@@ -32,17 +32,17 @@ options.saveFigures = false;
 options.saveDotFiles = false;
 
 %% Generate data
-[data,truth] = gtsam_utils.VisualISAMGenerateData(options);
+[data,truth] = gtsam.VisualISAMGenerateData(options);
 
 %% Initialize iSAM with the first pose and points
-[noiseModels,isam,result,nextPose] = gtsam_utils.VisualISAMInitialize(data,truth,options);
+[noiseModels,isam,result,nextPose] = gtsam.VisualISAMInitialize(data,truth,options);
 cla;
-gtsam_utils.VisualISAMPlot(truth, data, isam, result, options)
+gtsam.VisualISAMPlot(truth, data, isam, result, options)
 
 %% Main loop for iSAM: stepping through all poses
 for frame_i=3:options.nrCameras
-    [isam,result,nextPose] = gtsam_utils.VisualISAMStep(data,noiseModels,isam,result,truth,nextPose);
+    [isam,result,nextPose] = gtsam.VisualISAMStep(data,noiseModels,isam,result,truth,nextPose);
     if mod(frame_i,options.drawInterval)==0
-        gtsam_utils.VisualISAMPlot(truth, data, isam, result, options)
+        gtsam.VisualISAMPlot(truth, data, isam, result, options)
     end
 end

matlab/gtsam_examples/VisualISAM_gui.m

@@ -230,12 +230,12 @@ global frame_i truth data noiseModels isam result nextPoseIndex options
 initOptions(handles)
 
 % Generate Data
-[data,truth] = gtsam_utils.VisualISAMGenerateData(options);
+[data,truth] = gtsam.VisualISAMGenerateData(options);
 
 % Initialize and plot
-[noiseModels,isam,result,nextPoseIndex] = gtsam_utils.VisualISAMInitialize(data,truth,options);
+[noiseModels,isam,result,nextPoseIndex] = gtsam.VisualISAMInitialize(data,truth,options);
 cla
-gtsam_utils.VisualISAMPlot(truth, data, isam, result, options)
+gtsam.VisualISAMPlot(truth, data, isam, result, options)
 frame_i = 2;
 showFramei(hObject, handles)
 
@@ -246,10 +246,10 @@ global frame_i truth data noiseModels isam result nextPoseIndex options
 while (frame_i<size(truth.cameras,2))
     frame_i = frame_i+1;
     showFramei(hObject, handles)
-    [isam,result,nextPoseIndex] = gtsam_utils.VisualISAMStep(data,noiseModels,isam,result,truth,nextPoseIndex);
+    [isam,result,nextPoseIndex] = gtsam.VisualISAMStep(data,noiseModels,isam,result,truth,nextPoseIndex);
     if mod(frame_i,options.drawInterval)==0
         showWaiting(handles, 'Computing marginals...');
-        gtsam_utils.VisualISAMPlot(truth, data, isam, result, options)
+        gtsam.VisualISAMPlot(truth, data, isam, result, options)
         showWaiting(handles, '');
     end
 end
@@ -261,8 +261,8 @@ global frame_i truth data noiseModels isam result nextPoseIndex options
 if (frame_i<size(truth.cameras,2))
     frame_i = frame_i+1;
     showFramei(hObject, handles)
-    [isam,result,nextPoseIndex] = gtsam_utils.VisualISAMStep(data,noiseModels,isam,result,truth,nextPoseIndex);
+    [isam,result,nextPoseIndex] = gtsam.VisualISAMStep(data,noiseModels,isam,result,truth,nextPoseIndex);
     showWaiting(handles, 'Computing marginals...');
-    gtsam_utils.VisualISAMPlot(truth, data, isam, result, options)
+    gtsam.VisualISAMPlot(truth, data, isam, result, options)
     showWaiting(handles, '');
 end

matlab/gtsam_tests/testJacobianFactor.m

@@ -2,7 +2,6 @@
 % eliminate
 
 import gtsam.*
-import gtsam_utils.*
 
 % the combined linear factor
 Ax2 = [

matlab/gtsam_tests/testKalmanFilter.m

@@ -57,13 +57,13 @@ x_initial = [0.0;0.0];
 P_initial = 0.01*eye(2);
 
 %% Create an KF object
-import gtsam_utils.*
+import gtsam.*
 state = KF.init(x_initial, P_initial);
 EQUALITY('expected0,state.mean', expected0,state.mean);
 EQUALITY('expected0,state.mean', P00,state.covariance);
 
 %% Run iteration 1
-import gtsam_utils.*
+import gtsam.*
 state = KF.predict(state,F, B, u, modelQ);
 EQUALITY('expected1,state.mean', expected1,state.mean);
 EQUALITY('P01,state.covariance', P01,state.covariance);
@@ -72,14 +72,14 @@ EQUALITY('expected1,state.mean', expected1,state.mean);
 EQUALITY('I11,state.information', I11,state.information);
 
 %% Run iteration 2
-import gtsam_utils.*
+import gtsam.*
 state = KF.predict(state,F, B, u, modelQ);
 EQUALITY('expected2,state.mean', expected2,state.mean);
 state = KF.update(state,H,z2,modelR);
 EQUALITY('expected2,state.mean', expected2,state.mean);
 
 %% Run iteration 3
-import gtsam_utils.*
+import gtsam.*
 state = KF.predict(state,F, B, u, modelQ);
 EQUALITY('expected3,state.mean', expected3,state.mean);
 state = KF.update(state,H,z3,modelR);

matlab/gtsam_tests/testLocalizationExample.m

@@ -47,7 +47,6 @@ result = optimizer.optimizeSafely();
 
 %% Plot Covariance Ellipses
 import gtsam.*
-import gtsam_utils.*
 marginals = Marginals(graph, result);
 P={};
 for i=1:result.size()

matlab/gtsam_tests/testOdometryExample.m

@@ -42,6 +42,6 @@ marginals = Marginals(graph, result);
 marginals.marginalCovariance(1);
 
 %% Check first pose equality
-import gtsam_utils.*
+import gtsam.*
 pose_1 = result.at(1);
 CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

matlab/gtsam_tests/testPlanarSLAMExample.m

@@ -66,7 +66,6 @@ marginals = Marginals(graph, result);
 
 %% Check first pose and point equality
 import gtsam.*
-import gtsam_utils.*
 pose_1 = result.at(symbol('x',1));
 marginals.marginalCovariance(symbol('x',1));
 CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

matlab/gtsam_tests/testPose2SLAMExample.m

@@ -66,7 +66,6 @@ result = optimizer.optimizeSafely();
 
 %% Plot Covariance Ellipses
 import gtsam.*
-import gtsam_utils.*
 marginals = Marginals(graph, result);
 P = marginals.marginalCovariance(1);
 

matlab/gtsam_tests/testPose3SLAMExample.m

@@ -42,7 +42,6 @@ initial.insert(5, hexagon.at(5).retract(s*randn(6,1)));
 
 %% optimize
 import gtsam.*
-import gtsam_utils.*
 optimizer = LevenbergMarquardtOptimizer(fg, initial);
 result = optimizer.optimizeSafely;
 

matlab/gtsam_tests/testSFMExample.m

@@ -11,7 +11,6 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 import gtsam.*
-import gtsam_utils.*
 
 options.triangle = false;
 options.nrCameras = 10;
@@ -68,7 +67,6 @@ marginals.marginalCovariance(symbol('x',1));
 
 %% Check optimized results, should be equal to ground truth
 import gtsam.*
-import gtsam_utils.*
 for i=1:size(truth.cameras,2)
     pose_i = result.at(symbol('x',i));
     CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5))

matlab/gtsam_tests/testStereoVOExample.m

@@ -68,7 +68,6 @@ result = optimizer.optimize();
 
 %% check equality for the first pose and point
 import gtsam.*
-import gtsam_utils.*
 pose_x1 = result.at(x1);
 CHECK('pose_x1.equals(first_pose,1e-4)',pose_x1.equals(first_pose,1e-4));
 

matlab/gtsam_tests/testVisualISAMExample.m

@@ -32,15 +32,15 @@ options.saveFigures = false;
 options.saveDotFiles = false;
 
 %% Generate data
-import gtsam_utils.*
+import gtsam.*
 [data,truth] = VisualISAMGenerateData(options);
 
 %% Initialize iSAM with the first pose and points
-import gtsam_utils.*
+import gtsam.*
 [noiseModels,isam,result,nextPose] = VisualISAMInitialize(data,truth,options);
 
 %% Main loop for iSAM: stepping through all poses
-import gtsam_utils.*
+import gtsam.*
 for frame_i=3:options.nrCameras
     [isam,result,nextPose] = VisualISAMStep(data,noiseModels,isam,result,truth,nextPose);
 end