diff --git a/matlab/+gtsam_utils/CHECK.m b/matlab/+gtsam/CHECK.m similarity index 100% rename from matlab/+gtsam_utils/CHECK.m rename to matlab/+gtsam/CHECK.m diff --git a/matlab/+gtsam_utils/EQUALITY.m b/matlab/+gtsam/EQUALITY.m similarity index 100% rename from matlab/+gtsam_utils/EQUALITY.m rename to matlab/+gtsam/EQUALITY.m diff --git a/matlab/+gtsam_utils/VisualISAMGenerateData.m b/matlab/+gtsam/VisualISAMGenerateData.m similarity index 100% rename from matlab/+gtsam_utils/VisualISAMGenerateData.m rename to matlab/+gtsam/VisualISAMGenerateData.m diff --git a/matlab/+gtsam_utils/VisualISAMInitialize.m b/matlab/+gtsam/VisualISAMInitialize.m similarity index 100% rename from matlab/+gtsam_utils/VisualISAMInitialize.m rename to matlab/+gtsam/VisualISAMInitialize.m diff --git a/matlab/+gtsam_utils/VisualISAMPlot.m b/matlab/+gtsam/VisualISAMPlot.m similarity index 86% rename from matlab/+gtsam_utils/VisualISAMPlot.m rename to matlab/+gtsam/VisualISAMPlot.m index 41aa49ac6..5ef64cf62 100644 --- a/matlab/+gtsam_utils/VisualISAMPlot.m +++ b/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; diff --git a/matlab/+gtsam_utils/VisualISAMStep.m b/matlab/+gtsam/VisualISAMStep.m similarity index 100% rename from matlab/+gtsam_utils/VisualISAMStep.m rename to matlab/+gtsam/VisualISAMStep.m diff --git a/matlab/+gtsam_utils/circlePose2.m b/matlab/+gtsam/circlePose2.m similarity index 100% rename from matlab/+gtsam_utils/circlePose2.m rename to matlab/+gtsam/circlePose2.m diff --git a/matlab/+gtsam_utils/circlePose3.m b/matlab/+gtsam/circlePose3.m similarity index 100% rename from matlab/+gtsam_utils/circlePose3.m rename to matlab/+gtsam/circlePose3.m diff --git a/matlab/+gtsam_utils/covarianceEllipse.m b/matlab/+gtsam/covarianceEllipse.m similarity index 100% rename from matlab/+gtsam_utils/covarianceEllipse.m rename to matlab/+gtsam/covarianceEllipse.m diff --git a/matlab/+gtsam_utils/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m similarity index 100% rename from matlab/+gtsam_utils/covarianceEllipse3D.m rename to matlab/+gtsam/covarianceEllipse3D.m diff --git a/matlab/+gtsam_utils/findExampleDataFile.m b/matlab/+gtsam/findExampleDataFile.m similarity index 100% rename from matlab/+gtsam_utils/findExampleDataFile.m rename to matlab/+gtsam/findExampleDataFile.m diff --git a/matlab/+gtsam_utils/load3D.m b/matlab/+gtsam/load3D.m similarity index 100% rename from matlab/+gtsam_utils/load3D.m rename to matlab/+gtsam/load3D.m diff --git a/matlab/+gtsam_utils/plot2DPoints.m b/matlab/+gtsam/plot2DPoints.m similarity index 88% rename from matlab/+gtsam_utils/plot2DPoints.m rename to matlab/+gtsam/plot2DPoints.m index b98434627..d4703a5d7 100644 --- a/matlab/+gtsam_utils/plot2DPoints.m +++ b/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 diff --git a/matlab/+gtsam_utils/plot2DTrajectory.m b/matlab/+gtsam/plot2DTrajectory.m similarity index 93% rename from matlab/+gtsam_utils/plot2DTrajectory.m rename to matlab/+gtsam/plot2DTrajectory.m index c962c993b..329026398 100644 --- a/matlab/+gtsam_utils/plot2DTrajectory.m +++ b/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 diff --git a/matlab/+gtsam_utils/plot3DPoints.m b/matlab/+gtsam/plot3DPoints.m similarity index 88% rename from matlab/+gtsam_utils/plot3DPoints.m rename to matlab/+gtsam/plot3DPoints.m index e7a5ab0a0..8feab1744 100644 --- a/matlab/+gtsam_utils/plot3DPoints.m +++ b/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 diff --git a/matlab/+gtsam_utils/plot3DTrajectory.m b/matlab/+gtsam/plot3DTrajectory.m similarity index 91% rename from matlab/+gtsam_utils/plot3DTrajectory.m rename to matlab/+gtsam/plot3DTrajectory.m index 08b2f0daa..1cd35c22f 100644 --- a/matlab/+gtsam_utils/plot3DTrajectory.m +++ b/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 diff --git a/matlab/+gtsam_utils/plotCamera.m b/matlab/+gtsam/plotCamera.m similarity index 100% rename from matlab/+gtsam_utils/plotCamera.m rename to matlab/+gtsam/plotCamera.m diff --git a/matlab/+gtsam_utils/plotPoint2.m b/matlab/+gtsam/plotPoint2.m similarity index 76% rename from matlab/+gtsam_utils/plotPoint2.m rename to matlab/+gtsam/plotPoint2.m index a1bc85a88..e62804ce3 100644 --- a/matlab/+gtsam_utils/plotPoint2.m +++ b/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 \ No newline at end of file diff --git a/matlab/+gtsam_utils/plotPoint3.m b/matlab/+gtsam/plotPoint3.m similarity index 80% rename from matlab/+gtsam_utils/plotPoint3.m rename to matlab/+gtsam/plotPoint3.m index 6aafd5f21..390b44939 100644 --- a/matlab/+gtsam_utils/plotPoint3.m +++ b/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 diff --git a/matlab/+gtsam_utils/plotPose2.m b/matlab/+gtsam/plotPose2.m similarity index 84% rename from matlab/+gtsam_utils/plotPose2.m rename to matlab/+gtsam/plotPose2.m index a9997dc2c..c7fba28ed 100644 --- a/matlab/+gtsam_utils/plotPose2.m +++ b/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 \ No newline at end of file diff --git a/matlab/+gtsam_utils/plotPose3.m b/matlab/+gtsam/plotPose3.m similarity index 94% rename from matlab/+gtsam_utils/plotPose3.m rename to matlab/+gtsam/plotPose3.m index d24a998c8..e2c254e39 100644 --- a/matlab/+gtsam_utils/plotPose3.m +++ b/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 diff --git a/matlab/+gtsam_utils/load2D.m b/matlab/+gtsam_utils/load2D.m deleted file mode 100644 index c4b2e105d..000000000 --- a/matlab/+gtsam_utils/load2D.m +++ /dev/null @@ -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 - diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 66fa99def..b4e43d3d6 100644 --- a/matlab/CMakeLists.txt +++ b/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 diff --git a/matlab/gtsam_examples/LocalizationExample.m b/matlab/gtsam_examples/LocalizationExample.m index aad3af98c..c877c70c7 100644 --- a/matlab/gtsam_examples/LocalizationExample.m +++ b/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 diff --git a/matlab/gtsam_examples/OdometryExample.m b/matlab/gtsam_examples/OdometryExample.m index e47becda5..b913b187a 100644 --- a/matlab/gtsam_examples/OdometryExample.m +++ b/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 diff --git a/matlab/gtsam_examples/PlanarSLAMExample.m b/matlab/gtsam_examples/PlanarSLAMExample.m index 056db501e..e37326275 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample.m +++ b/matlab/gtsam_examples/PlanarSLAMExample.m @@ -72,8 +72,8 @@ import gtsam.* cla;hold on marginals = Marginals(graph, result); -gtsam_utils.plot2DTrajectory(result, [], marginals); -gtsam_utils.plot2DPoints(result, [], marginals); +gtsam.plot2DTrajectory(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-'); diff --git a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m b/matlab/gtsam_examples/PlanarSLAMExample_sampling.m index 5604d0ddc..59c99df53 100644 --- a/matlab/gtsam_examples/PlanarSLAMExample_sampling.m +++ b/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_utils.plot2DPoints(sample, [], marginals); +gtsam.plot2DTrajectory(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 \ No newline at end of file diff --git a/matlab/gtsam_examples/Pose2SLAMExample.m b/matlab/gtsam_examples/Pose2SLAMExample.m index a8ad32254..fccacc894 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample.m +++ b/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 diff --git a/matlab/gtsam_examples/Pose2SLAMExample_circle.m b/matlab/gtsam_examples/Pose2SLAMExample_circle.m index b40c67322..957a30df4 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_circle.m +++ b/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')); diff --git a/matlab/gtsam_examples/Pose2SLAMExample_graph.m b/matlab/gtsam_examples/Pose2SLAMExample_graph.m index cc8b38cdb..3eed090a2 100644 --- a/matlab/gtsam_examples/Pose2SLAMExample_graph.m +++ b/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; diff --git a/matlab/gtsam_examples/Pose3SLAMExample.m b/matlab/gtsam_examples/Pose3SLAMExample.m index bdc101b9d..b1521ff5b 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample.m +++ b/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) diff --git a/matlab/gtsam_examples/Pose3SLAMExample_graph.m b/matlab/gtsam_examples/Pose3SLAMExample_graph.m index 574894878..19cc31439 100644 --- a/matlab/gtsam_examples/Pose3SLAMExample_graph.m +++ b/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; \ No newline at end of file diff --git a/matlab/gtsam_examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m index 852c83fdc..7e4ac4031 100644 --- a/matlab/gtsam_examples/SBAExample.m +++ b/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; diff --git a/matlab/gtsam_examples/SFMExample.m b/matlab/gtsam_examples/SFMExample.m index 7f583b386..92eeb482b 100644 --- a/matlab/gtsam_examples/SFMExample.m +++ b/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_utils.plot3DTrajectory(result, '*', 1, 8, marginals); +gtsam.plot3DPoints(result, [], marginals); +gtsam.plot3DTrajectory(result, '*', 1, 8, marginals); axis([-40 40 -40 40 -10 20]);axis equal view(3) diff --git a/matlab/gtsam_examples/StereoVOExample.m b/matlab/gtsam_examples/StereoVOExample.m index e62c4395c..94c4b3f37 100644 --- a/matlab/gtsam_examples/StereoVOExample.m +++ b/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_utils.plot3DTrajectory(result, 'g', 1, 0.3); -gtsam_utils.plot3DPoints(result); +gtsam.plot3DTrajectory(initialEstimate, 'r', 1, 0.3); +gtsam.plot3DTrajectory(result, 'g', 1, 0.3); +gtsam.plot3DPoints(result); diff --git a/matlab/gtsam_examples/StereoVOExample_large.m b/matlab/gtsam_examples/StereoVOExample_large.m index 5ae3a8fd7..850af4f80 100644 --- a/matlab/gtsam_examples/StereoVOExample_large.m +++ b/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_utils.plot3DTrajectory(result, 'g', 1, 0.5); -gtsam_utils.plot3DPoints(result); +gtsam.plot3DTrajectory(initial, 'r', 1, 0.5); +gtsam.plot3DTrajectory(result, 'g', 1, 0.5); +gtsam.plot3DPoints(result); axis([-5 20 -20 20 0 100]); axis equal diff --git a/matlab/gtsam_examples/VisualISAMExample.m b/matlab/gtsam_examples/VisualISAMExample.m index 64634d336..31c240029 100644 --- a/matlab/gtsam_examples/VisualISAMExample.m +++ b/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 diff --git a/matlab/gtsam_examples/VisualISAM_gui.m b/matlab/gtsam_examples/VisualISAM_gui.m index f7990268f..bc08e7abf 100644 --- a/matlab/gtsam_examples/VisualISAM_gui.m +++ b/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