Wrap generates Matlab namespaces, so now 'import gtsam.*' allows class names like Values, NonlinearFactor, to be used. Without import, syntax is gtsam.Values, etc.

matlab/CMakeLists.txt

@@ -6,12 +6,12 @@ message(STATUS "Installing Matlab Utility Functions")
 file(GLOB matlab_utils_m "${GTSAM_SOURCE_ROOT_DIR}/matlab/*.m")
 file(GLOB matlab_utils_fig "${GTSAM_SOURCE_ROOT_DIR}/matlab/*.fig")
 set(matlab_utils ${matlab_utils_m} ${matlab_utils_fig})
-install(FILES ${matlab_utils} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam)
+install(FILES ${matlab_utils} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/)
 
 # Tests
 message(STATUS "Installing Matlab Toolbox Tests")
 file(GLOB matlab_tests "${GTSAM_SOURCE_ROOT_DIR}/matlab/tests/*.m")
-install(FILES ${matlab_tests} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/tests)
+install(FILES ${matlab_tests} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_tests)
 
 # Examples
 message(STATUS "Installing Matlab Toolbox Examples")
@@ -19,11 +19,11 @@ message(STATUS "Installing Matlab Toolbox Examples")
 file(GLOB matlab_examples_m "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/*.m")
 file(GLOB matlab_examples_fig "${GTSAM_SOURCE_ROOT_DIR}/matlab/examples/*.fig")
 set(matlab_examples ${matlab_examples_m} ${matlab_examples_fig})
-install(FILES ${matlab_examples} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/examples)
+install(FILES ${matlab_examples} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples)
 
 message(STATUS "Installing Matlab Toolbox Examples (Data)")
 # Data files: *.graph and *.txt
 file(GLOB matlab_examples_data_graph "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph")
 file(GLOB matlab_examples_data_txt "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt")
 set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_txt}) 
-install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/Data)
+install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data)

matlab/VisualISAMGenerateData.m

@@ -3,33 +3,35 @@ function [data,truth] = VisualISAMGenerateData(options)
 % Authors: Duy Nguyen Ta and Frank Dellaert
 
 %% Generate simulated data
+import gtsam.*
 if options.triangle % Create a triangle target, just 3 points on a plane
     nrPoints = 3;
     r = 10;
     for j=1:nrPoints
         theta = (j-1)*2*pi/nrPoints;
-        truth.points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
+        truth.points{j} = Point3([r*cos(theta), r*sin(theta), 0]');
     end
 else % 3D landmarks as vertices of a cube
     nrPoints = 8;
-    truth.points = {gtsamPoint3([10 10 10]'),...
+    truth.points = {Point3([10 10 10]'),...
-        gtsamPoint3([-10 10 10]'),...
+        Point3([-10 10 10]'),...
-        gtsamPoint3([-10 -10 10]'),...
+        Point3([-10 -10 10]'),...
-        gtsamPoint3([10 -10 10]'),...
+        Point3([10 -10 10]'),...
-        gtsamPoint3([10 10 -10]'),...
+        Point3([10 10 -10]'),...
-        gtsamPoint3([-10 10 -10]'),...
+        Point3([-10 10 -10]'),...
-        gtsamPoint3([-10 -10 -10]'),...
+        Point3([-10 -10 -10]'),...
-        gtsamPoint3([10 -10 -10]')};
+        Point3([10 -10 -10]')};
 end
 
 %% Create camera cameras on a circle around the triangle
+import gtsam.*
 height = 10; r = 40;
-truth.K = gtsamCal3_S2(500,500,0,640/2,480/2);
+truth.K = Cal3_S2(500,500,0,640/2,480/2);
 data.K = truth.K;
 for i=1:options.nrCameras
     theta = (i-1)*2*pi/options.nrCameras;
-    t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
+    t = Point3([r*cos(theta), r*sin(theta), height]');
-    truth.cameras{i} = gtsamSimpleCamera.Lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), truth.K);
+    truth.cameras{i} = SimpleCamera.Lookat(t, Point3, Point3([0,0,1]'), truth.K);
     % Create measurements
     for j=1:nrPoints
         % All landmarks seen in every frame

matlab/VisualISAMInitialize.m

@@ -3,18 +3,19 @@ function [noiseModels,isam,result] = VisualISAMInitialize(data,truth,options)
 % Authors: Duy Nguyen Ta, Frank Dellaert and Alex Cunningham
 
 %% Initialize iSAM
-isam = visualSLAMISAM(options.reorderInterval);
+isam = visualSLAM.ISAM(options.reorderInterval);
 
 %% Set Noise parameters
-noiseModels.pose = gtsamnoiseModelDiagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
+import gtsam.*
-noiseModels.odometry = gtsamnoiseModelDiagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
+noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
-noiseModels.point = gtsamnoiseModelIsotropic.Sigma(3, 0.1);
+noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
-noiseModels.measurement = gtsamnoiseModelIsotropic.Sigma(2, 1.0);
+noiseModels.point = noiseModel.Isotropic.Sigma(3, 0.1);
+noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0);
 
 %% Add constraints/priors
 % TODO: should not be from ground truth!
-newFactors = visualSLAMGraph;
+newFactors = visualSLAM.Graph;
-initialEstimates = visualSLAMValues;
+initialEstimates = visualSLAM.Values;
 for i=1:2
     ii = symbol('x',i);
     if i==1

matlab/VisualISAMPlot.m

@@ -21,10 +21,11 @@ for j=0:N-1  % NOTE: uses indexing directly from a C++ vector, so zero-indexed
 end
 
 %% Plot cameras
+import gtsam.*
 for i=1:options.cameraInterval:M
     ii = symbol('x',i);
     pose_i = result.pose(ii);
-    if options.hardConstraint & (i==1)
+    if options.hardConstraint && (i==1)
         plotPose3(pose_i,[],10);
     else
         P = isam.marginalCovariance(ii);

matlab/VisualISAMStep.m

@@ -4,15 +4,17 @@ function [isam,result] = VisualISAMStep(data,noiseModels,isam,result,truth, opti
 
 % iSAM expects us to give it a new set of factors 
 % along with initial estimates for any new variables introduced.
-newFactors = visualSLAMGraph;
+newFactors = visualSLAM.Graph;
-initialEstimates = visualSLAMValues;
+initialEstimates = visualSLAM.Values;
 
 %% Add odometry
+import gtsam.*
 i = result.nrPoses+1;
 odometry = data.odometry{i-1};
 newFactors.addRelativePose(symbol('x',i-1), symbol('x',i), odometry, noiseModels.odometry);
 
 %% Add visual measurement factors and initializations as necessary
+import gtsam.*
 for k=1:length(data.Z{i})
     zij = data.Z{i}{k};
     j = data.J{i}{k};
@@ -26,6 +28,7 @@ for k=1:length(data.Z{i})
 end
 
 %% Initial estimates for the new pose.
+import gtsam.*
 prevPose = result.pose(symbol('x',i-1));
 initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry));
 

matlab/examples/LocalizationExample.m

@@ -16,36 +16,41 @@
 %  - The robot is on a grid, moving 2 meters each step
 
 %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
-graph = pose2SLAMGraph;
+graph = pose2SLAM.Graph;
 
 %% Add two odometry factors
-odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
+import gtsam.*
-odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
 graph.addRelativePose(1, 2, odometry, odometryNoise);
 graph.addRelativePose(2, 3, odometry, odometryNoise);
 
 %% Add three "GPS" measurements
+import gtsam.*
 % We use Pose2 Priors here with high variance on theta
-noiseModel = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.1; 10]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]);
-graph.addPosePrior(1, gtsamPose2(0.0, 0.0, 0.0), noiseModel);
+graph.addPosePrior(1, Pose2(0.0, 0.0, 0.0), priorNoise);
-graph.addPosePrior(2, gtsamPose2(2.0, 0.0, 0.0), noiseModel);
+graph.addPosePrior(2, Pose2(2.0, 0.0, 0.0), priorNoise);
-graph.addPosePrior(3, gtsamPose2(4.0, 0.0, 0.0), noiseModel);
+graph.addPosePrior(3, Pose2(4.0, 0.0, 0.0), priorNoise);
 
 %% print
 graph.print(sprintf('\nFactor graph:\n'));
 
 %% Initialize to noisy points
-initialEstimate = pose2SLAMValues;
+import gtsam.*
-initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2));
+initialEstimate = pose2SLAM.Values;
-initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2));
+initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
-initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
+initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
+initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
 initialEstimate.print(sprintf('\nInitial estimate:\n  '));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
+import gtsam.*
 result = graph.optimize(initialEstimate,1);
 result.print(sprintf('\nFinal result:\n  '));
 
 %% Plot Covariance Ellipses
+import gtsam.*
 cla;
 X=result.poses();
 plot(X(:,1),X(:,2),'k*-'); hold on

matlab/examples/OdometryExample.m

@@ -16,16 +16,18 @@
 %  - The robot is on a grid, moving 2 meters each step
 
 %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
-graph = pose2SLAMGraph;
+graph = pose2SLAM.Graph;
 
 %% Add a Gaussian prior on pose x_1
-priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin
+import gtsam.*
-priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
+priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
 graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
 
 %% Add two odometry factors
-odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
+import gtsam.*
-odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
 graph.addRelativePose(1, 2, odometry, odometryNoise);
 graph.addRelativePose(2, 3, odometry, odometryNoise);
 
@@ -33,10 +35,11 @@ graph.addRelativePose(2, 3, odometry, odometryNoise);
 graph.print(sprintf('\nFactor graph:\n'));
 
 %% Initialize to noisy points
-initialEstimate = pose2SLAMValues;
+import gtsam.*
-initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2));
+initialEstimate = pose2SLAM.Values;
-initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2));
+initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
-initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
+initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
+initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
 initialEstimate.print(sprintf('\nInitial estimate:\n  '));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
@@ -44,6 +47,7 @@ result = graph.optimize(initialEstimate,1);
 result.print(sprintf('\nFinal result:\n  '));
 
 %% Plot Covariance Ellipses
+import gtsam.*
 cla;
 X=result.poses();
 plot(X(:,1),X(:,2),'k*-'); hold on

matlab/examples/PlanarSLAMExample.m

@@ -20,40 +20,45 @@
 %  - Landmarks are 2 meters away from the robot trajectory
 
 %% Create keys for variables
+import gtsam.*
 i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
 j1 = symbol('l',1); j2 = symbol('l',2);
 
 %% Create graph container and add factors to it
-graph = planarSLAMGraph;
+graph = planarSLAM.Graph;
 
 %% Add prior
-priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
+import gtsam.*
-priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]);
+priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
 graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph
 
 %% Add odometry
-odometry = gtsamPose2(2.0, 0.0, 0.0);
+import gtsam.*
-odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
+odometry = Pose2(2.0, 0.0, 0.0);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
 graph.addRelativePose(i1, i2, odometry, odometryNoise);
 graph.addRelativePose(i2, i3, odometry, odometryNoise);
 
 %% Add bearing/range measurement factors
+import gtsam.*
 degrees = pi/180;
-noiseModel = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.2]);
+brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
-graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel);
+graph.addBearingRange(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise);
-graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel);
+graph.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise);
-graph.addBearingRange(i3, j2, gtsamRot2(90*degrees), 2, noiseModel);
+graph.addBearingRange(i3, j2, Rot2(90*degrees), 2, brNoise);
 
 % print
 graph.print(sprintf('\nFull graph:\n'));
 
 %% Initialize to noisy points
-initialEstimate = planarSLAMValues;
+import gtsam.*
-initialEstimate.insertPose(i1, gtsamPose2(0.5, 0.0, 0.2));
+initialEstimate = planarSLAM.Values;
-initialEstimate.insertPose(i2, gtsamPose2(2.3, 0.1,-0.2));
+initialEstimate.insertPose(i1, Pose2(0.5, 0.0, 0.2));
-initialEstimate.insertPose(i3, gtsamPose2(4.1, 0.1, 0.1));
+initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2));
-initialEstimate.insertPoint(j1, gtsamPoint2(1.8, 2.1));
+initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1));
-initialEstimate.insertPoint(j2, gtsamPoint2(4.1, 1.8));
+initialEstimate.insertPoint(j1, Point2(1.8, 2.1));
+initialEstimate.insertPoint(j2, Point2(4.1, 1.8));
 
 initialEstimate.print(sprintf('\nInitial estimate:\n'));
 
@@ -62,6 +67,7 @@ result = graph.optimize(initialEstimate,1);
 result.print(sprintf('\nFinal result:\n'));
 
 %% Plot Covariance Ellipses
+import gtsam.*
 cla;hold on
 marginals = graph.marginals(result);
 for i=1:3

matlab/examples/PlanarSLAMExample_sampling.m

@@ -12,37 +12,39 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Create the same factor graph as in PlanarSLAMExample
+import gtsam.*
 i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
-graph = planarSLAMGraph;
+graph = planarSLAM.Graph;
-priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
+priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
 graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph
-odometry = gtsamPose2(2.0, 0.0, 0.0);
+odometry = Pose2(2.0, 0.0, 0.0);
-odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
 graph.addRelativePose(i1, i2, odometry, odometryNoise);
 graph.addRelativePose(i2, i3, odometry, odometryNoise);
 
 %% Except, for measurements we offer a choice
+import gtsam.*
 j1 = symbol('l',1); j2 = symbol('l',2);
 degrees = pi/180;
-noiseModel = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.2]);
+brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
 if 1
-    graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel);
+    graph.addBearingRange(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise);
-    graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel);
+    graph.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise);
 else
-    bearingModel = gtsamnoiseModelDiagonal.Sigmas(0.1);    
+    bearingModel = noiseModel.Diagonal.Sigmas(0.1);    
-    graph.addBearing(i1, j1, gtsamRot2(45*degrees), bearingModel);
+    graph.addBearing(i1, j1, Rot2(45*degrees), bearingModel);
-    graph.addBearing(i2, j1, gtsamRot2(90*degrees), bearingModel);
+    graph.addBearing(i2, j1, Rot2(90*degrees), bearingModel);
 end
-graph.addBearingRange(i3, j2, gtsamRot2(90*degrees), 2, noiseModel);    
+graph.addBearingRange(i3, j2, Rot2(90*degrees), 2, brNoise);    
 
 %% Initialize MCMC sampler with ground truth
-sample = planarSLAMValues;
+sample = planarSLAM.Values;
-sample.insertPose(i1, gtsamPose2(0,0,0));
+sample.insertPose(i1, Pose2(0,0,0));
-sample.insertPose(i2, gtsamPose2(2,0,0));
+sample.insertPose(i2, Pose2(2,0,0));
-sample.insertPose(i3, gtsamPose2(4,0,0));
+sample.insertPose(i3, Pose2(4,0,0));
-sample.insertPoint(j1, gtsamPoint2(2,2));
+sample.insertPoint(j1, Point2(2,2));
-sample.insertPoint(j2, gtsamPoint2(4,2));
+sample.insertPoint(j2, Point2(4,2));
 
 %% Calculate and plot Covariance Ellipses
 figure(1);clf;hold on
@@ -74,6 +76,6 @@ axis equal
 N=1000;
 for s=1:N
     delta = S{2}*randn(2,1);
-    proposedPoint = gtsamPoint2(point{2}.x+delta(1),point{2}.y+delta(2));
+    proposedPoint = Point2(point{2}.x+delta(1),point{2}.y+delta(2));
     plotPoint2(proposedPoint,'k.')
 end

matlab/examples/Pose2SLAMExample.m

@@ -19,36 +19,40 @@
 %  - The robot is on a grid, moving 2 meters each step
 
 %% Create graph container and add factors to it
-graph = pose2SLAMGraph;
+graph = pose2SLAM.Graph;
 
 %% Add prior
+import gtsam.*
 % gaussian for prior
-priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
+priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
 graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
 
 %% Add odometry
+import gtsam.*
 % general noisemodel for odometry
-odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
-graph.addRelativePose(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
+graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
-graph.addRelativePose(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
+graph.addRelativePose(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise);
-graph.addRelativePose(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
+graph.addRelativePose(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise);
-graph.addRelativePose(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
+graph.addRelativePose(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise);
 
 %% Add pose constraint
-model = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
+import gtsam.*
-graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
+model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+graph.addRelativePose(5, 2, Pose2(2.0, 0.0, pi/2), model);
 
 % print
 graph.print(sprintf('\nFactor graph:\n'));
 
 %% Initialize to noisy points
-initialEstimate = pose2SLAMValues;
+import gtsam.*
-initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2 ));
+initialEstimate = pose2SLAM.Values;
-initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2 ));
+initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2 ));
-initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, pi/2));
+initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2 ));
-initialEstimate.insertPose(4, gtsamPose2(4.0, 2.0, pi  ));
+initialEstimate.insertPose(3, Pose2(4.1, 0.1, pi/2));
-initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2));
+initialEstimate.insertPose(4, Pose2(4.0, 2.0, pi  ));
+initialEstimate.insertPose(5, Pose2(2.1, 2.1,-pi/2));
 initialEstimate.print(sprintf('\nInitial estimate:\n'));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd

matlab/examples/Pose2SLAMExample_advanced.m

@@ -21,38 +21,42 @@
 %  - The robot is on a grid, moving 2 meters each step
 
 %% Create graph container and add factors to it
-graph = pose2SLAMGraph;
+graph = pose2SLAM.Graph;
 
 %% Add prior
+import gtsam.*
 % gaussian for prior
-priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
-priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
+priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
 graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
 
 %% Add odometry
+import gtsam.*
 % general noisemodel for odometry
-odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
-odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
+odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
 graph.addRelativePose(1, 2, odometry, odometryNoise);
 graph.addRelativePose(2, 3, odometry, odometryNoise);
 
 %% Add measurements
+import gtsam.*
 % general noisemodel for measurements
-measurementNoise = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.2]);
+measurementNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
 
 % print
 graph.print('full graph');
 
 %% Initialize to noisy points
-initialEstimate = pose2SLAMValues;
+import gtsam.*
-initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2));
+initialEstimate = pose2SLAM.Values;
-initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2));
+initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
-initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
+initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
+initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
 
 initialEstimate.print('initial estimate');
 
 %% set up solver, choose ordering and optimize
-%params = gtsamNonlinearOptimizationParameters_newDecreaseThresholds(1e-15, 1e-15);
+%params = NonlinearOptimizationParameters_newDecreaseThresholds(1e-15, 1e-15);
 %
 %ord = graph.orderingCOLAMD(initialEstimate);
 %

matlab/examples/Pose2SLAMExample_circle.m

@@ -11,15 +11,15 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Create a hexagon of poses
-hexagon = pose2SLAMValues.Circle(6,1.0);
+hexagon = pose2SLAM.Values.Circle(6,1.0);
 p0 = hexagon.pose(0);
 p1 = hexagon.pose(1);
 
 %% create a Pose graph with one equality constraint and one measurement
-fg = pose2SLAMGraph;
+fg = pose2SLAM.Graph;
 fg.addPoseConstraint(0, p0);
 delta = p0.between(p1);
-covariance = gtsamnoiseModelDiagonal.Sigmas([0.05; 0.05; 5*pi/180]);
+covariance = gtsam.noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);
 fg.addRelativePose(0,1, delta, covariance);
 fg.addRelativePose(1,2, delta, covariance);
 fg.addRelativePose(2,3, delta, covariance);
@@ -28,7 +28,7 @@ fg.addRelativePose(4,5, delta, covariance);
 fg.addRelativePose(5,0, delta, covariance);
 
 %% Create initial config
-initial = pose2SLAMValues;
+initial = pose2SLAM.Values;
 initial.insertPose(0, p0);
 initial.insertPose(1, hexagon.pose(1).retract([-0.1, 0.1,-0.1]'));
 initial.insertPose(2, hexagon.pose(2).retract([ 0.1,-0.1, 0.1]'));

matlab/examples/Pose2SLAMExample_graph.m

@@ -11,13 +11,15 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Initialize graph, initial estimate, and odometry noise
-model = gtsamnoiseModelDiagonal.Sigmas([0.05; 0.05; 5*pi/180]);
+import gtsam.*
+model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);
 [graph,initial]=load2D('../../examples/Data/w100-odom.graph',model);
 initial.print(sprintf('Initial estimate:\n'));
 
 %% Add a Gaussian prior on pose x_1
-priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin
+import gtsam.*
-priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.01; 0.01; 0.01]);
+priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin
+priorNoise = noiseModel.Diagonal.Sigmas([0.01; 0.01; 0.01]);
 graph.addPosePrior(0, priorMean, priorNoise); % add directly to graph
 
 %% Plot Initial Estimate

matlab/examples/Pose2SLAMwSPCG.m

@@ -17,36 +17,39 @@
 %  - The robot is on a grid, moving 2 meters each step
 
 %% Create graph container and add factors to it
-graph = pose2SLAMGraph;
+graph = pose2SLAM.Graph;
 
 %% Add prior
+import gtsam.*
 % gaussian for prior
-priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
+priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
-graph.addPrior(1, priorMean, priorNoise); % add directly to graph
+graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
 
 %% Add odometry
+import gtsam.*
 % general noisemodel for odometry
-odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
-graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
+graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
-graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
+graph.addRelativePose(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise);
-graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
+graph.addRelativePose(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise);
-graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
+graph.addRelativePose(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise);
 
 %% Add pose constraint
-model = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
+import gtsam.*
-graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
+model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+graph.addRelativePose(5, 2, Pose2(2.0, 0.0, pi/2), model);
 
 % print
 graph.print(sprintf('\nFactor graph:\n'));
 
 %% Initialize to noisy points
-initialEstimate = pose2SLAMValues;
+initialEstimate = pose2SLAM.Values;
-initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2 ));
+initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2 ));
-initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2 ));
+initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2 ));
-initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, pi/2));
+initialEstimate.insertPose(3, Pose2(4.1, 0.1, pi/2));
-initialEstimate.insertPose(4, gtsamPose2(4.0, 2.0, pi  ));
+initialEstimate.insertPose(4, Pose2(4.0, 2.0, pi  ));
-initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2));
+initialEstimate.insertPose(5, Pose2(2.1, 2.1,-pi/2));
 initialEstimate.print(sprintf('\nInitial estimate:\n'));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd

matlab/examples/Pose3SLAMExample.m

@@ -11,15 +11,15 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Create a hexagon of poses
-hexagon = pose3SLAMValues.Circle(6,1.0);
+hexagon = pose3SLAM.Values.Circle(6,1.0);
 p0 = hexagon.pose(0);
 p1 = hexagon.pose(1);
 
 %% create a Pose graph with one equality constraint and one measurement
-fg = pose3SLAMGraph;
+fg = pose3SLAM.Graph;
 fg.addPoseConstraint(0, p0);
 delta = p0.between(p1);
-covariance = gtsamnoiseModelDiagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
+covariance = gtsam.noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
 fg.addRelativePose(0,1, delta, covariance);
 fg.addRelativePose(1,2, delta, covariance);
 fg.addRelativePose(2,3, delta, covariance);
@@ -28,7 +28,7 @@ fg.addRelativePose(4,5, delta, covariance);
 fg.addRelativePose(5,0, delta, covariance);
 
 %% Create initial config
-initial = pose3SLAMValues;
+initial = pose3SLAM.Values;
 s = 0.10;
 initial.insertPose(0, p0);
 initial.insertPose(1, hexagon.pose(1).retract(s*randn(6,1)));

matlab/examples/Pose3SLAMExample_graph.m

@@ -16,7 +16,7 @@ N = 2500;
 filename = '../../examples/Data/sphere2500.txt';
 
 %% Initialize graph, initial estimate, and odometry noise
-model = gtsamnoiseModelDiagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
+model = gtsam.noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
 [graph,initial]=load3D(filename,model,true,N);
 
 %% Plot Initial Estimate
@@ -27,6 +27,6 @@ plot3DTrajectory(initial,'g-',false);
 
 %% Read again, now with all constraints, and optimize
 graph = load3D(filename,model,false,N);
-graph.addHardConstraint(0, first);
+graph.addPoseConstraint(0, first);
 result = graph.optimize(initial);
 plot3DTrajectory(result,'r-',false); axis equal;

matlab/examples/SBAExample.m

@@ -30,11 +30,12 @@ cameraNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1 ...
                      0.001*ones(1,5)]';
 
 %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph)
-graph = sparseBAGraph;
+graph = sparseBA.Graph;
 
  
 %% Add factors for all measurements
-measurementNoise = gtsamnoiseModelIsotropic.Sigma(2,measurementNoiseSigma);
+import gtsam.*
+measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma);
 for i=1:length(data.Z)
     for k=1:length(data.Z{i})
         j = data.J{i}{k};
@@ -43,11 +44,12 @@ for i=1:length(data.Z)
 end
 
 %% Add Gaussian priors for a pose and a landmark to constrain the system
-cameraPriorNoise  = gtsamnoiseModelDiagonal.Sigmas(cameraNoiseSigmas);
+import gtsam.*
-firstCamera = gtsamSimpleCamera(truth.cameras{1}.pose, truth.K);
+cameraPriorNoise  = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas);
+firstCamera = SimpleCamera(truth.cameras{1}.pose, truth.K);
 graph.addSimpleCameraPrior(symbol('c',1), firstCamera, cameraPriorNoise);
 
-pointPriorNoise  = gtsamnoiseModelIsotropic.Sigma(3,pointNoiseSigma);
+pointPriorNoise  = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
 graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise);
 
 %% Print the graph
@@ -55,10 +57,11 @@ graph.print(sprintf('\nFactor graph:\n'));
 
 
 %% Initialize cameras and points close to ground truth in this example
-initialEstimate = sparseBAValues;
+import gtsam.*
+initialEstimate = sparseBA.Values;
 for i=1:size(truth.cameras,2)
     pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1));
-    camera_i = gtsamSimpleCamera(pose_i, truth.K);
+    camera_i = SimpleCamera(pose_i, truth.K);
     initialEstimate.insertSimpleCamera(symbol('c',i), camera_i);
 end
 for j=1:size(truth.points,2)
@@ -69,7 +72,8 @@ initialEstimate.print(sprintf('\nInitial estimate:\n  '));
 
 %% Fine grain optimization, allowing user to iterate step by step
 
-parameters = gtsamLevenbergMarquardtParams;
+import gtsam.*
+parameters = LevenbergMarquardtParams;
 parameters.setlambdaInitial(1.0);
 parameters.setVerbosityLM('trylambda');
 

matlab/examples/SFMExample.m

@@ -29,10 +29,11 @@ pointNoiseSigma = 0.1;
 poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
 
 %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph)
-graph = visualSLAMGraph;
+graph = visualSLAM.Graph;
 
 %% Add factors for all measurements
-measurementNoise = gtsamnoiseModelIsotropic.Sigma(2,measurementNoiseSigma);
+import gtsam.*
+measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma);
 for i=1:length(data.Z)
     for k=1:length(data.Z{i})
         j = data.J{i}{k};
@@ -41,16 +42,17 @@ for i=1:length(data.Z)
 end
 
 %% Add Gaussian priors for a pose and a landmark to constrain the system
-posePriorNoise  = gtsamnoiseModelDiagonal.Sigmas(poseNoiseSigmas);
+import gtsam.*
+posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
 graph.addPosePrior(symbol('x',1), truth.cameras{1}.pose, posePriorNoise);
-pointPriorNoise  = gtsamnoiseModelIsotropic.Sigma(3,pointNoiseSigma);
+pointPriorNoise  = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
 graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise);
 
 %% Print the graph
 graph.print(sprintf('\nFactor graph:\n'));
 
 %% Initialize cameras and points close to ground truth in this example
-initialEstimate = visualSLAMValues;
+initialEstimate = visualSLAM.Values;
 for i=1:size(truth.cameras,2)
     pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1));
     initialEstimate.insertPose(symbol('x',i), pose_i);
@@ -63,7 +65,8 @@ initialEstimate.print(sprintf('\nInitial estimate:\n  '));
 
 %% Fine grain optimization, allowing user to iterate step by step
 
-parameters = gtsamLevenbergMarquardtParams;
+import gtsam.*
+parameters = LevenbergMarquardtParams;
 parameters.setlambdaInitial(1.0);
 parameters.setVerbosityLM('trylambda');
 

matlab/examples/StereoVOExample.m

@@ -22,37 +22,41 @@ x1 = symbol('x',1); x2 = symbol('x',2);
 l1 = symbol('l',1); l2 = symbol('l',2); l3 = symbol('l',3);
 
 %% Create graph container and add factors to it
-graph = visualSLAMGraph;
+graph = visualSLAM.Graph;
 
 %% add a constraint on the starting pose
-first_pose = gtsamPose3();
+import gtsam.*
+first_pose = Pose3();
 graph.addPoseConstraint(x1, first_pose);
 
 %% Create realistic calibration and measurement noise model
 % format: fx fy skew cx cy baseline
-K = gtsamCal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
+import gtsam.*
-stereo_model = gtsamnoiseModelDiagonal.Sigmas([1.0; 1.0; 1.0]);
+K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
+stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
 
 %% Add measurements
+import gtsam.*
 % pose 1
-graph.addStereoMeasurement(gtsamStereoPoint2(520, 480, 440), stereo_model, x1, l1, K);
+graph.addStereoMeasurement(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K);
-graph.addStereoMeasurement(gtsamStereoPoint2(120,  80, 440), stereo_model, x1, l2, K);
+graph.addStereoMeasurement(StereoPoint2(120,  80, 440), stereo_model, x1, l2, K);
-graph.addStereoMeasurement(gtsamStereoPoint2(320, 280, 140), stereo_model, x1, l3, K);
+graph.addStereoMeasurement(StereoPoint2(320, 280, 140), stereo_model, x1, l3, K);
 
 %pose 2
-graph.addStereoMeasurement(gtsamStereoPoint2(570, 520, 490), stereo_model, x2, l1, K);
+graph.addStereoMeasurement(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K);
-graph.addStereoMeasurement(gtsamStereoPoint2( 70,  20, 490), stereo_model, x2, l2, K);
+graph.addStereoMeasurement(StereoPoint2( 70,  20, 490), stereo_model, x2, l2, K);
-graph.addStereoMeasurement(gtsamStereoPoint2(320, 270, 115), stereo_model, x2, l3, K);
+graph.addStereoMeasurement(StereoPoint2(320, 270, 115), stereo_model, x2, l3, K);
 
 
 %% Create initial estimate for camera poses and landmarks
-initialEstimate = visualSLAMValues;
+import gtsam.*
+initialEstimate = visualSLAM.Values;
 initialEstimate.insertPose(x1, first_pose);
 % noisy estimate for pose 2
-initialEstimate.insertPose(x2, gtsamPose3(gtsamRot3(), gtsamPoint3(0.1,-.1,1.1)));
+initialEstimate.insertPose(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1)));
-initialEstimate.insertPoint(l1, gtsamPoint3( 1,  1, 5));
+initialEstimate.insertPoint(l1, Point3( 1,  1, 5));
-initialEstimate.insertPoint(l2, gtsamPoint3(-1,  1, 5));
+initialEstimate.insertPoint(l2, Point3(-1,  1, 5));
-initialEstimate.insertPoint(l3, gtsamPoint3( 0,-.5, 5));
+initialEstimate.insertPoint(l3, Point3( 0,-.5, 5));
 
 %% optimize
 fprintf(1,'Optimizing\n'); tic

matlab/examples/StereoVOExample_large.m

@@ -11,40 +11,43 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Load calibration
+import gtsam.*
 % format: fx fy skew cx cy baseline
 calib = dlmread('../../examples/Data/VO_calibration.txt');
-K = gtsamCal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6));
+K = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6));
-stereo_model = gtsamnoiseModelDiagonal.Sigmas([1.0; 1.0; 1.0]);
+stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
 
 %% create empty graph and values
-graph = visualSLAMGraph;
+graph = visualSLAM.Graph;
-initial = visualSLAMValues;
+initial = visualSLAM.Values;
 
 
 %% load the initial poses from VO
 % row format: camera_id 4x4 pose (row, major)
+import gtsam.*
 fprintf(1,'Reading data\n');
 cameras = dlmread('../../examples/Data/VO_camera_poses_large.txt');
 for i=1:size(cameras,1)
-    pose = gtsamPose3(reshape(cameras(i,2:17),4,4)');
+    pose = Pose3(reshape(cameras(i,2:17),4,4)');
     initial.insertPose(symbol('x',cameras(i,1)),pose);
 end
 
 %% load stereo measurements and initialize landmarks
 % camera_id landmark_id uL uR v X Y Z
+import gtsam.*
 measurements = dlmread('../../examples/Data/VO_stereo_factors_large.txt');
 
 fprintf(1,'Creating Graph\n'); tic
 for i=1:size(measurements,1)
     sf = measurements(i,:);
-    graph.addStereoMeasurement(gtsamStereoPoint2(sf(3),sf(4),sf(5)), stereo_model, ...
+    graph.addStereoMeasurement(StereoPoint2(sf(3),sf(4),sf(5)), stereo_model, ...
         symbol('x', sf(1)), symbol('l', sf(2)), K);
     
     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.pose(symbol('x', sf(1)));
-        world_point = pose.transform_from(gtsamPoint3(sf(6),sf(7),sf(8)));
+        world_point = pose.transform_from(Point3(sf(6),sf(7),sf(8)));
         initial.insertPoint(symbol('l',sf(2)), world_point);
     end
 end

matlab/load2D.m

@@ -13,17 +13,17 @@ fclose(fid);
 lines=columns{1};
 
 % loop over lines and add vertices
-graph = pose2SLAMGraph;
+graph = pose2SLAM.Graph;
-initial = pose2SLAMValues;
+initial = pose2SLAM.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.insertPose(v{2}, gtsamPose2(v{3}, v{4}, v{5}));
+        initial.insertPose(v{2}, gtsam.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.addOdometry(e{2}, e{3}, gtsamPose2(e{4}, e{5}, e{6}), model);
+        graph.addRelativePose(e{2}, e{3}, gtsam.Pose2(e{4}, e{5}, e{6}), model);
     end
 end
 

matlab/load3D.m

@@ -15,9 +15,9 @@ fclose(fid);
 lines=columns{1};
 
 % loop over lines and add vertices
-graph = pose3SLAMGraph;
+graph = pose3SLAM.Graph;
-initial = pose3SLAMValues;
+initial = pose3SLAM.Values;
-origin=gtsamPose3;
+origin=gtsam.Pose3;
 initial.insertPose(0,origin);
 n=size(lines,1);
 if nargin<4, N=n;end
@@ -26,21 +26,21 @@ for i=1:n
     line_i=lines{i};
     if strcmp('VERTEX3',line_i(1:7))
         v = textscan(line_i,'%s %d %f %f %f %f %f %f',1);
-        i=v{2};
+        i1=v{2};
-        if (~successive & i<N | successive & i==0)
+        if (~successive && i1<N || successive && i1==0)
-            t = gtsamPoint3(v{3}, v{4}, v{5});
+            t = gtsam.Point3(v{3}, v{4}, v{5});
-            R = gtsamRot3.Ypr(v{8}, -v{7}, v{6});
+            R = gtsam.Rot3.Ypr(v{8}, -v{7}, v{6});
-            initial.insertPose(i, gtsamPose3(R,t));
+            initial.insertPose(i1, gtsam.Pose3(R,t));
         end
     elseif strcmp('EDGE3',line_i(1:5))
         e = textscan(line_i,'%s %d %d %f %f %f %f %f %f',1);
         i1=e{2};
         i2=e{3};
-        if i1<N & i2<N
+        if i1<N && i2<N
-            if ~successive | abs(i2-i1)==1
+            if ~successive || abs(i2-i1)==1
-                t = gtsamPoint3(e{4}, e{5}, e{6});
+                t = gtsam.Point3(e{4}, e{5}, e{6});
-                R = gtsamRot3.Ypr(e{9}, e{8}, e{7});
+                R = gtsam.Rot3.Ypr(e{9}, e{8}, e{7});
-                dpose = gtsamPose3(R,t);
+                dpose = gtsam.Pose3(R,t);
                 graph.addRelativePose(i1, i2, dpose, model);
                 if successive
                     if i2>i1

matlab/symbol.m

@@ -1,4 +1,4 @@
 function key = symbol(c,i)
 % generate a key corresponding to a symbol
-s = gtsamSymbol(c,i);
+s = gtsam.Symbol(c,i);
 key = s.key();

matlab/symbolChr.m

@@ -1,4 +1,4 @@
 function c = symbolChr(key)
 % generate the chr from a key 
-s = gtsamSymbol(key);
+s = gtsam.Symbol(key);
 c = s.chr();

matlab/symbolIndex.m

@@ -1,4 +1,4 @@
 function i = symbolIndex(key)
 % generate the index from a key 
-s = gtsamSymbol(key);
+s = gtsam.Symbol(key);
 i = s.index();

matlab/tests/testJacobianFactor.m

@@ -1,6 +1,8 @@
 %-----------------------------------------------------------------------
 % eliminate
 
+import gtsam.*
+
 % the combined linear factor
 Ax2 = [
 -5., 0.
@@ -30,8 +32,8 @@ x1 = 3;
 % the RHS
 b2=[-1;1.5;2;-1];
 sigmas = [1;1;1;1];
-model4 = gtsamnoiseModelDiagonal.Sigmas(sigmas);
+model4 = noiseModel.Diagonal.Sigmas(sigmas);
-combined = gtsamJacobianFactor(x2, Ax2,  l1, Al1, x1, Ax1, b2, model4);
+combined = JacobianFactor(x2, Ax2,  l1, Al1, x1, Ax1, b2, model4);
 
 % eliminate the first variable (x2) in the combined factor, destructive !
 actualCG = combined.eliminateFirst();
@@ -50,7 +52,7 @@ S13 = [
 +0.00,-8.94427
 ];
 d=[2.23607;-1.56525];
-expectedCG = gtsamGaussianConditional(x2,d,R11,l1,S12,x1,S13,[1;1]);
+expectedCG = GaussianConditional(x2,d,R11,l1,S12,x1,S13,[1;1]);
 % check if the result matches
 CHECK('actualCG.equals(expectedCG,1e-5)',actualCG.equals(expectedCG,1e-4));
 
@@ -69,8 +71,8 @@ Bx1 = [
 % the RHS
 b1= [0.0;0.894427];
 
-model2 = gtsamnoiseModelDiagonal.Sigmas([1;1]);
+model2 = noiseModel.Diagonal.Sigmas([1;1]);
-expectedLF = gtsamJacobianFactor(l1, Bl1, x1, Bx1, b1, model2);
+expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2);
 
 % check if the result matches the combined (reduced) factor
 % FIXME: JacobianFactor/GaussianFactor mismatch

matlab/tests/testKalmanFilter.m

@@ -19,16 +19,17 @@
 %  */
 
 %% Create the controls and measurement properties for our example
+import gtsam.*
 F = eye(2,2);
 B = eye(2,2);
 u = [1.0; 0.0];
-modelQ = gtsamnoiseModelDiagonal.Sigmas([0.1;0.1]);
+modelQ = noiseModel.Diagonal.Sigmas([0.1;0.1]);
 Q = 0.01*eye(2,2);
 H = eye(2,2);
 z1 = [1.0, 0.0]';
 z2 = [2.0, 0.0]';
 z3 = [3.0, 0.0]';
-modelR = gtsamnoiseModelDiagonal.Sigmas([0.1;0.1]);
+modelR = noiseModel.Diagonal.Sigmas([0.1;0.1]);
 R = 0.01*eye(2,2);
 
 %% Create the set of expected output TestValues
@@ -48,7 +49,8 @@ P23 = inv(I22) + Q;
 I33 = inv(P23) + inv(R);
 
 %% Create an KalmanFilter object
-KF = gtsamKalmanFilter(2);
+import gtsam.*
+KF = KalmanFilter(2);
 
 %% Create the Kalman Filter initialization point
 x_initial = [0.0;0.0];

matlab/tests/testLocalizationExample.m

@@ -11,30 +11,32 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
-graph = pose2SLAMGraph;
+graph = pose2SLAM.Graph;
 
 %% Add two odometry factors
-odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
+import gtsam.*
-odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
 graph.addRelativePose(1, 2, odometry, odometryNoise);
 graph.addRelativePose(2, 3, odometry, odometryNoise);
 
 %% Add three "GPS" measurements
 % We use Pose2 Priors here with high variance on theta
-groundTruth = pose2SLAMValues;
+import gtsam.*
-groundTruth.insertPose(1, gtsamPose2(0.0, 0.0, 0.0));
+groundTruth = pose2SLAM.Values;
-groundTruth.insertPose(2, gtsamPose2(2.0, 0.0, 0.0));
+groundTruth.insertPose(1, Pose2(0.0, 0.0, 0.0));
-groundTruth.insertPose(3, gtsamPose2(4.0, 0.0, 0.0));
+groundTruth.insertPose(2, Pose2(2.0, 0.0, 0.0));
-noiseModel = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.1; 10]);
+groundTruth.insertPose(3, Pose2(4.0, 0.0, 0.0));
+model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]);
 for i=1:3
-    graph.addPosePrior(i, groundTruth.pose(i), noiseModel);
+    graph.addPosePrior(i, groundTruth.pose(i), model);
 end
 
 %% Initialize to noisy points
-initialEstimate = pose2SLAMValues;
+initialEstimate = pose2SLAM.Values;
-initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2));
+initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
-initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2));
+initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
-initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
+initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
 result = graph.optimize(initialEstimate,0);

matlab/tests/testOdometryExample.m

@@ -11,24 +11,27 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
-graph = pose2SLAMGraph;
+graph = pose2SLAM.Graph;
 
 %% Add a Gaussian prior on pose x_1
-priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior mean is at origin
+import gtsam.*
-priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
+priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
 graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
 
 %% Add two odometry factors
-odometry = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
+import gtsam.*
-odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
 graph.addRelativePose(1, 2, odometry, odometryNoise);
 graph.addRelativePose(2, 3, odometry, odometryNoise);
 
 %% Initialize to noisy points
-initialEstimate = pose2SLAMValues;
+import gtsam.*
-initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2));
+initialEstimate = pose2SLAM.Values;
-initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2));
+initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
-initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1));
+initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
+initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
 result = graph.optimize(initialEstimate,0);
@@ -37,4 +40,4 @@ marginals.marginalCovariance(1);
 
 %% Check first pose equality
 pose_1 = result.pose(1);
-CHECK('pose_1.equals(gtsamPose2,1e-4)',pose_1.equals(gtsamPose2,1e-4));
+CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));

matlab/tests/testPlanarSLAMExample.m

@@ -24,45 +24,50 @@ i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
 j1 = symbol('l',1); j2 = symbol('l',2);
 
 %% Create graph container and add factors to it
-graph = planarSLAMGraph;
+graph = planarSLAM.Graph;
 
 %% Add prior
-priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
+import gtsam.*
-priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]);
+priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
 graph.addPosePrior(i1, priorMean, priorNoise); % add directly to graph
 
 %% Add odometry
-odometry = gtsamPose2(2.0, 0.0, 0.0);
+import gtsam.*
-odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
+odometry = Pose2(2.0, 0.0, 0.0);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
 graph.addRelativePose(i1, i2, odometry, odometryNoise);
 graph.addRelativePose(i2, i3, odometry, odometryNoise);
 
 %% Add bearing/range measurement factors
+import gtsam.*
 degrees = pi/180;
-noiseModel = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.2]);
+brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
-graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel);
+graph.addBearingRange(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise);
-graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel);
+graph.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise);
-graph.addBearingRange(i3, j2, gtsamRot2(90*degrees), 2, noiseModel);
+graph.addBearingRange(i3, j2, Rot2(90*degrees), 2, brNoise);
 
 %% Initialize to noisy points
-initialEstimate = planarSLAMValues;
+import gtsam.*
-initialEstimate.insertPose(i1, gtsamPose2(0.5, 0.0, 0.2));
+initialEstimate = planarSLAM.Values;
-initialEstimate.insertPose(i2, gtsamPose2(2.3, 0.1,-0.2));
+initialEstimate.insertPose(i1, Pose2(0.5, 0.0, 0.2));
-initialEstimate.insertPose(i3, gtsamPose2(4.1, 0.1, 0.1));
+initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2));
-initialEstimate.insertPoint(j1, gtsamPoint2(1.8, 2.1));
+initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1));
-initialEstimate.insertPoint(j2, gtsamPoint2(4.1, 1.8));
+initialEstimate.insertPoint(j1, Point2(1.8, 2.1));
+initialEstimate.insertPoint(j2, Point2(4.1, 1.8));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
 result = graph.optimize(initialEstimate,0);
 marginals = graph.marginals(result);
 
 %% Check first pose and point equality
+import gtsam.*
 pose_1 = result.pose(symbol('x',1));
 marginals.marginalCovariance(symbol('x',1));
-CHECK('pose_1.equals(gtsamPose2,1e-4)',pose_1.equals(gtsamPose2,1e-4));
+CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
 
 point_1 = result.point(symbol('l',1));
 marginals.marginalCovariance(symbol('l',1));
-CHECK('point_1.equals(gtsamPoint2(2,2),1e-4)',point_1.equals(gtsamPoint2(2,2),1e-4));
+CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4));
 
 

matlab/tests/testPose2SLAMExample.m

@@ -19,33 +19,37 @@
 %  - The robot is on a grid, moving 2 meters each step
 
 %% Create graph container and add factors to it
-graph = pose2SLAMGraph;
+graph = pose2SLAM.Graph;
 
 %% Add prior
+import gtsam.*
 % gaussian for prior
-priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
+priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
 graph.addPosePrior(1, priorMean, priorNoise); % add directly to graph
 
 %% Add odometry
 % general noisemodel for odometry
-odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
+import gtsam.*
-graph.addRelativePose(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
-graph.addRelativePose(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
+graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
-graph.addRelativePose(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
+graph.addRelativePose(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise);
-graph.addRelativePose(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise);
+graph.addRelativePose(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise);
+graph.addRelativePose(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise);
 
 %% Add pose constraint
-model = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]);
+import gtsam.*
-graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model);
+model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+graph.addRelativePose(5, 2, Pose2(2.0, 0.0, pi/2), model);
 
 %% Initialize to noisy points
-initialEstimate = pose2SLAMValues;
+import gtsam.*
-initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2 ));
+initialEstimate = pose2SLAM.Values;
-initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2 ));
+initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2 ));
-initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, pi/2));
+initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2 ));
-initialEstimate.insertPose(4, gtsamPose2(4.0, 2.0, pi  ));
+initialEstimate.insertPose(3, Pose2(4.1, 0.1, pi/2));
-initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2));
+initialEstimate.insertPose(4, Pose2(4.0, 2.0, pi  ));
+initialEstimate.insertPose(5, Pose2(2.1, 2.1,-pi/2));
 
 %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
 result = graph.optimize(initialEstimate,0);
@@ -56,9 +60,9 @@ marginals = graph.marginals(result);
 P = marginals.marginalCovariance(1);
 
 pose_1 = result.pose(1);
-CHECK('pose_1.equals(gtsamPose2,1e-4)',pose_1.equals(gtsamPose2,1e-4));
+CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4));
 
 poseSPCG_1 = resultSPCG.pose(1);
-CHECK('poseSPCG_1.equals(gtsamPose2,1e-4)',poseSPCG_1.equals(gtsamPose2,1e-4));
+CHECK('poseSPCG_1.equals(Pose2,1e-4)',poseSPCG_1.equals(Pose2,1e-4));
 
 

matlab/tests/testPose3SLAMExample.m

@@ -11,15 +11,16 @@
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 %% Create a hexagon of poses
-hexagon = pose3SLAMValues.Circle(6,1.0);
+hexagon = pose3SLAM.Values.Circle(6,1.0);
 p0 = hexagon.pose(0);
 p1 = hexagon.pose(1);
 
 %% create a Pose graph with one equality constraint and one measurement
-fg = pose3SLAMGraph;
+import gtsam.*
+fg = pose3SLAM.Graph;
 fg.addPoseConstraint(0, p0);
 delta = p0.between(p1);
-covariance = gtsamnoiseModelDiagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
+covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
 fg.addRelativePose(0,1, delta, covariance);
 fg.addRelativePose(1,2, delta, covariance);
 fg.addRelativePose(2,3, delta, covariance);
@@ -28,7 +29,7 @@ fg.addRelativePose(4,5, delta, covariance);
 fg.addRelativePose(5,0, delta, covariance);
 
 %% Create initial config
-initial = pose3SLAMValues;
+initial = pose3SLAM.Values;
 s = 0.10;
 initial.insertPose(0, p0);
 initial.insertPose(1, hexagon.pose(1).retract(s*randn(6,1)));
@@ -41,6 +42,6 @@ initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1)));
 result = fg.optimize(initial,0);
 
 pose_1 = result.pose(1);
-CHECK('pose_1.equals(gtsamPose3,1e-4)',pose_1.equals(p1,1e-4));
+CHECK('pose_1.equals(Pose3,1e-4)',pose_1.equals(p1,1e-4));
 
 

matlab/tests/testSFMExample.m

@@ -20,10 +20,11 @@ measurementNoiseSigma = 1.0;
 pointNoiseSigma = 0.1;
 poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
 
-graph = visualSLAMGraph;
+graph = visualSLAM.Graph;
 
 %% Add factors for all measurements
-measurementNoise = gtsamnoiseModelIsotropic.Sigma(2,measurementNoiseSigma);
+import gtsam.*
+measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma);
 for i=1:length(data.Z)
     for k=1:length(data.Z{i})
         j = data.J{i}{k};
@@ -31,13 +32,13 @@ for i=1:length(data.Z)
     end
 end
 
-posePriorNoise  = gtsamnoiseModelDiagonal.Sigmas(poseNoiseSigmas);
+posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
 graph.addPosePrior(symbol('x',1), truth.cameras{1}.pose, posePriorNoise);
-pointPriorNoise  = gtsamnoiseModelIsotropic.Sigma(3,pointNoiseSigma);
+pointPriorNoise  = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
 graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise);
 
 %% Initial estimate
-initialEstimate = visualSLAMValues;
+initialEstimate = visualSLAM.Values;
 for i=1:size(truth.cameras,2)
     pose_i = truth.cameras{i}.pose;
     initialEstimate.insertPose(symbol('x',i), pose_i);
@@ -48,7 +49,8 @@ for j=1:size(truth.points,2)
 end
 
 %% Optimization
-parameters = gtsamLevenbergMarquardtParams;
+import gtsam.*
+parameters = LevenbergMarquardtParams;
 optimizer = graph.optimizer(initialEstimate, parameters);
 for i=1:5
     optimizer.iterate();

matlab/tests/testStereoVOExample.m

@@ -22,38 +22,42 @@ x1 = symbol('x',1); x2 = symbol('x',2);
 l1 = symbol('l',1); l2 = symbol('l',2); l3 = symbol('l',3);
 
 %% Create graph container and add factors to it
-graph = visualSLAMGraph;
+graph = visualSLAM.Graph;
 
 %% add a constraint on the starting pose
-first_pose = gtsamPose3();
+import gtsam.*
+first_pose = Pose3();
 graph.addPoseConstraint(x1, first_pose);
 
 %% Create realistic calibration and measurement noise model
 % format: fx fy skew cx cy baseline
-K = gtsamCal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
+import gtsam.*
-stereo_model = gtsamnoiseModelDiagonal.Sigmas([1.0; 1.0; 1.0]);
+K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
+stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
 
 %% Add measurements
+import gtsam.*
 % pose 1
-graph.addStereoMeasurement(gtsamStereoPoint2(520, 480, 440), stereo_model, x1, l1, K);
+graph.addStereoMeasurement(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K);
-graph.addStereoMeasurement(gtsamStereoPoint2(120,  80, 440), stereo_model, x1, l2, K);
+graph.addStereoMeasurement(StereoPoint2(120,  80, 440), stereo_model, x1, l2, K);
-graph.addStereoMeasurement(gtsamStereoPoint2(320, 280, 140), stereo_model, x1, l3, K);
+graph.addStereoMeasurement(StereoPoint2(320, 280, 140), stereo_model, x1, l3, K);
 
 %pose 2
-graph.addStereoMeasurement(gtsamStereoPoint2(570, 520, 490), stereo_model, x2, l1, K);
+graph.addStereoMeasurement(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K);
-graph.addStereoMeasurement(gtsamStereoPoint2( 70,  20, 490), stereo_model, x2, l2, K);
+graph.addStereoMeasurement(StereoPoint2( 70,  20, 490), stereo_model, x2, l2, K);
-graph.addStereoMeasurement(gtsamStereoPoint2(320, 270, 115), stereo_model, x2, l3, K);
+graph.addStereoMeasurement(StereoPoint2(320, 270, 115), stereo_model, x2, l3, K);
 
 
 %% Create initial estimate for camera poses and landmarks
-initialEstimate = visualSLAMValues;
+import gtsam.*
+initialEstimate = visualSLAM.Values;
 initialEstimate.insertPose(x1, first_pose);
 % noisy estimate for pose 2
-initialEstimate.insertPose(x2, gtsamPose3(gtsamRot3(), gtsamPoint3(0.1,-.1,1.1)));
+initialEstimate.insertPose(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1)));
-expected_l1 = gtsamPoint3( 1,  1, 5);
+expected_l1 = Point3( 1,  1, 5);
 initialEstimate.insertPoint(l1, expected_l1);
-initialEstimate.insertPoint(l2, gtsamPoint3(-1,  1, 5));
+initialEstimate.insertPoint(l2, Point3(-1,  1, 5));
-initialEstimate.insertPoint(l3, gtsamPoint3( 0,-.5, 5));
+initialEstimate.insertPoint(l3, Point3( 0,-.5, 5));
 
 %% optimize
 result = graph.optimize(initialEstimate,0);

wrap/Argument.cpp

@@ -48,7 +48,7 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
   file.oss << "  ";
 
   string cppType = qualifiedType("::");
-  string matlabType = qualifiedType();
+  string matlabUniqueType = qualifiedType();
 
   if (is_ptr)
 		// A pointer: emit an "unwrap_shared_ptr" call which returns a pointer
@@ -65,7 +65,7 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
 		file.oss << cppType << " " << name << " = unwrap< ";
 
 	file.oss << cppType << " >(" << matlabName;
-  if (is_ptr || is_ref) file.oss << ", \"ptr_" << matlabType << "\"";
+  if (is_ptr || is_ref) file.oss << ", \"ptr_" << matlabUniqueType << "\"";
   file.oss << ");" << endl;
 }
 

wrap/Class.cpp

@@ -24,6 +24,7 @@
 #include <stdint.h> // works on Linux GCC
 
 #include <boost/foreach.hpp>
+#include <boost/filesystem.hpp>
 #include <boost/lexical_cast.hpp>
 
 #include "Class.h"
@@ -34,29 +35,47 @@ using namespace std;
 using namespace wrap;
 
 /* ************************************************************************* */
-void Class::matlab_proxy(const string& classFile, const string& wrapperName,
+void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
 												 const TypeAttributesTable& typeAttributes,
 												 FileWriter& wrapperFile, vector<string>& functionNames) const {
-  // open destination classFile
+  
+	// Create namespace folders
+	{
+		using namespace boost::filesystem;
+		path curPath = toolboxPath;
+		BOOST_FOREACH(const string& subdir, namespaces) {
+			curPath /= "+" + subdir;
+			if(!is_directory(curPath))
+				if(exists("+" + subdir))
+					throw OutputError("Need to write files to directory " + curPath.string() + ", which already exists as a file but is not a directory");
+				else
+					boost::filesystem::create_directory(curPath);
+		}
+	}
+
+	// open destination classFile
+	string classFile = toolboxPath;
+	if(!namespaces.empty())
+		classFile += "/+" + wrap::qualifiedName("/+", namespaces);
+	classFile += "/" + name + ".m";
   FileWriter proxyFile(classFile, verbose_, "%");
 
   // get the name of actual matlab object
-  const string matlabName = qualifiedName(), cppName = qualifiedName("::");
+  const string matlabQualName = qualifiedName("."), matlabUniqueName = qualifiedName(), cppName = qualifiedName("::");
-	const string matlabBaseName = wrap::qualifiedName("", qualifiedParent);
+	const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent);
 	const string cppBaseName = wrap::qualifiedName("::", qualifiedParent);
 
   // emit class proxy code
   // we want our class to inherit the handle class for memory purposes
-	const string parent = qualifiedParent.empty() ?
+	const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName;
-		"handle" : ::wrap::qualifiedName("", qualifiedParent);
+  proxyFile.oss << "classdef " << name << " < " << parent << endl;
-  proxyFile.oss << "classdef " << matlabName << " < " << parent << endl;
   proxyFile.oss << "  properties" << endl;
-  proxyFile.oss << "    ptr_" << matlabName << " = 0" << endl;
+  proxyFile.oss << "    ptr_" << matlabUniqueName << " = 0" << endl;
   proxyFile.oss << "  end" << endl;
   proxyFile.oss << "  methods" << endl;
 
   // Constructor
-  proxyFile.oss << "    function obj = " << matlabName << "(varargin)" << endl;
+  proxyFile.oss << "    function obj = " << name << "(varargin)" << endl;
   // Special pointer constructors - one in MATLAB to create an object and
 	// assign a pointer returned from a C++ function.  In turn this MATLAB
 	// constructor calls a special C++ function that just adds the object to
@@ -70,26 +89,26 @@ void Class::matlab_proxy(const string& classFile, const string& wrapperName,
   BOOST_FOREACH(ArgumentList a, constructor.args_list)
   {
 		const int id = (int)functionNames.size();
-		constructor.proxy_fragment(proxyFile, wrapperName, matlabName, matlabBaseName, id, a);
+		constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), id, a);
 		const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile,
-			cppName, matlabName, cppBaseName, id, using_namespaces, a);
+			cppName, matlabUniqueName, cppBaseName, id, using_namespaces, a);
 		wrapperFile.oss << "\n";
     functionNames.push_back(wrapFunctionName);
   }
   proxyFile.oss << "      else\n";
-	proxyFile.oss << "        error('Arguments do not match any overload of " << matlabName << " constructor');" << endl;
+	proxyFile.oss << "        error('Arguments do not match any overload of " << matlabQualName << " constructor');" << endl;
 	proxyFile.oss << "      end\n";
 	if(!qualifiedParent.empty())
 		proxyFile.oss << "      obj = obj@" << matlabBaseName << "(uint64(" << ptr_constructor_key << "), base_ptr);\n";
-	proxyFile.oss << "      obj.ptr_" << matlabName << " = my_ptr;\n";
+	proxyFile.oss << "      obj.ptr_" << matlabUniqueName << " = my_ptr;\n";
   proxyFile.oss << "    end\n\n";
 
 	// Deconstructor
 	{
 		const int id = (int)functionNames.size();
-		deconstructor.proxy_fragment(proxyFile, wrapperName, matlabName, id);
+		deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id);
 		proxyFile.oss << "\n";
-		const string functionName = deconstructor.wrapper_fragment(wrapperFile, cppName, matlabName, id, using_namespaces);
+		const string functionName = deconstructor.wrapper_fragment(wrapperFile, cppName, matlabUniqueName, id, using_namespaces);
 		wrapperFile.oss << "\n";
 		functionNames.push_back(functionName);
 	}
@@ -99,7 +118,7 @@ void Class::matlab_proxy(const string& classFile, const string& wrapperName,
 	// Methods
 	BOOST_FOREACH(const Methods::value_type& name_m, methods) {
 		const Method& m = name_m.second;
-		m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabName, wrapperName, using_namespaces, typeAttributes, functionNames);
+		m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, using_namespaces, typeAttributes, functionNames);
 		proxyFile.oss << "\n";
 		wrapperFile.oss << "\n";
 	}
@@ -111,7 +130,7 @@ void Class::matlab_proxy(const string& classFile, const string& wrapperName,
 	// Static methods
 	BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) {
 		const StaticMethod& m = name_m.second;
-		m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabName, wrapperName, using_namespaces, typeAttributes, functionNames);
+		m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, using_namespaces, typeAttributes, functionNames);
 		proxyFile.oss << "\n";
 		wrapperFile.oss << "\n";
 	}
@@ -131,19 +150,18 @@ string Class::qualifiedName(const string& delim) const {
 /* ************************************************************************* */
 void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const string& wrapperName, vector<string>& functionNames) const {
 
-  const string matlabName = qualifiedName(), cppName = qualifiedName("::");
+  const string matlabUniqueName = qualifiedName(), cppName = qualifiedName("::");
-	const string baseMatlabName = wrap::qualifiedName("", qualifiedParent);
 	const string baseCppName = wrap::qualifiedName("::", qualifiedParent);
 
 	const int collectorInsertId = (int)functionNames.size();
-	const string collectorInsertFunctionName = matlabName + "_collectorInsertAndMakeBase_" + boost::lexical_cast<string>(collectorInsertId);
+	const string collectorInsertFunctionName = matlabUniqueName + "_collectorInsertAndMakeBase_" + boost::lexical_cast<string>(collectorInsertId);
 	functionNames.push_back(collectorInsertFunctionName);
 
 	int upcastFromVoidId;
 	string upcastFromVoidFunctionName;
 	if(isVirtual) {
 		upcastFromVoidId = (int)functionNames.size();
-		upcastFromVoidFunctionName = matlabName + "_upcastFromVoid_" + boost::lexical_cast<string>(upcastFromVoidId);
+		upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + boost::lexical_cast<string>(upcastFromVoidId);
 		functionNames.push_back(upcastFromVoidFunctionName);
 	}
 
@@ -184,7 +202,7 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wra
 	// Get self pointer passed in
 	wrapperFile.oss << "  Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));\n";
 	// Add to collector
-	wrapperFile.oss << "  collector_" << matlabName << ".insert(self);\n";
+	wrapperFile.oss << "  collector_" << matlabUniqueName << ".insert(self);\n";
 	// If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy)
 	if(!qualifiedParent.empty()) {
 		wrapperFile.oss << "\n";

wrap/Class.h

@@ -53,7 +53,7 @@ struct Class {
   bool verbose_;                            ///< verbose flag
 
   // And finally MATLAB code is emitted, methods below called by Module::matlab_code
-  void matlab_proxy(const std::string& classFile, const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
+  void matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
 		FileWriter& wrapperFile, std::vector<std::string>& functionNames) const;          ///< emit proxy class
   std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter
 

wrap/Constructor.cpp

@@ -38,7 +38,7 @@ string Constructor::matlab_wrapper_name(const string& className) const {
 
 /* ************************************************************************* */
 void Constructor::proxy_fragment(FileWriter& file, const std::string& wrapperName,
-        const string& matlabName, const string& matlabBaseName, const int id, const ArgumentList args) const {
+        bool hasParent, const int id, const ArgumentList args) const {
 	size_t nrArgs = args.size();
 	// check for number of arguments...
   file.oss << "      elseif nargin == " << nrArgs;
@@ -47,14 +47,14 @@ void Constructor::proxy_fragment(FileWriter& file, const std::string& wrapperNam
   bool first = true;
   for(size_t i=0;i<nrArgs;i++) {
     if (!first) file.oss << " && ";
-    file.oss << "isa(varargin{" << i+1 << "},'" << args[i].matlabClass() << "')";
+    file.oss << "isa(varargin{" << i+1 << "},'" << args[i].matlabClass(".") << "')";
     first=false;
   }
   // emit code for calling constructor
-	if(matlabBaseName.empty())
+	if(hasParent)
-		file.oss << "\n        my_ptr = ";
-	else
 		file.oss << "\n        [ my_ptr, base_ptr ] = ";
+	else
+		file.oss << "\n        my_ptr = ";
   file.oss << wrapperName << "(" << id;
   // emit constructor arguments
   for(size_t i=0;i<nrArgs;i++) {
@@ -67,13 +67,13 @@ void Constructor::proxy_fragment(FileWriter& file, const std::string& wrapperNam
 /* ************************************************************************* */
 string Constructor::wrapper_fragment(FileWriter& file,
 				 const string& cppClassName,
-				 const string& matlabClassName,
+				 const string& matlabUniqueName,
 				 const string& cppBaseClassName,
 				 int id,
 				 const vector<string>& using_namespaces, 
 				 const ArgumentList& al) const {
 
-	const string wrapFunctionName = matlabClassName + "_constructor_" + boost::lexical_cast<string>(id);
+	const string wrapFunctionName = matlabUniqueName + "_constructor_" + boost::lexical_cast<string>(id);
 
   file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
   file.oss << "{\n";
@@ -87,7 +87,7 @@ string Constructor::wrapper_fragment(FileWriter& file,
 	if(al.size() > 0)
 		al.matlab_unwrap(file); // unwrap arguments
 	file.oss << "  Shared *self = new Shared(new " << cppClassName << "(" << al.names() << "));" << endl;
-	file.oss << "  collector_" << matlabClassName << ".insert(self);\n";
+	file.oss << "  collector_" << matlabUniqueName << ".insert(self);\n";
 
 	if(verbose_)
     file.oss << "  std::cout << \"constructed \" << self << \" << std::endl;" << endl;

wrap/Constructor.h

@@ -50,13 +50,12 @@ struct Constructor {
 	 * if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end
 	 */
 	void proxy_fragment(FileWriter& file, const std::string& wrapperName,
-	        const std::string& className, const std::string& matlabBaseName, const int id,
+	        bool hasParent, const int id, const ArgumentList args) const;
-	        const ArgumentList args) const;
 
 	/// cpp wrapper
 	std::string wrapper_fragment(FileWriter& file,
 			 const std::string& cppClassName,
-			 const std::string& matlabClassName,
+			 const std::string& matlabUniqueName,
 			 const std::string& cppBaseClassName,
 			 int id,
 			 const std::vector<std::string>& using_namespaces,

wrap/Deconstructor.cpp

@@ -37,23 +37,23 @@ string Deconstructor::matlab_wrapper_name(const string& className) const {
 /* ************************************************************************* */
 void Deconstructor::proxy_fragment(FileWriter& file,
 		const std::string& wrapperName,
-		const std::string& qualifiedMatlabName, int id) const {
+		const std::string& matlabUniqueName, int id) const {
 
 	file.oss << "    function delete(obj)\n";
-	file.oss << "      " << wrapperName << "(" << id << ", obj.ptr_" << qualifiedMatlabName << ");\n";
+	file.oss << "      " << wrapperName << "(" << id << ", obj.ptr_" << matlabUniqueName << ");\n";
 	file.oss << "    end\n";
 }
 
 /* ************************************************************************* */
 string Deconstructor::wrapper_fragment(FileWriter& file,
 				 const string& cppClassName,
-				 const string& matlabClassName,
+				 const string& matlabUniqueName,
 				 int id,
 				 const vector<string>& using_namespaces) const {
   
-	const string matlabName = matlab_wrapper_name(matlabClassName);
+	const string matlabName = matlab_wrapper_name(matlabUniqueName);
 
-	const string wrapFunctionName = matlabClassName + "_deconstructor_" + boost::lexical_cast<string>(id);
+	const string wrapFunctionName = matlabUniqueName + "_deconstructor_" + boost::lexical_cast<string>(id);
     
   file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])" << endl;
   file.oss << "{" << endl;
@@ -62,11 +62,11 @@ string Deconstructor::wrapper_fragment(FileWriter& file,
   //Deconstructor takes 1 arg, the mxArray obj
   file.oss << "  checkArguments(\"" << matlabName << "\",nargout,nargin," << "1" << ");" << endl;
 	file.oss << "  Shared *self = *reinterpret_cast<Shared**>(mxGetData(in[0]));\n";
-	file.oss << "  Collector_" << matlabClassName << "::iterator item;\n";
+	file.oss << "  Collector_" << matlabUniqueName << "::iterator item;\n";
-	file.oss << "  item = collector_" << matlabClassName << ".find(self);\n";
+	file.oss << "  item = collector_" << matlabUniqueName << ".find(self);\n";
-	file.oss << "  if(item != collector_" << matlabClassName << ".end()) {\n";
+	file.oss << "  if(item != collector_" << matlabUniqueName << ".end()) {\n";
   file.oss << "    delete self;\n";
-	file.oss << "    collector_" << matlabClassName << ".erase(item);\n";
+	file.oss << "    collector_" << matlabUniqueName << ".erase(item);\n";
 	file.oss << "  }\n";
   file.oss << "}" << endl;
 

wrap/Deconstructor.h

@@ -48,12 +48,12 @@ struct Deconstructor {
 	/// m-file
 	void proxy_fragment(FileWriter& file,
 		const std::string& wrapperName,
-		const std::string& qualifiedMatlabName, int id) const;
+		const std::string& matlabUniqueName, int id) const;
 
 	/// cpp wrapper
 	std::string wrapper_fragment(FileWriter& file,
 			 const std::string& cppClassName,
-			 const std::string& matlabClassName,
+			 const std::string& matlabUniqueName,
 			 int id,
 			 const std::vector<std::string>& using_namespaces) const;
 };

wrap/Method.cpp

@@ -39,7 +39,8 @@ void Method::addOverload(bool verbose, bool is_const, const std::string& name,
 
 void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
 																		 const string& cppClassName,
-																		 const string& matlabClassName,
+																		 const std::string& matlabQualName,
+																		 const std::string& matlabUniqueName,
 																		 const string& wrapperName,
 																		 const vector<string>& using_namespaces,
 																		 const TypeAttributesTable& typeAttributes,
@@ -63,7 +64,7 @@ void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperF
 		bool first = true;
 		for(size_t i=0;i<nrArgs;i++) {
 			if (!first) proxyFile.oss << " && ";
-			proxyFile.oss << "isa(varargin{" << i+1 << "},'" << args[i].matlabClass() << "')";
+			proxyFile.oss << "isa(varargin{" << i+1 << "},'" << args[i].matlabClass(".") << "')";
 			first=false;
 		}
 		proxyFile.oss << "\n";
@@ -81,7 +82,7 @@ void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperF
 		// Output C++ wrapper code
 		
 		const string wrapFunctionName = wrapper_fragment(
-			wrapperFile, cppClassName, matlabClassName, overload, id, using_namespaces, typeAttributes);
+			wrapperFile, cppClassName, matlabUniqueName, overload, id, using_namespaces, typeAttributes);
 
 		// Add to function list
 		functionNames.push_back(wrapFunctionName);
@@ -90,7 +91,7 @@ void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperF
 	
   proxyFile.oss << "      else\n";
 	proxyFile.oss << "        error('Arguments do not match any overload of function " <<
-		matlabClassName << "." << name << "');" << endl;
+		matlabQualName << "." << name << "');" << endl;
 
 	proxyFile.oss << "      end\n";
 	proxyFile.oss << "    end\n";
@@ -99,7 +100,7 @@ void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperF
 /* ************************************************************************* */
 string Method::wrapper_fragment(FileWriter& file, 
 			    const string& cppClassName,
-			    const string& matlabClassName,
+			    const string& matlabUniqueName,
 					int overload,
 					int id,
 			    const vector<string>& using_namespaces,
@@ -107,7 +108,7 @@ string Method::wrapper_fragment(FileWriter& file,
 
   // generate code
 
-	const string wrapFunctionName = matlabClassName + "_" + name + "_" + boost::lexical_cast<string>(id);
+	const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + boost::lexical_cast<string>(id);
 
 	const ArgumentList& args = argLists[overload];
 	const ReturnValue& returnVal = returnVals[overload];
@@ -138,7 +139,7 @@ string Method::wrapper_fragment(FileWriter& file,
 
   // get class pointer
   // example: shared_ptr<Test> = unwrap_shared_ptr< Test >(in[0], "Test");
-  file.oss << "  Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabClassName << "\");" << endl;
+  file.oss << "  Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
   // unwrap arguments, see Argument.cpp
   args.matlab_unwrap(file,1);
 

wrap/Method.h

@@ -50,7 +50,7 @@ struct Method {
 	// MATLAB code generation
 	// classPath is class directory, e.g., ../matlab/@Point2
 	void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
-		const std::string& cppClassName,	const std::string& matlabClassName,
+		const std::string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName,
 		const std::string& wrapperName, const std::vector<std::string>& using_namespaces,
 		const TypeAttributesTable& typeAttributes,
 		std::vector<std::string>& functionNames) const;
@@ -58,7 +58,7 @@ struct Method {
 private:
 	std::string wrapper_fragment(FileWriter& file,
 	    const std::string& cppClassName,
-	    const std::string& matlabClassname,
+	    const std::string& matlabUniqueName,
 			int overload,
 			int id,
 	    const std::vector<std::string>& using_namespaces,

wrap/Module.cpp

@@ -14,7 +14,7 @@
  * @author Frank Dellaert
  * @author Alex Cunningham
  * @author Andrew Melim
- * @author Richard Roberts
+ * @author Richard Roberts
  **/
 
 #include "Module.h"
@@ -448,8 +448,7 @@ void Module::matlab_code(const string& toolboxPath, const string& headerPath) co
 
 		// create proxy class and wrapper code
 		BOOST_FOREACH(const Class& cls, expandedClasses) {
-      string classFile = toolboxPath + "/" + cls.qualifiedName() + ".m";
+      cls.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames);
-      cls.matlab_proxy(classFile, wrapperName, typeAttributes, wrapperFile, functionNames);
     }  
 
 		// finish wrapper file
@@ -527,11 +526,12 @@ vector<string> Module::GenerateValidTypes(const vector<Class>& classes, const ve
 void Module::WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector<Class>& classes) {
 	// Generate all collectors
 	BOOST_FOREACH(const Class& cls, classes) {
-		const string matlabName = cls.qualifiedName(), cppName = cls.qualifiedName("::");
+		const string matlabUniqueName = cls.qualifiedName(),
+			cppName = cls.qualifiedName("::");
 		wrapperFile.oss << "typedef std::set<boost::shared_ptr<" << cppName << ">*> "
-			<< "Collector_" << matlabName << ";\n";
+			<< "Collector_" << matlabUniqueName << ";\n";
-		wrapperFile.oss << "static Collector_" << matlabName <<
+		wrapperFile.oss << "static Collector_" << matlabUniqueName <<
-			" collector_" << matlabName << ";\n";
+			" collector_" << matlabUniqueName << ";\n";
 	}
 
 	// generate mexAtExit cleanup function
@@ -542,10 +542,10 @@ void Module::WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::st
 		"  std::streambuf *outbuf = std::cout.rdbuf(&mout);\n\n"
 		"  bool anyDeleted = false;\n";
 	BOOST_FOREACH(const Class& cls, classes) {
-		const string matlabName = cls.qualifiedName();
+		const string matlabUniqueName = cls.qualifiedName();
 		const string cppName = cls.qualifiedName("::");
-		const string collectorType = "Collector_" + matlabName;
+		const string collectorType = "Collector_" + matlabUniqueName;
-		const string collectorName = "collector_" + matlabName;
+		const string collectorName = "collector_" + matlabUniqueName;
 		wrapperFile.oss <<
 			"  for(" << collectorType << "::iterator iter = " << collectorName << ".begin();\n"
 			"      iter != " << collectorName << ".end(); ) {\n"
@@ -574,7 +574,7 @@ void Module::WriteRTTIRegistry(FileWriter& wrapperFile, const std::string& modul
 	BOOST_FOREACH(const Class& cls, classes) {
 		if(cls.isVirtual)
 			wrapperFile.oss <<
-			"    types.insert(std::make_pair(typeid(" << cls.qualifiedName("::") << ").name(), \"" << cls.qualifiedName() << "\"));\n";
+			"    types.insert(std::make_pair(typeid(" << cls.qualifiedName("::") << ").name(), \"" << cls.qualifiedName(".") << "\"));\n";
 	}
 	wrapperFile.oss << "\n";
 

wrap/ReturnValue.cpp

@@ -48,8 +48,8 @@ string ReturnValue::qualifiedType2(const string& delim) const {
 /* ************************************************************************* */
 //TODO:Fix this
 void ReturnValue::wrap_result(const string& result, FileWriter& file, const TypeAttributesTable& typeAttributes) const {
-	string cppType1 = qualifiedType1("::"), matlabType1 = qualifiedType1();
+	string cppType1 = qualifiedType1("::"), matlabType1 = qualifiedType1(".");
-	string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2();
+	string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2(".");
 
   if (isPair) {
   	// first return value in pair

wrap/StaticMethod.cpp

@@ -40,7 +40,8 @@ void StaticMethod::addOverload(bool verbose, const std::string& name,
 
 void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
 																		 const string& cppClassName,
-																		 const string& matlabClassName,
+																		 const std::string& matlabQualName,
+																		 const std::string& matlabUniqueName,
 																		 const string& wrapperName,
 																		 const vector<string>& using_namespaces,
 																		 const TypeAttributesTable& typeAttributes,
@@ -66,7 +67,7 @@ void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wr
 		bool first = true;
 		for(size_t i=0;i<nrArgs;i++) {
 			if (!first) proxyFile.oss << " && ";
-			proxyFile.oss << "isa(varargin{" << i+1 << "},'" << args[i].matlabClass() << "')";
+			proxyFile.oss << "isa(varargin{" << i+1 << "},'" << args[i].matlabClass(".") << "')";
 			first=false;
 		}
 		proxyFile.oss << "\n";
@@ -84,7 +85,7 @@ void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wr
 		// Output C++ wrapper code
 		
 		const string wrapFunctionName = wrapper_fragment(
-			wrapperFile, cppClassName, matlabClassName, overload, id, using_namespaces, typeAttributes);
+			wrapperFile, cppClassName, matlabUniqueName, overload, id, using_namespaces, typeAttributes);
 
 		// Add to function list
 		functionNames.push_back(wrapFunctionName);
@@ -93,7 +94,7 @@ void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wr
 	
   proxyFile.oss << "      else\n";
 	proxyFile.oss << "        error('Arguments do not match any overload of function " <<
-		matlabClassName << "." << upperName << "');" << endl;
+		matlabQualName << "." << upperName << "');" << endl;
 
 	proxyFile.oss << "      end\n";
 	proxyFile.oss << "    end\n";
@@ -102,7 +103,7 @@ void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wr
 /* ************************************************************************* */
 string StaticMethod::wrapper_fragment(FileWriter& file, 
 			    const string& cppClassName,
-			    const string& matlabClassName,
+			    const string& matlabUniqueName,
 					int overload,
 					int id,
 			    const vector<string>& using_namespaces,
@@ -110,7 +111,7 @@ string StaticMethod::wrapper_fragment(FileWriter& file,
 
   // generate code
 
-	const string wrapFunctionName = matlabClassName + "_" + name + "_" + boost::lexical_cast<string>(id);
+	const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + boost::lexical_cast<string>(id);
 
 	const ArgumentList& args = argLists[overload];
 	const ReturnValue& returnVal = returnVals[overload];
@@ -136,7 +137,7 @@ string StaticMethod::wrapper_fragment(FileWriter& file,
 
   // check arguments
   // NOTE: for static functions, there is no object passed
-  file.oss << "  checkArguments(\"" << matlabClassName << "." << name << "\",nargout,nargin," << args.size() << ");\n";
+  file.oss << "  checkArguments(\"" << matlabUniqueName << "." << name << "\",nargout,nargin," << args.size() << ");\n";
 
   // unwrap arguments, see Argument.cpp
   args.matlab_unwrap(file,0); // We start at 0 because there is no self object

wrap/StaticMethod.h

@@ -50,7 +50,7 @@ struct StaticMethod {
 	// MATLAB code generation
 	// classPath is class directory, e.g., ../matlab/@Point2
 	void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
-		const std::string& cppClassName,	const std::string& matlabClassName,
+		const std::string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName,
 		const std::string& wrapperName, const std::vector<std::string>& using_namespaces,
 		const TypeAttributesTable& typeAttributes,
 		std::vector<std::string>& functionNames) const;
@@ -58,7 +58,7 @@ struct StaticMethod {
 private:
 	std::string wrapper_fragment(FileWriter& file,
 	    const std::string& cppClassName,
-	    const std::string& matlabClassname,
+	    const std::string& matlabUniqueName,
 			int overload,
 			int id,
 	    const std::vector<std::string>& using_namespaces,

wrap/tests/expected_namespaces/+ns1/ClassA.m

@@ -1,17 +1,17 @@
 % automatically generated by wrap
-classdef ns1ClassA < handle
+classdef ClassA < handle
   properties
     ptr_ns1ClassA = 0
   end
   methods
-    function obj = ns1ClassA(varargin)
+    function obj = ClassA(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
         testNamespaces_wrapper(0, my_ptr);
       elseif nargin == 0
         my_ptr = testNamespaces_wrapper(1);
       else
-        error('Arguments do not match any overload of ns1ClassA constructor');
+        error('Arguments do not match any overload of ns1.ClassA constructor');
       end
       obj.ptr_ns1ClassA = my_ptr;
     end

wrap/tests/expected_namespaces/+ns1/ClassB.m

@@ -1,17 +1,17 @@
 % automatically generated by wrap
-classdef ns1ClassB < handle
+classdef ClassB < handle
   properties
     ptr_ns1ClassB = 0
   end
   methods
-    function obj = ns1ClassB(varargin)
+    function obj = ClassB(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
         testNamespaces_wrapper(3, my_ptr);
       elseif nargin == 0
         my_ptr = testNamespaces_wrapper(4);
       else
-        error('Arguments do not match any overload of ns1ClassB constructor');
+        error('Arguments do not match any overload of ns1.ClassB constructor');
       end
       obj.ptr_ns1ClassB = my_ptr;
     end

wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m

@@ -1,17 +1,17 @@
 % automatically generated by wrap
-classdef ns2ns3ClassB < handle
+classdef ClassB < handle
   properties
     ptr_ns2ns3ClassB = 0
   end
   methods
-    function obj = ns2ns3ClassB(varargin)
+    function obj = ClassB(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
         testNamespaces_wrapper(13, my_ptr);
       elseif nargin == 0
         my_ptr = testNamespaces_wrapper(14);
       else
-        error('Arguments do not match any overload of ns2ns3ClassB constructor');
+        error('Arguments do not match any overload of ns2.ns3.ClassB constructor');
       end
       obj.ptr_ns2ns3ClassB = my_ptr;
     end

wrap/tests/expected_namespaces/+ns2/ClassA.m

@@ -1,17 +1,17 @@
 % automatically generated by wrap
-classdef ns2ClassA < handle
+classdef ClassA < handle
   properties
     ptr_ns2ClassA = 0
   end
   methods
-    function obj = ns2ClassA(varargin)
+    function obj = ClassA(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
         testNamespaces_wrapper(6, my_ptr);
       elseif nargin == 0
         my_ptr = testNamespaces_wrapper(7);
       else
-        error('Arguments do not match any overload of ns2ClassA constructor');
+        error('Arguments do not match any overload of ns2.ClassA constructor');
       end
       obj.ptr_ns2ClassA = my_ptr;
     end
@@ -28,15 +28,15 @@ classdef ns2ClassA < handle
       if length(varargin) == 0
         varargout{1} = testNamespaces_wrapper(9, this, varargin{:});
       else
-        error('Arguments do not match any overload of function ns2ClassA.memberFunction');
+        error('Arguments do not match any overload of function ns2.ClassA.memberFunction');
       end
     end
 
     function varargout = nsArg(this, varargin)
-      if length(varargin) == 1 && isa(varargin{1},'ns1ClassB')
+      if length(varargin) == 1 && isa(varargin{1},'ns1.ClassB')
         varargout{1} = testNamespaces_wrapper(10, this, varargin{:});
       else
-        error('Arguments do not match any overload of function ns2ClassA.nsArg');
+        error('Arguments do not match any overload of function ns2.ClassA.nsArg');
       end
     end
 
@@ -44,7 +44,7 @@ classdef ns2ClassA < handle
       if length(varargin) == 1 && isa(varargin{1},'double')
         varargout{1} = testNamespaces_wrapper(11, this, varargin{:});
       else
-        error('Arguments do not match any overload of function ns2ClassA.nsReturn');
+        error('Arguments do not match any overload of function ns2.ClassA.nsReturn');
       end
     end
 
@@ -55,7 +55,7 @@ classdef ns2ClassA < handle
       if length(varargin) == 0
         varargout{1} = testNamespaces_wrapper(12, varargin{:});
       else
-        error('Arguments do not match any overload of function ns2ClassA.Afunction');
+        error('Arguments do not match any overload of function ns2.ClassA.Afunction');
       end
     end
 

wrap/tests/expected_namespaces/+ns2/ClassC.m

@@ -1,17 +1,17 @@
 % automatically generated by wrap
-classdef ns2ClassC < handle
+classdef ClassC < handle
   properties
     ptr_ns2ClassC = 0
   end
   methods
-    function obj = ns2ClassC(varargin)
+    function obj = ClassC(varargin)
       if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682)
         my_ptr = varargin{2};
         testNamespaces_wrapper(16, my_ptr);
       elseif nargin == 0
         my_ptr = testNamespaces_wrapper(17);
       else
-        error('Arguments do not match any overload of ns2ClassC constructor');
+        error('Arguments do not match any overload of ns2.ClassC constructor');
       end
       obj.ptr_ns2ClassC = my_ptr;
     end

wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp

@@ -223,7 +223,7 @@ void ns2ClassA_nsReturn_11(int nargout, mxArray *out[], int nargin, const mxArra
   checkArguments("nsReturn",nargout,nargin-1,1);
   Shared obj = unwrap_shared_ptr<ns2::ClassA>(in[0], "ptr_ns2ClassA");
   double q = unwrap< double >(in[1]);
-  out[0] = wrap_shared_ptr(SharedClassB(new ns2::ns3::ClassB(obj->nsReturn(q))),"ns2ns3ClassB", false);
+  out[0] = wrap_shared_ptr(SharedClassB(new ns2::ns3::ClassB(obj->nsReturn(q))),"ns2.ns3.ClassB", false);
 }
 
 void ns2ClassA_afunction_12(int nargout, mxArray *out[], int nargin, const mxArray *in[])

wrap/tests/testWrap.cpp

@@ -227,11 +227,11 @@ TEST( wrap, matlab_code_namespaces ) {
 
 
 	EXPECT(files_equal(exp_path + "ClassD.m"                    , act_path + "ClassD.m"                   ));
-	EXPECT(files_equal(exp_path + "ns1ClassA.m"                 , act_path + "ns1ClassA.m"                ));
+	EXPECT(files_equal(exp_path + "+ns1/ClassA.m"               , act_path + "+ns1/ClassA.m"              ));
-	EXPECT(files_equal(exp_path + "ns1ClassB.m"                 , act_path + "ns1ClassB.m"                ));
+	EXPECT(files_equal(exp_path + "+ns1/ClassB.m"               , act_path + "+ns1/ClassB.m"              ));
-	EXPECT(files_equal(exp_path + "ns2ClassA.m"                 , act_path + "ns2ClassA.m"                ));
+	EXPECT(files_equal(exp_path + "+ns2/ClassA.m"               , act_path + "+ns2/ClassA.m"              ));
-	EXPECT(files_equal(exp_path + "ns2ClassC.m"                 , act_path + "ns2ClassC.m"                ));
+	EXPECT(files_equal(exp_path + "+ns2/ClassC.m"               , act_path + "+ns2/ClassC.m"              ));
-	EXPECT(files_equal(exp_path + "ns2ns3ClassB.m"              , act_path + "ns2ns3ClassB.m"             ));
+	EXPECT(files_equal(exp_path + "+ns2/+ns3/ClassB.m"          , act_path + "+ns2/+ns3/ClassB.m"         ));
 	EXPECT(files_equal(exp_path + "testNamespaces_wrapper.cpp"  , act_path + "testNamespaces_wrapper.cpp" ));
 }
 

wrap/utilities.h

@@ -41,6 +41,15 @@ class CantOpenFile : public std::exception {
 	virtual const char* what() const throw() { return what_.c_str(); }
 };
 
+class OutputError : public std::exception {
+private:
+	const std::string what_;
+public:
+	OutputError(const std::string& what) : what_(what) {}
+	~OutputError() throw() {}
+	virtual const char* what() const throw() { return what_.c_str(); }
+};
+
 class ParseFailed : public std::exception {
  private:
   const std::string what_;