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

Use p instead of l for points

release/4.3a0
Frank Dellaert 2012-06-14 05:05:41 +00:00
parent 5a5d246b30
commit 2524c632df
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
matlab/examples/SFMExample.m
View File

@ -36,7 +36,7 @@ measurementNoise = gtsamSharedNoiseModel_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.addMeasurement(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('l',j), data.K); graph.addMeasurement(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('p',j), data.K);
end end
end end
@ -44,7 +44,7 @@ end
posePriorNoise = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas); posePriorNoise = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas);
graph.addPosePrior(symbol('x',1), truth.cameras{1}.pose, posePriorNoise); graph.addPosePrior(symbol('x',1), truth.cameras{1}.pose, posePriorNoise);
pointPriorNoise = gtsamSharedNoiseModel_Sigma(3,pointNoiseSigma); pointPriorNoise = gtsamSharedNoiseModel_Sigma(3,pointNoiseSigma);
graph.addPointPrior(symbol('l',1), truth.points{1}, pointPriorNoise); graph.addPointPrior(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'));
@ -55,7 +55,7 @@ for i=1:size(truth.cameras,2)
initialEstimate.insertPose(symbol('x',i), truth.cameras{i}.pose); initialEstimate.insertPose(symbol('x',i), truth.cameras{i}.pose);
end end
for j=1:size(truth.points,2) for j=1:size(truth.points,2)
initialEstimate.insertPoint(symbol('l',j), truth.points{j}); initialEstimate.insertPoint(symbol('p',j), truth.points{j});
end end
initialEstimate.print(sprintf('\nInitial estimate:\n ')); initialEstimate.print(sprintf('\nInitial estimate:\n '));
@ -68,8 +68,8 @@ marginals = graph.marginals(result);
cla cla
hold on; hold on;
for j=1:result.nrPoints for j=1:result.nrPoints
P = marginals.marginalCovariance(symbol('l',j)); P = marginals.marginalCovariance(symbol('p',j));
point_j = result.point(symbol('l',j)); point_j = result.point(symbol('p',j));
plot3(point_j.x, point_j.y, point_j.z,'marker','o'); plot3(point_j.x, point_j.y, point_j.z,'marker','o');
covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
end end