12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

release/4.3a0
Richard Roberts 2012-07-18 15:47:06 +00:00
parent 8fa77de2a0
commit 8dbffd4629
55 changed files with 493 additions and 389 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
matlab/CMakeLists.txt
View File

@ -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)

26
matlab/VisualISAMGenerateData.m
View File

@ -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

15
matlab/VisualISAMInitialize.m
View File

@ -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

3
matlab/VisualISAMPlot.m
View File

@ -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);

7
matlab/VisualISAMStep.m
View File

@ -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));

27
matlab/examples/LocalizationExample.m
View File

@ -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

22
matlab/examples/OdometryExample.m
View File

@ -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

36
matlab/examples/PlanarSLAMExample.m
View File

@ -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

40
matlab/examples/PlanarSLAMExample_sampling.m
View File

@ -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

36
matlab/examples/Pose2SLAMExample.m
View File

@ -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

26
matlab/examples/Pose2SLAMExample_advanced.m
View File

@ -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);
%

8
matlab/examples/Pose2SLAMExample_circle.m
View File

@ -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]'));

8
matlab/examples/Pose2SLAMExample_graph.m
View File

@ -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

37
matlab/examples/Pose2SLAMwSPCG.m
View File

@ -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

8
matlab/examples/Pose3SLAMExample.m
View File

@ -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)));

4
matlab/examples/Pose3SLAMExample_graph.m
View File

@ -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;

20
matlab/examples/SBAExample.m
View File

@ -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');

15
matlab/examples/SFMExample.m
View File

@ -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');

34
matlab/examples/StereoVOExample.m
View File

@ -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

17
matlab/examples/StereoVOExample_large.m
View File

@ -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

8
matlab/load2D.m
View File

@ -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

26
matlab/load3D.m
View File

@ -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 & i<N | successive & i==0)
t = gtsamPoint3(v{3}, v{4}, v{5});
R = gtsamRot3.Ypr(v{8}, -v{7}, v{6});
initial.insertPose(i, gtsamPose3(R,t));
i1=v{2};
if (~successive && i1<N || successive && i1==0)
t = gtsam.Point3(v{3}, v{4}, v{5});
R = gtsam.Rot3.Ypr(v{8}, -v{7}, v{6});
initial.insertPose(i1, gtsam.Pose3(R,t));
end
elseif strcmp('EDGE3',line_i(1:5))
e = textscan(line_i,'%s %d %d %f %f %f %f %f %f',1);
i1=e{2};
i2=e{3};
if i1<N & i2<N
if ~successive | abs(i2-i1)==1
t = gtsamPoint3(e{4}, e{5}, e{6});
R = gtsamRot3.Ypr(e{9}, e{8}, e{7});
dpose = gtsamPose3(R,t);
if i1<N && i2<N
if ~successive || abs(i2-i1)==1
t = gtsam.Point3(e{4}, e{5}, e{6});
R = gtsam.Rot3.Ypr(e{9}, e{8}, e{7});
dpose = gtsam.Pose3(R,t);
graph.addRelativePose(i1, i2, dpose, model);
if successive
if i2>i1

2
matlab/symbol.m
View File

@ -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();

2
matlab/symbolChr.m
View File

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

2
matlab/symbolIndex.m
View File

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

12
matlab/tests/testJacobianFactor.m
View File

@ -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

8
matlab/tests/testKalmanFilter.m
View File

@ -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];

28
matlab/tests/testLocalizationExample.m
View File

@ -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);

23
matlab/tests/testOdometryExample.m
View File

@ -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));

39
matlab/tests/testPlanarSLAMExample.m
View File

@ -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));

40
matlab/tests/testPose2SLAMExample.m
View File

@ -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));

11
matlab/tests/testPose3SLAMExample.m
View File

@ -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));

14
matlab/tests/testSFMExample.m
View File

@ -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();

34
matlab/tests/testStereoVOExample.m
View File

@ -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);

4
wrap/Argument.cpp
View File

@ -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;
}

62
wrap/Class.cpp
View File

@ -24,6 +24,7 @@
#include <stdint.h> // works on Linux GCC
#include <boost/foreach.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include "Class.h"
@ -34,29 +35,47 @@ using namespace std;
using namespace wrap;
/* ************************************************************************* */
void Class::matlab_proxy(const string& classFile, const string& wrapperName,
void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
const TypeAttributesTable& typeAttributes,
FileWriter& wrapperFile, vector<string>& functionNames) const {
// open destination classFile
// Create namespace folders
{
using namespace boost::filesystem;
path curPath = toolboxPath;
BOOST_FOREACH(const string& subdir, namespaces) {
curPath /= "+" + subdir;
if(!is_directory(curPath))
if(exists("+" + subdir))
throw OutputError("Need to write files to directory " + curPath.string() + ", which already exists as a file but is not a directory");
else
boost::filesystem::create_directory(curPath);
}
}
// open destination classFile
string classFile = toolboxPath;
if(!namespaces.empty())
classFile += "/+" + wrap::qualifiedName("/+", namespaces);
classFile += "/" + name + ".m";
FileWriter proxyFile(classFile, verbose_, "%");
// get the name of actual matlab object
const string matlabName = qualifiedName(), cppName = qualifiedName("::");
const string 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<string>& 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<string>(collectorInsertId);
const string collectorInsertFunctionName = matlabUniqueName + "_collectorInsertAndMakeBase_" + boost::lexical_cast<string>(collectorInsertId);
functionNames.push_back(collectorInsertFunctionName);
int upcastFromVoidId;
string upcastFromVoidFunctionName;
if(isVirtual) {
upcastFromVoidId = (int)functionNames.size();
upcastFromVoidFunctionName = matlabName + "_upcastFromVoid_" + boost::lexical_cast<string>(upcastFromVoidId);
upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + boost::lexical_cast<string>(upcastFromVoidId);
functionNames.push_back(upcastFromVoidFunctionName);
}
@ -184,7 +202,7 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wra
// Get self pointer passed in
wrapperFile.oss << " Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));\n";
// Add to collector
wrapperFile.oss << " collector_" << matlabName << ".insert(self);\n";
wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n";
// If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy)
if(!qualifiedParent.empty()) {
wrapperFile.oss << "\n";

2
wrap/Class.h
View File

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

16
wrap/Constructor.cpp
View File

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

5
wrap/Constructor.h
View File

@ -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<std::string>& using_namespaces,

18
wrap/Deconstructor.cpp
View File

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

4
wrap/Deconstructor.h
View File

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

15
wrap/Method.cpp
View File

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

4
wrap/Method.h
View File

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

22
wrap/Module.cpp
View File

@ -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<string> Module::GenerateValidTypes(const vector<Class>& classes, const ve
void Module::WriteCollectorsAndCleanupFcn(FileWriter& wrapperFile, const std::string& moduleName, const std::vector<Class>& classes) {
// Generate all collectors
BOOST_FOREACH(const Class& cls, classes) {
const string matlabName = cls.qualifiedName(), cppName = cls.qualifiedName("::");
const string matlabUniqueName = cls.qualifiedName(),
cppName = cls.qualifiedName("::");
wrapperFile.oss << "typedef std::set<boost::shared_ptr<" << cppName << ">*> "
<< "Collector_" << matlabName << ";\n";
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";

4
wrap/ReturnValue.cpp
View File

@ -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

15
wrap/StaticMethod.cpp
View File

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

4
wrap/StaticMethod.h
View File

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

6
wrap/tests/expected_namespaces/ns1ClassA.m → wrap/tests/expected_namespaces/+ns1/ClassA.m
View File

@ -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

6
wrap/tests/expected_namespaces/ns1ClassB.m → wrap/tests/expected_namespaces/+ns1/ClassB.m
View File

@ -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

6
wrap/tests/expected_namespaces/ns2ns3ClassB.m → wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m
View File

@ -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

16
wrap/tests/expected_namespaces/ns2ClassA.m → wrap/tests/expected_namespaces/+ns2/ClassA.m
View File

@ -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

6
wrap/tests/expected_namespaces/ns2ClassC.m → wrap/tests/expected_namespaces/+ns2/ClassC.m
View File

@ -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

2
wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp
View File

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

10
wrap/tests/testWrap.cpp
View File

@ -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" ));
}

9
wrap/utilities.h
View File

@ -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_;