diff --git a/matlab/tests/testLocalizationExample.m b/matlab/tests/testLocalizationExample.m index 35c16d85f..bbe91641d 100644 --- a/matlab/tests/testLocalizationExample.m +++ b/matlab/tests/testLocalizationExample.m @@ -11,41 +11,46 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) -graph = pose2SLAM.Graph; +import gtsam.* +graph = NonlinearFactorGraph; %% Add two odometry factors 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); +graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); %% Add three "GPS" measurements % We use Pose2 Priors here with high variance on theta 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)); +groundTruth = Values; +groundTruth.insert(1, Pose2(0.0, 0.0, 0.0)); +groundTruth.insert(2, Pose2(2.0, 0.0, 0.0)); +groundTruth.insert(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), model); + graph.add(PriorFactorPose2(i, groundTruth.at(i), model)); end %% Initialize to noisy points -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)); +import gtsam.* +initialEstimate = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,0); +import gtsam.* +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); %% Plot Covariance Ellipses -marginals = graph.marginals(result); +import gtsam.* +marginals = Marginals(graph, result); P={}; for i=1:result.size() - pose_i = result.pose(i); - CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.pose(i),1e-4)); + pose_i = result.at(i); + CHECK('pose_i.equals(groundTruth.pose(i)',pose_i.equals(groundTruth.at(i),1e-4)); P{i}=marginals.marginalCovariance(i); end diff --git a/matlab/tests/testOdometryExample.m b/matlab/tests/testOdometryExample.m index e793df9a8..17b90157a 100644 --- a/matlab/tests/testOdometryExample.m +++ b/matlab/tests/testOdometryExample.m @@ -11,33 +11,35 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) -graph = pose2SLAM.Graph; +import gtsam.* +graph = NonlinearFactorGraph; %% Add a Gaussian prior on pose x_1 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 +graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add two odometry factors 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); +graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)); %% Initialize to noisy points 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 = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,0); -marginals = graph.marginals(result); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); +marginals = Marginals(graph, result); marginals.marginalCovariance(1); %% Check first pose equality -pose_1 = result.pose(1); +pose_1 = result.at(1); 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 4c3e6c9da..c6d4cb352 100644 --- a/matlab/tests/testPlanarSLAMExample.m +++ b/matlab/tests/testPlanarSLAMExample.m @@ -24,49 +24,52 @@ 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 = planarSLAM.Graph; +import gtsam.* +graph = NonlinearFactorGraph; %% Add prior 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 +graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph %% Add odometry 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); +graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); %% Add bearing/range measurement factors import gtsam.* degrees = pi/180; 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); +graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); +graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); +graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); %% Initialize to noisy points 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 = Values; +initialEstimate.insert(i1, Pose2(0.5, 0.0, 0.2)); +initialEstimate.insert(i2, Pose2(2.3, 0.1,-0.2)); +initialEstimate.insert(i3, Pose2(4.1, 0.1, 0.1)); +initialEstimate.insert(j1, Point2(1.8, 2.1)); +initialEstimate.insert(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); +import gtsam.* +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); +marginals = Marginals(graph, result); %% Check first pose and point equality import gtsam.* -pose_1 = result.pose(symbol('x',1)); +pose_1 = result.at(symbol('x',1)); marginals.marginalCovariance(symbol('x',1)); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); -point_1 = result.point(symbol('l',1)); +point_1 = result.at(symbol('l',1)); marginals.marginalCovariance(symbol('l',1)); 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 cd778cba3..9d934aab9 100644 --- a/matlab/tests/testPose2SLAMExample.m +++ b/matlab/tests/testPose2SLAMExample.m @@ -19,50 +19,60 @@ % - The robot is on a grid, moving 2 meters each step %% Create graph container and add factors to it -graph = pose2SLAM.Graph; +import gtsam.* +graph = NonlinearFactorGraph; %% Add prior import gtsam.* % gaussian for prior 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 +graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph %% Add odometry % general noisemodel for odometry 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); +graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise)); +graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise)); +graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise)); +graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise)); %% Add pose constraint import gtsam.* model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); -graph.addRelativePose(5, 2, Pose2(2.0, 0.0, pi/2), model); +graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model)); %% Initialize to noisy points 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 = Values; +initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); +initialEstimate.insert(2, Pose2(2.3, 0.1,-0.2 )); +initialEstimate.insert(3, Pose2(4.1, 0.1, pi/2)); +initialEstimate.insert(4, Pose2(4.0, 2.0, pi )); +initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2)); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -result = graph.optimize(initialEstimate,0); -resultSPCG = graph.optimizeSPCG(initialEstimate,0); +import gtsam.* +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); + +%% Optimize using SPCG +%import gtsam.* +%params = LevenbergMarquardtParams; +%params.setLinearSolverType('CONJUGATE_GRADIENT'); +%optimizerSPCG = LevenbergMarquardtOptimizer(graph, initialEstimate, params); +%resultSPCG = optimizerSPCG.optimize(); %% Plot Covariance Ellipses -marginals = graph.marginals(result); +import gtsam.* +marginals = Marginals(graph, result); P = marginals.marginalCovariance(1); -pose_1 = result.pose(1); +pose_1 = result.at(1); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); -poseSPCG_1 = resultSPCG.pose(1); -CHECK('poseSPCG_1.equals(Pose2,1e-4)',poseSPCG_1.equals(Pose2,1e-4)); +%poseSPCG_1 = resultSPCG.at(1); +%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 ac3c0d5bb..0d47990e8 100644 --- a/matlab/tests/testPose3SLAMExample.m +++ b/matlab/tests/testPose3SLAMExample.m @@ -17,31 +17,32 @@ p1 = hexagon.pose(1); %% create a Pose graph with one equality constraint and one measurement import gtsam.* -fg = pose3SLAM.Graph; -fg.addPoseConstraint(0, p0); +fg = NonlinearFactorGraph; +fg.add(NonlinearEqualityPose3(0, p0)); delta = p0.between(p1); 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); -fg.addRelativePose(3,4, delta, covariance); -fg.addRelativePose(4,5, delta, covariance); -fg.addRelativePose(5,0, delta, covariance); +fg.add(BetweenFactorPose3(0,1, delta, covariance)); +fg.add(BetweenFactorPose3(1,2, delta, covariance)); +fg.add(BetweenFactorPose3(2,3, delta, covariance)); +fg.add(BetweenFactorPose3(3,4, delta, covariance)); +fg.add(BetweenFactorPose3(4,5, delta, covariance)); +fg.add(BetweenFactorPose3(5,0, delta, covariance)); %% Create initial config -initial = pose3SLAM.Values; +initial = Values; s = 0.10; -initial.insertPose(0, p0); -initial.insertPose(1, hexagon.pose(1).retract(s*randn(6,1))); -initial.insertPose(2, hexagon.pose(2).retract(s*randn(6,1))); -initial.insertPose(3, hexagon.pose(3).retract(s*randn(6,1))); -initial.insertPose(4, hexagon.pose(4).retract(s*randn(6,1))); -initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1))); +initial.insert(0, p0); +initial.insert(1, hexagon.pose(1).retract(s*randn(6,1))); +initial.insert(2, hexagon.pose(2).retract(s*randn(6,1))); +initial.insert(3, hexagon.pose(3).retract(s*randn(6,1))); +initial.insert(4, hexagon.pose(4).retract(s*randn(6,1))); +initial.insert(5, hexagon.pose(5).retract(s*randn(6,1))); %% optimize -result = fg.optimize(initial,0); +optimizer = LevenbergMarquardtOptimizer(fg, initial); +result = optimizer.optimizeSafely; -pose_1 = result.pose(1); +pose_1 = result.at(1); 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 b3595dc18..d83517c7e 100644 --- a/matlab/tests/testSFMExample.m +++ b/matlab/tests/testSFMExample.m @@ -10,17 +10,19 @@ % @author Duy-Nguyen Ta %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +import gtsam.* + options.triangle = false; options.nrCameras = 10; options.showImages = false; -[data,truth] = VisualISAMGenerateData(options); +[data,truth] = support.VisualISAMGenerateData(options); measurementNoiseSigma = 1.0; pointNoiseSigma = 0.1; poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; -graph = visualSLAM.Graph; +graph = NonlinearFactorGraph; %% Add factors for all measurements import gtsam.* @@ -28,47 +30,43 @@ measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma); for i=1:length(data.Z) for k=1:length(data.Z{i}) j = data.J{i}{k}; - graph.addMeasurement(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('p',j), data.K); + graph.add(GenericProjectionFactorCal3_S2(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('p',j), data.K)); end end posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas); -graph.addPosePrior(symbol('x',1), truth.cameras{1}.pose, posePriorNoise); +graph.add(PriorFactorPose3(symbol('x',1), truth.cameras{1}.pose, posePriorNoise)); pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); -graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise); +graph.add(PriorFactorPose3(symbol('p',1), truth.points{1}, pointPriorNoise)); %% Initial estimate -initialEstimate = visualSLAM.Values; +initialEstimate = Values; for i=1:size(truth.cameras,2) pose_i = truth.cameras{i}.pose; - initialEstimate.insertPose(symbol('x',i), pose_i); + initialEstimate.insert(symbol('x',i), pose_i); end for j=1:size(truth.points,2) point_j = truth.points{j}; - initialEstimate.insertPoint(symbol('p',j), point_j); + initialEstimate.insert(symbol('p',j), point_j); end %% Optimization import gtsam.* -parameters = LevenbergMarquardtParams; -optimizer = graph.optimizer(initialEstimate, parameters); +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); for i=1:5 optimizer.iterate(); end result = optimizer.values(); %% Marginalization -marginals = graph.marginals(result); +marginals = Marginals(graph, result); marginals.marginalCovariance(symbol('p',1)); marginals.marginalCovariance(symbol('x',1)); %% Check optimized results, should be equal to ground truth -for i=1:size(truth.cameras,2) - pose_i = result.pose(symbol('x',i)); - CHECK('pose_i.equals(truth.cameras{i}.pose,1e-5)',pose_i.equals(truth.cameras{i}.pose,1e-5)) -end - -for j=1:size(truth.points,2) - point_j = result.point(symbol('p',j)); - CHECK('point_j.equals(truth.points{j},1e-5)',point_j.equals(truth.points{j},1e-5)) +keys = truth.keys; +for i=0:keys.size-1 + truth_i = truth.at(keys.at(i)); + result_i = result.at(keys.at(i)); + CHECK('result_i.equals(truth_i,1e-5)',result_i.equals(truth_i,1e-5)) end