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

VisualISAMExample_triangle

release/4.3a0
Duy-Nguyen Ta 2012-06-07 07:43:56 +00:00
parent 2a633b75c8
commit ce71979c8c
1 changed files with 13 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
examples/matlab/VisualISAMExample_Duy.m → examples/matlab/VisualISAMExample_triangle.m
View File

@ -20,7 +20,7 @@ for j=1:nPoints
end end
%% Create camera cameras on a circle around the triangle %% Create camera cameras on a circle around the triangle
nCameras = 30; nCameras = 10;
height = 10; height = 10;
r = 30; r = 30;
cameras = {}; cameras = {};
@ -32,12 +32,13 @@ for i=1:nCameras
end end
odometry = cameras{1}.pose.between(cameras{2}.pose); odometry = cameras{1}.pose.between(cameras{2}.pose);
poseNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.5 0.5 0.5]'); posepriorNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 5.0 5.0 5.0]');
odometryNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 2.0 2.0 2.0]');
pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1); pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1);
measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0); measurementNoise = gtsamSharedNoiseModel_Sigma(2, 1.0);
%% Create an ISAM object for inference %% Create an ISAM object for inference
isam = visualSLAMISAM(5); isam = visualSLAMISAM(2);
%% Update ISAM %% Update ISAM
newFactors = visualSLAMGraph; newFactors = visualSLAMGraph;
@ -47,12 +48,10 @@ for i=1:nCameras
% Prior for the first pose or odometry for subsequent cameras % Prior for the first pose or odometry for subsequent cameras
if (i==1) if (i==1)
newFactors.addPosePrior(symbol('x',1), cameras{1}.pose, poseNoise); newFactors.addPosePrior(symbol('x',1), cameras{1}.pose, posepriorNoise);
for j=1:nPoints newFactors.addPointPrior(symbol('l',1), points{1}, pointNoise);
newFactors.addPointPrior(symbol('l',j), points{j}, pointNoise);
end
else else
newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, poseNoise); newFactors.addOdometry(symbol('x',i-1), symbol('x',i), odometry, odometryNoise);
end end
% Visual measurement factors % Visual measurement factors
@ -65,14 +64,14 @@ for i=1:nCameras
% the first frame. % the first frame.
if (i==1) if (i==1)
initialEstimates.insertPose(symbol('x',i), cameras{i}.pose); initialEstimates.insertPose(symbol('x',i), cameras{i}.pose);
for j=1:size(points,2) for j=1:nPoints
initialEstimates.insertPoint(symbol('l',j), points{j}); initialEstimates.insertPoint(symbol('l',j), points{j});
end end
else else
%TODO: this might be suboptimal since "result" is not the fully %TODO: this might be suboptimal since "result" is not the fully
%optimized result %optimized result
if (i==2), prevPose = cameras{1}.pose; if (i==2), prevPose = cameras{1}.pose;
else, prevPose = result.pose(symbol('x',i-1)); end else prevPose = result.pose(symbol('x',i-1)); end
initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry)); initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry));
end end
@ -83,16 +82,16 @@ for i=1:nCameras
result = isam.estimate(); result = isam.estimate();
% Plot results % Plot results
h=figure(1); h=figure(1); clf;
hold on; hold on;
for j=1:size(points,2) for j=1:nPoints
P = isam.marginalCovariance(symbol('l',j)); P = isam.marginalCovariance(symbol('l',j));
point_j = result.point(symbol('l',j)); point_j = result.point(symbol('l',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
for ii=i-1:i for ii=1:i
P = isam.marginalCovariance(symbol('x',ii)); P = isam.marginalCovariance(symbol('x',ii));
pose_ii = result.pose(symbol('x',ii)); pose_ii = result.pose(symbol('x',ii));
plotPose3(pose_ii,P,10); plotPose3(pose_ii,P,10);