From 8dbffd46294046c2d6aeed5bca2ff7acd3c540ed Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 18 Jul 2012 15:47:06 +0000 Subject: [PATCH] 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 | 8 +-- matlab/VisualISAMGenerateData.m | 26 ++++---- matlab/VisualISAMInitialize.m | 15 ++--- matlab/VisualISAMPlot.m | 3 +- matlab/VisualISAMStep.m | 7 ++- matlab/examples/LocalizationExample.m | 27 ++++---- matlab/examples/OdometryExample.m | 22 ++++--- matlab/examples/PlanarSLAMExample.m | 36 ++++++----- matlab/examples/PlanarSLAMExample_sampling.m | 40 ++++++------ matlab/examples/Pose2SLAMExample.m | 36 ++++++----- matlab/examples/Pose2SLAMExample_advanced.m | 26 ++++---- matlab/examples/Pose2SLAMExample_circle.m | 8 +-- matlab/examples/Pose2SLAMExample_graph.m | 8 ++- matlab/examples/Pose2SLAMwSPCG.m | 37 ++++++----- matlab/examples/Pose3SLAMExample.m | 8 +-- matlab/examples/Pose3SLAMExample_graph.m | 4 +- matlab/examples/SBAExample.m | 20 +++--- matlab/examples/SFMExample.m | 15 +++-- matlab/examples/StereoVOExample.m | 34 +++++----- matlab/examples/StereoVOExample_large.m | 17 ++--- matlab/load2D.m | 8 +-- matlab/load3D.m | 26 ++++---- matlab/symbol.m | 2 +- matlab/symbolChr.m | 2 +- matlab/symbolIndex.m | 2 +- matlab/tests/testJacobianFactor.m | 12 ++-- matlab/tests/testKalmanFilter.m | 8 ++- matlab/tests/testLocalizationExample.m | 28 +++++---- matlab/tests/testOdometryExample.m | 23 ++++--- matlab/tests/testPlanarSLAMExample.m | 39 +++++++----- matlab/tests/testPose2SLAMExample.m | 40 ++++++------ matlab/tests/testPose3SLAMExample.m | 11 ++-- matlab/tests/testSFMExample.m | 14 +++-- matlab/tests/testStereoVOExample.m | 34 +++++----- wrap/Argument.cpp | 4 +- wrap/Class.cpp | 62 ++++++++++++------- wrap/Class.h | 2 +- wrap/Constructor.cpp | 16 ++--- wrap/Constructor.h | 5 +- wrap/Deconstructor.cpp | 18 +++--- wrap/Deconstructor.h | 4 +- wrap/Method.cpp | 15 ++--- wrap/Method.h | 4 +- wrap/Module.cpp | 22 +++---- wrap/ReturnValue.cpp | 4 +- wrap/StaticMethod.cpp | 15 ++--- wrap/StaticMethod.h | 4 +- .../{ns1ClassA.m => +ns1/ClassA.m} | 6 +- .../{ns1ClassB.m => +ns1/ClassB.m} | 6 +- .../{ns2ns3ClassB.m => +ns2/+ns3/ClassB.m} | 6 +- .../{ns2ClassA.m => +ns2/ClassA.m} | 16 ++--- .../{ns2ClassC.m => +ns2/ClassC.m} | 6 +- .../testNamespaces_wrapper.cpp | 2 +- wrap/tests/testWrap.cpp | 10 +-- wrap/utilities.h | 9 +++ 55 files changed, 493 insertions(+), 389 deletions(-) rename wrap/tests/expected_namespaces/{ns1ClassA.m => +ns1/ClassA.m} (80%) rename wrap/tests/expected_namespaces/{ns1ClassB.m => +ns1/ClassB.m} (80%) rename wrap/tests/expected_namespaces/{ns2ns3ClassB.m => +ns2/+ns3/ClassB.m} (79%) rename wrap/tests/expected_namespaces/{ns2ClassA.m => +ns2/ClassA.m} (84%) rename wrap/tests/expected_namespaces/{ns2ClassC.m => +ns2/ClassC.m} (80%) diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 84df4055c..3e77de602 100644 --- a/matlab/CMakeLists.txt +++ b/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) diff --git a/matlab/VisualISAMGenerateData.m b/matlab/VisualISAMGenerateData.m index 1c71185e2..2e972d9af 100644 --- a/matlab/VisualISAMGenerateData.m +++ b/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]'),... - gtsamPoint3([-10 10 10]'),... - gtsamPoint3([-10 -10 10]'),... - gtsamPoint3([10 -10 10]'),... - gtsamPoint3([10 10 -10]'),... - gtsamPoint3([-10 10 -10]'),... - gtsamPoint3([-10 -10 -10]'),... - gtsamPoint3([10 -10 -10]')}; + truth.points = {Point3([10 10 10]'),... + Point3([-10 10 10]'),... + Point3([-10 -10 10]'),... + Point3([10 -10 10]'),... + Point3([10 10 -10]'),... + Point3([-10 10 -10]'),... + Point3([-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]'); - truth.cameras{i} = gtsamSimpleCamera.Lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), truth.K); + t = Point3([r*cos(theta), r*sin(theta), height]'); + 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 diff --git a/matlab/VisualISAMInitialize.m b/matlab/VisualISAMInitialize.m index b2b960a7b..cbcb09c5c 100644 --- a/matlab/VisualISAMInitialize.m +++ b/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]'); -noiseModels.odometry = gtsamnoiseModelDiagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); -noiseModels.point = gtsamnoiseModelIsotropic.Sigma(3, 0.1); -noiseModels.measurement = gtsamnoiseModelIsotropic.Sigma(2, 1.0); +import gtsam.* +noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); +noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]'); +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; -initialEstimates = visualSLAMValues; +newFactors = visualSLAM.Graph; +initialEstimates = visualSLAM.Values; for i=1:2 ii = symbol('x',i); if i==1 diff --git a/matlab/VisualISAMPlot.m b/matlab/VisualISAMPlot.m index e140686d9..1f2b7d710 100644 --- a/matlab/VisualISAMPlot.m +++ b/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); diff --git a/matlab/VisualISAMStep.m b/matlab/VisualISAMStep.m index 5033a8b39..ef056553e 100644 --- a/matlab/VisualISAMStep.m +++ b/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; -initialEstimates = visualSLAMValues; +newFactors = visualSLAM.Graph; +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)); diff --git a/matlab/examples/LocalizationExample.m b/matlab/examples/LocalizationExample.m index 3f48bb8e4..0a54111b0 100644 --- a/matlab/examples/LocalizationExample.m +++ b/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) -odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta +import gtsam.* +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]); -graph.addPosePrior(1, gtsamPose2(0.0, 0.0, 0.0), noiseModel); -graph.addPosePrior(2, gtsamPose2(2.0, 0.0, 0.0), noiseModel); -graph.addPosePrior(3, gtsamPose2(4.0, 0.0, 0.0), noiseModel); +priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]); +graph.addPosePrior(1, Pose2(0.0, 0.0, 0.0), priorNoise); +graph.addPosePrior(2, Pose2(2.0, 0.0, 0.0), priorNoise); +graph.addPosePrior(3, Pose2(4.0, 0.0, 0.0), priorNoise); %% print graph.print(sprintf('\nFactor graph:\n')); %% Initialize to noisy points -initialEstimate = pose2SLAMValues; -initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1)); +import gtsam.* +initialEstimate = pose2SLAM.Values; +initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); +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 diff --git a/matlab/examples/OdometryExample.m b/matlab/examples/OdometryExample.m index e90550152..297702b51 100644 --- a/matlab/examples/OdometryExample.m +++ b/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 -priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta +import gtsam.* +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) -odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta +import gtsam.* +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; -initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1)); +import gtsam.* +initialEstimate = pose2SLAM.Values; +initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); +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 diff --git a/matlab/examples/PlanarSLAMExample.m b/matlab/examples/PlanarSLAMExample.m index 7557bd0ba..e664d66fc 100644 --- a/matlab/examples/PlanarSLAMExample.m +++ b/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 -priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); +import gtsam.* +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); -odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); +import gtsam.* +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]); -graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel); -graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel); -graph.addBearingRange(i3, j2, gtsamRot2(90*degrees), 2, noiseModel); +brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); +graph.addBearingRange(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise); +graph.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise); +graph.addBearingRange(i3, j2, Rot2(90*degrees), 2, brNoise); % print graph.print(sprintf('\nFull graph:\n')); %% Initialize to noisy points -initialEstimate = planarSLAMValues; -initialEstimate.insertPose(i1, gtsamPose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(i2, gtsamPose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(i3, gtsamPose2(4.1, 0.1, 0.1)); -initialEstimate.insertPoint(j1, gtsamPoint2(1.8, 2.1)); -initialEstimate.insertPoint(j2, gtsamPoint2(4.1, 1.8)); +import gtsam.* +initialEstimate = planarSLAM.Values; +initialEstimate.insertPose(i1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1)); +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 diff --git a/matlab/examples/PlanarSLAMExample_sampling.m b/matlab/examples/PlanarSLAMExample_sampling.m index 560ab7ace..ec68d81e6 100644 --- a/matlab/examples/PlanarSLAMExample_sampling.m +++ b/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; -priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin -priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); +graph = planarSLAM.Graph; +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 -odometry = gtsamPose2(2.0, 0.0, 0.0); -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); %% 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(i2, j1, gtsamRot2(90*degrees), 2, noiseModel); + graph.addBearingRange(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise); + graph.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise); else - bearingModel = gtsamnoiseModelDiagonal.Sigmas(0.1); - graph.addBearing(i1, j1, gtsamRot2(45*degrees), bearingModel); - graph.addBearing(i2, j1, gtsamRot2(90*degrees), bearingModel); + bearingModel = noiseModel.Diagonal.Sigmas(0.1); + graph.addBearing(i1, j1, Rot2(45*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.insertPose(i1, gtsamPose2(0,0,0)); -sample.insertPose(i2, gtsamPose2(2,0,0)); -sample.insertPose(i3, gtsamPose2(4,0,0)); -sample.insertPoint(j1, gtsamPoint2(2,2)); -sample.insertPoint(j2, gtsamPoint2(4,2)); +sample = planarSLAM.Values; +sample.insertPose(i1, Pose2(0,0,0)); +sample.insertPose(i2, Pose2(2,0,0)); +sample.insertPose(i3, Pose2(4,0,0)); +sample.insertPoint(j1, Point2(2,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 \ No newline at end of file diff --git a/matlab/examples/Pose2SLAMExample.m b/matlab/examples/Pose2SLAMExample.m index 9cb5b8af4..4bb072fa8 100644 --- a/matlab/examples/Pose2SLAMExample.m +++ b/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 -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(1, priorMean, priorNoise); % add directly to graph %% Add odometry +import gtsam.* % general noisemodel for odometry -odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise); -graph.addRelativePose(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); +odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); +graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); +graph.addRelativePose(2, 3, Pose2(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]); -graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model); +import gtsam.* +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.insertPose(1, gtsamPose2(0.5, 0.0, 0.2 )); -initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2 )); -initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, pi/2)); -initialEstimate.insertPose(4, gtsamPose2(4.0, 2.0, pi )); -initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2)); +import gtsam.* +initialEstimate = pose2SLAM.Values; +initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2 )); +initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2 )); +initialEstimate.insertPose(3, Pose2(4.1, 0.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 diff --git a/matlab/examples/Pose2SLAMExample_advanced.m b/matlab/examples/Pose2SLAMExample_advanced.m index 45d288e50..fc23d67f1 100644 --- a/matlab/examples/Pose2SLAMExample_advanced.m +++ b/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]); -priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin +priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); +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]); -odometry = gtsamPose2(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]); +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; -initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1)); +import gtsam.* +initialEstimate = pose2SLAM.Values; +initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); +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); % diff --git a/matlab/examples/Pose2SLAMExample_circle.m b/matlab/examples/Pose2SLAMExample_circle.m index c089f5949..355f1a10e 100644 --- a/matlab/examples/Pose2SLAMExample_circle.m +++ b/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]')); diff --git a/matlab/examples/Pose2SLAMExample_graph.m b/matlab/examples/Pose2SLAMExample_graph.m index 258730aaa..ac6d1e317 100644 --- a/matlab/examples/Pose2SLAMExample_graph.m +++ b/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 -priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.01; 0.01; 0.01]); +import gtsam.* +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 diff --git a/matlab/examples/Pose2SLAMwSPCG.m b/matlab/examples/Pose2SLAMwSPCG.m index 3e773689b..5db736fab 100644 --- a/matlab/examples/Pose2SLAMwSPCG.m +++ b/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 -priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); -graph.addPrior(1, priorMean, priorNoise); % add directly to graph +priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin +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]); -graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise); -graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); -graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); -graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); +odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); +graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); +graph.addRelativePose(2, 3, Pose2(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]); -graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model); +import gtsam.* +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.insertPose(1, gtsamPose2(0.5, 0.0, 0.2 )); -initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2 )); -initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, pi/2)); -initialEstimate.insertPose(4, gtsamPose2(4.0, 2.0, pi )); -initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2)); +initialEstimate = pose2SLAM.Values; +initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2 )); +initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2 )); +initialEstimate.insertPose(3, Pose2(4.1, 0.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 diff --git a/matlab/examples/Pose3SLAMExample.m b/matlab/examples/Pose3SLAMExample.m index c1328585e..d4a8baf99 100644 --- a/matlab/examples/Pose3SLAMExample.m +++ b/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))); diff --git a/matlab/examples/Pose3SLAMExample_graph.m b/matlab/examples/Pose3SLAMExample_graph.m index f39fc306b..cace357ae 100644 --- a/matlab/examples/Pose3SLAMExample_graph.m +++ b/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; diff --git a/matlab/examples/SBAExample.m b/matlab/examples/SBAExample.m index 82fdd4646..a0f003eeb 100644 --- a/matlab/examples/SBAExample.m +++ b/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); -firstCamera = gtsamSimpleCamera(truth.cameras{1}.pose, truth.K); +import gtsam.* +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'); diff --git a/matlab/examples/SFMExample.m b/matlab/examples/SFMExample.m index 3eef9f916..7c4baa75b 100644 --- a/matlab/examples/SFMExample.m +++ b/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'); diff --git a/matlab/examples/StereoVOExample.m b/matlab/examples/StereoVOExample.m index 9cf838e94..98e1b5845 100644 --- a/matlab/examples/StereoVOExample.m +++ b/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); -stereo_model = gtsamnoiseModelDiagonal.Sigmas([1.0; 1.0; 1.0]); +import gtsam.* +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(gtsamStereoPoint2(120, 80, 440), stereo_model, x1, l2, K); -graph.addStereoMeasurement(gtsamStereoPoint2(320, 280, 140), stereo_model, x1, l3, K); +graph.addStereoMeasurement(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K); +graph.addStereoMeasurement(StereoPoint2(120, 80, 440), stereo_model, x1, l2, 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(gtsamStereoPoint2( 70, 20, 490), stereo_model, x2, l2, K); -graph.addStereoMeasurement(gtsamStereoPoint2(320, 270, 115), stereo_model, x2, l3, K); +graph.addStereoMeasurement(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K); +graph.addStereoMeasurement(StereoPoint2( 70, 20, 490), stereo_model, x2, l2, 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.insertPoint(l1, gtsamPoint3( 1, 1, 5)); -initialEstimate.insertPoint(l2, gtsamPoint3(-1, 1, 5)); -initialEstimate.insertPoint(l3, gtsamPoint3( 0,-.5, 5)); +initialEstimate.insertPose(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1))); +initialEstimate.insertPoint(l1, Point3( 1, 1, 5)); +initialEstimate.insertPoint(l2, Point3(-1, 1, 5)); +initialEstimate.insertPoint(l3, Point3( 0,-.5, 5)); %% optimize fprintf(1,'Optimizing\n'); tic diff --git a/matlab/examples/StereoVOExample_large.m b/matlab/examples/StereoVOExample_large.m index d57628fa1..282f7162c 100644 --- a/matlab/examples/StereoVOExample_large.m +++ b/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)); -stereo_model = gtsamnoiseModelDiagonal.Sigmas([1.0; 1.0; 1.0]); +K = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); +stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); %% create empty graph and values -graph = visualSLAMGraph; -initial = visualSLAMValues; +graph = visualSLAM.Graph; +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 diff --git a/matlab/load2D.m b/matlab/load2D.m index e459dfc63..31e96818f 100644 --- a/matlab/load2D.m +++ b/matlab/load2D.m @@ -13,17 +13,17 @@ fclose(fid); lines=columns{1}; % loop over lines and add vertices -graph = pose2SLAMGraph; -initial = pose2SLAMValues; +graph = pose2SLAM.Graph; +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 diff --git a/matlab/load3D.m b/matlab/load3D.m index b457ea1af..5c9d08146 100644 --- a/matlab/load3D.m +++ b/matlab/load3D.m @@ -15,9 +15,9 @@ fclose(fid); lines=columns{1}; % loop over lines and add vertices -graph = pose3SLAMGraph; -initial = pose3SLAMValues; -origin=gtsamPose3; +graph = pose3SLAM.Graph; +initial = pose3SLAM.Values; +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}; - if (~successive & ii1 diff --git a/matlab/symbol.m b/matlab/symbol.m index c7c4af2c6..10a733593 100644 --- a/matlab/symbol.m +++ b/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(); \ No newline at end of file diff --git a/matlab/symbolChr.m b/matlab/symbolChr.m index 39c92ec85..6cabdb2b2 100644 --- a/matlab/symbolChr.m +++ b/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(); \ No newline at end of file diff --git a/matlab/symbolIndex.m b/matlab/symbolIndex.m index 3046cf229..ad6eaf97d 100644 --- a/matlab/symbolIndex.m +++ b/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(); \ No newline at end of file diff --git a/matlab/tests/testJacobianFactor.m b/matlab/tests/testJacobianFactor.m index 3afd2dc58..3bc8f4edd 100644 --- a/matlab/tests/testJacobianFactor.m +++ b/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); -combined = gtsamJacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4); +model4 = noiseModel.Diagonal.Sigmas(sigmas); +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]); -expectedLF = gtsamJacobianFactor(l1, Bl1, x1, Bx1, b1, model2); +model2 = noiseModel.Diagonal.Sigmas([1;1]); +expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2); % check if the result matches the combined (reduced) factor % FIXME: JacobianFactor/GaussianFactor mismatch diff --git a/matlab/tests/testKalmanFilter.m b/matlab/tests/testKalmanFilter.m index 652e37c9e..ee65d18d3 100644 --- a/matlab/tests/testKalmanFilter.m +++ b/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]; diff --git a/matlab/tests/testLocalizationExample.m b/matlab/tests/testLocalizationExample.m index 1efcde4a6..35c16d85f 100644 --- a/matlab/tests/testLocalizationExample.m +++ b/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) -odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta +import gtsam.* +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; -groundTruth.insertPose(1, gtsamPose2(0.0, 0.0, 0.0)); -groundTruth.insertPose(2, gtsamPose2(2.0, 0.0, 0.0)); -groundTruth.insertPose(3, gtsamPose2(4.0, 0.0, 0.0)); -noiseModel = gtsamnoiseModelDiagonal.Sigmas([0.1; 0.1; 10]); +import gtsam.* +groundTruth = pose2SLAM.Values; +groundTruth.insertPose(1, Pose2(0.0, 0.0, 0.0)); +groundTruth.insertPose(2, Pose2(2.0, 0.0, 0.0)); +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.insertPose(1, gtsamPose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1)); +initialEstimate = pose2SLAM.Values; +initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); +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); diff --git a/matlab/tests/testOdometryExample.m b/matlab/tests/testOdometryExample.m index 21d83c35e..e793df9a8 100644 --- a/matlab/tests/testOdometryExample.m +++ b/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 -priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta +import gtsam.* +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) -odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta +import gtsam.* +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; -initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, 0.1)); +import gtsam.* +initialEstimate = pose2SLAM.Values; +initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); +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)); diff --git a/matlab/tests/testPlanarSLAMExample.m b/matlab/tests/testPlanarSLAMExample.m index cb2517526..4c3e6c9da 100644 --- a/matlab/tests/testPlanarSLAMExample.m +++ b/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 -priorNoise = gtsamnoiseModelDiagonal.Sigmas([0.3; 0.3; 0.1]); +import gtsam.* +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); -odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); +import gtsam.* +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]); -graph.addBearingRange(i1, j1, gtsamRot2(45*degrees), sqrt(4+4), noiseModel); -graph.addBearingRange(i2, j1, gtsamRot2(90*degrees), 2, noiseModel); -graph.addBearingRange(i3, j2, gtsamRot2(90*degrees), 2, noiseModel); +brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); +graph.addBearingRange(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise); +graph.addBearingRange(i2, j1, Rot2(90*degrees), 2, brNoise); +graph.addBearingRange(i3, j2, Rot2(90*degrees), 2, brNoise); %% Initialize to noisy points -initialEstimate = planarSLAMValues; -initialEstimate.insertPose(i1, gtsamPose2(0.5, 0.0, 0.2)); -initialEstimate.insertPose(i2, gtsamPose2(2.3, 0.1,-0.2)); -initialEstimate.insertPose(i3, gtsamPose2(4.1, 0.1, 0.1)); -initialEstimate.insertPoint(j1, gtsamPoint2(1.8, 2.1)); -initialEstimate.insertPoint(j2, gtsamPoint2(4.1, 1.8)); +import gtsam.* +initialEstimate = planarSLAM.Values; +initialEstimate.insertPose(i1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insertPose(i2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insertPose(i3, Pose2(4.1, 0.1, 0.1)); +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)); diff --git a/matlab/tests/testPose2SLAMExample.m b/matlab/tests/testPose2SLAMExample.m index 41827c213..cd778cba3 100644 --- a/matlab/tests/testPose2SLAMExample.m +++ b/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 -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(1, priorMean, priorNoise); % add directly to graph %% Add odometry % general noisemodel for odometry -odometryNoise = gtsamnoiseModelDiagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise); -graph.addRelativePose(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); -graph.addRelativePose(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); +import gtsam.* +odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); +graph.addRelativePose(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); +graph.addRelativePose(2, 3, Pose2(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]); -graph.addRelativePose(5, 2, gtsamPose2(2.0, 0.0, pi/2), model); +import gtsam.* +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; -initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2 )); -initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2 )); -initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, pi/2)); -initialEstimate.insertPose(4, gtsamPose2(4.0, 2.0, pi )); -initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2)); +import gtsam.* +initialEstimate = pose2SLAM.Values; +initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2 )); +initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2 )); +initialEstimate.insertPose(3, Pose2(4.1, 0.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)); diff --git a/matlab/tests/testPose3SLAMExample.m b/matlab/tests/testPose3SLAMExample.m index 97e8b1b42..ac3c0d5bb 100644 --- a/matlab/tests/testPose3SLAMExample.m +++ b/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)); diff --git a/matlab/tests/testSFMExample.m b/matlab/tests/testSFMExample.m index bcf6aca32..b3595dc18 100644 --- a/matlab/tests/testSFMExample.m +++ b/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(); diff --git a/matlab/tests/testStereoVOExample.m b/matlab/tests/testStereoVOExample.m index da9249128..69fba200d 100644 --- a/matlab/tests/testStereoVOExample.m +++ b/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); -stereo_model = gtsamnoiseModelDiagonal.Sigmas([1.0; 1.0; 1.0]); +import gtsam.* +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(gtsamStereoPoint2(120, 80, 440), stereo_model, x1, l2, K); -graph.addStereoMeasurement(gtsamStereoPoint2(320, 280, 140), stereo_model, x1, l3, K); +graph.addStereoMeasurement(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K); +graph.addStereoMeasurement(StereoPoint2(120, 80, 440), stereo_model, x1, l2, 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(gtsamStereoPoint2( 70, 20, 490), stereo_model, x2, l2, K); -graph.addStereoMeasurement(gtsamStereoPoint2(320, 270, 115), stereo_model, x2, l3, K); +graph.addStereoMeasurement(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K); +graph.addStereoMeasurement(StereoPoint2( 70, 20, 490), stereo_model, x2, l2, 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))); -expected_l1 = gtsamPoint3( 1, 1, 5); +initialEstimate.insertPose(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1))); +expected_l1 = Point3( 1, 1, 5); initialEstimate.insertPoint(l1, expected_l1); -initialEstimate.insertPoint(l2, gtsamPoint3(-1, 1, 5)); -initialEstimate.insertPoint(l3, gtsamPoint3( 0,-.5, 5)); +initialEstimate.insertPoint(l2, Point3(-1, 1, 5)); +initialEstimate.insertPoint(l3, Point3( 0,-.5, 5)); %% optimize result = graph.optimize(initialEstimate,0); diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 279050e25..6956b310b 100644 --- a/wrap/Argument.cpp +++ b/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; } diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 5f7d7aa6e..8e04543e4 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -24,6 +24,7 @@ #include // works on Linux GCC #include +#include #include #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& 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 matlabBaseName = wrap::qualifiedName("", qualifiedParent); + const string matlabQualName = qualifiedName("."), matlabUniqueName = qualifiedName(), cppName = qualifiedName("::"); + 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() ? - "handle" : ::wrap::qualifiedName("", qualifiedParent); - proxyFile.oss << "classdef " << matlabName << " < " << parent << endl; + const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName; + proxyFile.oss << "classdef " << name << " < " << 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& functionNames) const { - const string matlabName = qualifiedName(), cppName = qualifiedName("::"); - const string baseMatlabName = wrap::qualifiedName("", qualifiedParent); + const string matlabUniqueName = qualifiedName(), cppName = qualifiedName("::"); const string baseCppName = wrap::qualifiedName("::", qualifiedParent); const int collectorInsertId = (int)functionNames.size(); - const string collectorInsertFunctionName = matlabName + "_collectorInsertAndMakeBase_" + boost::lexical_cast(collectorInsertId); + const string collectorInsertFunctionName = matlabUniqueName + "_collectorInsertAndMakeBase_" + boost::lexical_cast(collectorInsertId); functionNames.push_back(collectorInsertFunctionName); int upcastFromVoidId; string upcastFromVoidFunctionName; if(isVirtual) { upcastFromVoidId = (int)functionNames.size(); - upcastFromVoidFunctionName = matlabName + "_upcastFromVoid_" + boost::lexical_cast(upcastFromVoidId); + upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + boost::lexical_cast(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 (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"; diff --git a/wrap/Class.h b/wrap/Class.h index 9a6ca12c7..c41cbb470 100644 --- a/wrap/Class.h +++ b/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& functionNames) const; ///< emit proxy class std::string qualifiedName(const std::string& delim = "") const; ///< creates a namespace-qualified name, optional delimiter diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index afb35f62d..274180e08 100644 --- a/wrap/Constructor.cpp +++ b/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& using_namespaces, const ArgumentList& al) const { - const string wrapFunctionName = matlabClassName + "_constructor_" + boost::lexical_cast(id); + const string wrapFunctionName = matlabUniqueName + "_constructor_" + boost::lexical_cast(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; diff --git a/wrap/Constructor.h b/wrap/Constructor.h index 50d0c41f5..e2eca1291 100644 --- a/wrap/Constructor.h +++ b/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, - const ArgumentList args) const; + bool hasParent, const int id, 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& using_namespaces, diff --git a/wrap/Deconstructor.cpp b/wrap/Deconstructor.cpp index c02ab0e93..c9239ee74 100644 --- a/wrap/Deconstructor.cpp +++ b/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& 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(id); + const string wrapFunctionName = matlabUniqueName + "_deconstructor_" + boost::lexical_cast(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(mxGetData(in[0]));\n"; - file.oss << " Collector_" << matlabClassName << "::iterator item;\n"; - file.oss << " item = collector_" << matlabClassName << ".find(self);\n"; - file.oss << " if(item != collector_" << matlabClassName << ".end()) {\n"; + file.oss << " Collector_" << matlabUniqueName << "::iterator item;\n"; + file.oss << " item = collector_" << matlabUniqueName << ".find(self);\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; diff --git a/wrap/Deconstructor.h b/wrap/Deconstructor.h index 1ef5b80b0..0d7dadaad 100644 --- a/wrap/Deconstructor.h +++ b/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& using_namespaces) const; }; diff --git a/wrap/Method.cpp b/wrap/Method.cpp index f7ee5c71c..eb143b4dc 100644 --- a/wrap/Method.cpp +++ b/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& 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& using_namespaces, @@ -107,7 +108,7 @@ string Method::wrapper_fragment(FileWriter& file, // generate code - const string wrapFunctionName = matlabClassName + "_" + name + "_" + boost::lexical_cast(id); + const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + boost::lexical_cast(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 = 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); diff --git a/wrap/Method.h b/wrap/Method.h index c23fd7c3d..e23a24186 100644 --- a/wrap/Method.h +++ b/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& using_namespaces, const TypeAttributesTable& typeAttributes, std::vector& 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& using_namespaces, diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 8ad476cd3..1f077b31f 100644 --- a/wrap/Module.cpp +++ b/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(classFile, wrapperName, typeAttributes, wrapperFile, functionNames); + cls.matlab_proxy(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames); } // finish wrapper file @@ -527,11 +526,12 @@ vector Module::GenerateValidTypes(const vector& classes, const ve void Module::WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector& 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*> " - << "Collector_" << matlabName << ";\n"; - wrapperFile.oss << "static Collector_" << matlabName << - " collector_" << matlabName << ";\n"; + << "Collector_" << matlabUniqueName << ";\n"; + wrapperFile.oss << "static Collector_" << matlabUniqueName << + " 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 collectorName = "collector_" + matlabName; + const string collectorType = "Collector_" + matlabUniqueName; + 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"; diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index a3efb094e..c209a3131 100644 --- a/wrap/ReturnValue.cpp +++ b/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 cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2(); + string cppType1 = qualifiedType1("::"), matlabType1 = qualifiedType1("."); + string cppType2 = qualifiedType2("::"), matlabType2 = qualifiedType2("."); if (isPair) { // first return value in pair diff --git a/wrap/StaticMethod.cpp b/wrap/StaticMethod.cpp index 19dcd375e..fcef79857 100644 --- a/wrap/StaticMethod.cpp +++ b/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& 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& using_namespaces, @@ -110,7 +111,7 @@ string StaticMethod::wrapper_fragment(FileWriter& file, // generate code - const string wrapFunctionName = matlabClassName + "_" + name + "_" + boost::lexical_cast(id); + const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + boost::lexical_cast(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 diff --git a/wrap/StaticMethod.h b/wrap/StaticMethod.h index 8951f965b..55d9f3e99 100644 --- a/wrap/StaticMethod.h +++ b/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& using_namespaces, const TypeAttributesTable& typeAttributes, std::vector& 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& using_namespaces, diff --git a/wrap/tests/expected_namespaces/ns1ClassA.m b/wrap/tests/expected_namespaces/+ns1/ClassA.m similarity index 80% rename from wrap/tests/expected_namespaces/ns1ClassA.m rename to wrap/tests/expected_namespaces/+ns1/ClassA.m index 29f48ab4e..f0682c815 100644 --- a/wrap/tests/expected_namespaces/ns1ClassA.m +++ b/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 diff --git a/wrap/tests/expected_namespaces/ns1ClassB.m b/wrap/tests/expected_namespaces/+ns1/ClassB.m similarity index 80% rename from wrap/tests/expected_namespaces/ns1ClassB.m rename to wrap/tests/expected_namespaces/+ns1/ClassB.m index a1e9bc9a5..a263bc1e8 100644 --- a/wrap/tests/expected_namespaces/ns1ClassB.m +++ b/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 diff --git a/wrap/tests/expected_namespaces/ns2ns3ClassB.m b/wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m similarity index 79% rename from wrap/tests/expected_namespaces/ns2ns3ClassB.m rename to wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m index b5e57ea19..2c19e13a8 100644 --- a/wrap/tests/expected_namespaces/ns2ns3ClassB.m +++ b/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 diff --git a/wrap/tests/expected_namespaces/ns2ClassA.m b/wrap/tests/expected_namespaces/+ns2/ClassA.m similarity index 84% rename from wrap/tests/expected_namespaces/ns2ClassA.m rename to wrap/tests/expected_namespaces/+ns2/ClassA.m index 7b3df9ed6..e7a074c08 100644 --- a/wrap/tests/expected_namespaces/ns2ClassA.m +++ b/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 diff --git a/wrap/tests/expected_namespaces/ns2ClassC.m b/wrap/tests/expected_namespaces/+ns2/ClassC.m similarity index 80% rename from wrap/tests/expected_namespaces/ns2ClassC.m rename to wrap/tests/expected_namespaces/+ns2/ClassC.m index 5ba6e7125..f56a9c5cb 100644 --- a/wrap/tests/expected_namespaces/ns2ClassC.m +++ b/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 diff --git a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp index 6e65b1a0f..3eb0b0311 100644 --- a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp +++ b/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(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[]) diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 4aa5b6f58..0ed1f84ef 100644 --- a/wrap/tests/testWrap.cpp +++ b/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 + "ns1ClassB.m" , act_path + "ns1ClassB.m" )); - EXPECT(files_equal(exp_path + "ns2ClassA.m" , act_path + "ns2ClassA.m" )); - EXPECT(files_equal(exp_path + "ns2ClassC.m" , act_path + "ns2ClassC.m" )); - EXPECT(files_equal(exp_path + "ns2ns3ClassB.m" , act_path + "ns2ns3ClassB.m" )); + EXPECT(files_equal(exp_path + "+ns1/ClassA.m" , act_path + "+ns1/ClassA.m" )); + EXPECT(files_equal(exp_path + "+ns1/ClassB.m" , act_path + "+ns1/ClassB.m" )); + EXPECT(files_equal(exp_path + "+ns2/ClassA.m" , act_path + "+ns2/ClassA.m" )); + EXPECT(files_equal(exp_path + "+ns2/ClassC.m" , act_path + "+ns2/ClassC.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" )); } diff --git a/wrap/utilities.h b/wrap/utilities.h index 0a252484f..7e28d6793 100644 --- a/wrap/utilities.h +++ b/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_;