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

Updated MATLAB SBAExample

release/4.3a0
Richard Roberts 2012-07-23 21:27:44 +00:00
parent cd69779754
commit 1db1663800
1 changed files with 9 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
matlab/examples/SBAExample.m
View File

@ -30,7 +30,8 @@ cameraNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1 ...
0.001*ones(1,5)]'; 0.001*ones(1,5)]';
%% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph) %% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph)
graph = sparseBA.Graph; import gtsam.*
graph = NonlinearFactorGraph;
%% Add factors for all measurements %% Add factors for all measurements
@ -39,7 +40,7 @@ measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma);
for i=1:length(data.Z) for i=1:length(data.Z)
for k=1:length(data.Z{i}) for k=1:length(data.Z{i})
j = data.J{i}{k}; j = data.J{i}{k};
graph.addSimpleCameraMeasurement(data.Z{i}{k}, measurementNoise, symbol('c',i), symbol('p',j)); graph.add(GeneralSFMFactorCal3_S2(data.Z{i}{k}, measurementNoise, symbol('c',i), symbol('p',j)));
end end
end end
@ -47,10 +48,10 @@ end
import gtsam.* import gtsam.*
cameraPriorNoise = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas); cameraPriorNoise = noiseModel.Diagonal.Sigmas(cameraNoiseSigmas);
firstCamera = SimpleCamera(truth.cameras{1}.pose, truth.K); firstCamera = SimpleCamera(truth.cameras{1}.pose, truth.K);
graph.addSimpleCameraPrior(symbol('c',1), firstCamera, cameraPriorNoise); graph.add(PriorFactorSimpleCamera(symbol('c',1), firstCamera, cameraPriorNoise));
pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise); graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise));
%% Print the graph %% Print the graph
graph.print(sprintf('\nFactor graph:\n')); graph.print(sprintf('\nFactor graph:\n'));
@ -58,15 +59,15 @@ graph.print(sprintf('\nFactor graph:\n'));
%% Initialize cameras and points close to ground truth in this example %% Initialize cameras and points close to ground truth in this example
import gtsam.* import gtsam.*
initialEstimate = sparseBA.Values; initialEstimate = Values;
for i=1:size(truth.cameras,2) for i=1:size(truth.cameras,2)
pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1)); pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1));
camera_i = SimpleCamera(pose_i, truth.K); camera_i = SimpleCamera(pose_i, truth.K);
initialEstimate.insertSimpleCamera(symbol('c',i), camera_i); initialEstimate.insert(symbol('c',i), camera_i);
end end
for j=1:size(truth.points,2) for j=1:size(truth.points,2)
point_j = truth.points{j}.retract(0.1*randn(3,1)); point_j = truth.points{j}.retract(0.1*randn(3,1));
initialEstimate.insertPoint(symbol('p',j), point_j); initialEstimate.insert(symbol('p',j), point_j);
end end
initialEstimate.print(sprintf('\nInitial estimate:\n ')); initialEstimate.print(sprintf('\nInitial estimate:\n '));
@ -77,7 +78,7 @@ parameters = LevenbergMarquardtParams;
parameters.setlambdaInitial(1.0); parameters.setlambdaInitial(1.0);
parameters.setVerbosityLM('trylambda'); parameters.setVerbosityLM('trylambda');
optimizer = graph.optimizer(initialEstimate, parameters); optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, parameters);
for i=1:5 for i=1:5
optimizer.iterate(); optimizer.iterate();