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

create and use cameras, not poses

release/4.3a0
Frank Dellaert 2012-06-07 04:09:22 +00:00
parent 903580abb4
commit c78e649042
3 changed files with 30 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
examples/matlab/VisualISAMExample.m
View File

@ -19,19 +19,18 @@ for j=1:nPoints
points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
end
%% Create camera poses on a circle around the triangle
%% Create camera cameras on a circle around the triangle
nCameras = 10;
height = 10;
r = 30;
poses = {};
cameras = {};
K = gtsamCal3_S2(500,500,0,640/2,480/2);
for i=1:nCameras
theta = (i-1)*2*pi/nCameras;
t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
camera = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K)
poses{i} = camera.pose();
cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K);
end
odometry = poses{1}.between(poses{2});
odometry = cameras{1}.pose.between(cameras{2}.pose);
poseNoise = gtsamSharedNoiseModel_Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
pointNoise = gtsamSharedNoiseModel_Sigma(3, 0.1);
@ -45,9 +44,9 @@ newFactors = visualSLAMGraph;
initialEstimates = visualSLAMValues;
for i=1:nCameras
% Prior for the first pose or odometry for subsequent poses
% Prior for the first pose or odometry for subsequent cameras
if (i==1)
newFactors.addPosePrior(symbol('x',1), poses{1}, poseNoise);
newFactors.addPosePrior(symbol('x',1), cameras{1}.pose, poseNoise);
for j=1:nPoints
newFactors.addPointPrior(symbol('l',j), points{j}, pointNoise);
end
@ -57,22 +56,21 @@ for i=1:nCameras
% Visual measurement factors
for j=1:nPoints
camera = gtsamSimpleCamera(K,poses{i});
zij = camera.project(points{j});
zij = cameras{i}.project(points{j});
newFactors.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K);
end
% Initial estimates for the new pose. Also initialize points while in
% the first frame.
if (i==1)
initialEstimates.insertPose(symbol('x',i), poses{i});
initialEstimates.insertPose(symbol('x',i), cameras{i}.pose);
for j=1:size(points,2)
initialEstimates.insertPoint(symbol('l',j), points{j});
end
else
%TODO: this might be suboptimal since "result" is not the fully
%optimized result
if (i==2), prevPose = poses{1};
if (i==2), prevPose = cameras{1}.pose;
else, prevPose = result.pose(symbol('x',i-1)); end
initialEstimates.insertPose(symbol('x',i), prevPose.compose(odometry));
end

23
examples/matlab/VisualSLAMExample.m
View File

@ -27,17 +27,16 @@ points = {gtsamPoint3([10 10 10]'),...
gtsamPoint3([-10 -10 -10]'),...
gtsamPoint3([10 -10 -10]')};
% Camera poses on a circle around the cube, pointing at the world origin
% Camera cameras on a circle around the cube, pointing at the world origin
nCameras = 6;
height = 0;
r = 30;
poses = {};
cameras = {};
K = gtsamCal3_S2(500,500,0,640/2,480/2);
for i=1:nCameras
theta = (i-1)*2*pi/nCameras;
t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
camera = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K)
poses{i} = camera.pose();
cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K);
end
measurementNoiseSigma = 1.0;
@ -50,26 +49,25 @@ graph = visualSLAMGraph;
%% Add factors for all measurements
measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma);
for i=1:nCameras
camera = gtsamSimpleCamera(K,poses{i});
for j=1:size(points,2)
zij = camera.project(points{j}); % you can add noise here if desired
zij = cameras{i}.project(points{j}); % you can add noise here if desired
graph.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K);
end
end
%% Add Gaussian priors for a pose and a landmark to constrain the system
posePriorNoise = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas);
graph.addPosePrior(symbol('x',1), poses{1}, posePriorNoise);
graph.addPosePrior(symbol('x',1), cameras{1}.pose, posePriorNoise);
pointPriorNoise = gtsamSharedNoiseModel_Sigma(3,pointNoiseSigma);
graph.addPointPrior(symbol('l',1), points{1}, pointPriorNoise);
%% Print the graph
graph.print(sprintf('\nFactor graph:\n'));
%% Initialize to noisy poses and points
%% Initialize to noisy cameras and points
initialEstimate = visualSLAMValues;
for i=1:size(poses,2)
initialEstimate.insertPose(symbol('x',i), poses{i});
for i=1:size(cameras,2)
initialEstimate.insertPose(symbol('x',i), cameras{i}.pose);
end
for j=1:size(points,2)
initialEstimate.insertPoint(symbol('l',j), points{j});
@ -91,10 +89,11 @@ for j=1:size(points,2)
covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
end
for i=1:size(poses,2)
for i=1:size(cameras,2)
P = marginals.marginalCovariance(symbol('x',i))
pose_i = result.pose(symbol('x',i))
plotPose3(pose_i,P,10);
end
axis([-35 35 -35 35 -15 15]);
axis equal
view(-37,40)

21
examples/matlab/VisualSLAMExample_triangle.m
View File

@ -18,17 +18,16 @@ for j=1:3
points{j} = gtsamPoint3([r*cos(theta), r*sin(theta), 0]');
end
%% Create camera poses on a circle around the triangle
%% Create camera cameras on a circle around the triangle
nCameras = 6;
height = 10;
r = 30;
poses = {};
cameras = {};
K = gtsamCal3_S2(500,500,0,640/2,480/2);
for i=1:nCameras
theta = (i-1)*2*pi/nCameras;
t = gtsamPoint3([r*cos(theta), r*sin(theta), height]');
camera = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K)
poses{i} = camera.pose();
cameras{i} = gtsamSimpleCamera_lookat(t, gtsamPoint3, gtsamPoint3([0,0,1]'), K)
end
%% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph)
@ -38,9 +37,8 @@ graph = visualSLAMGraph;
measurementNoiseSigma=1; % in pixels
measurementNoise = gtsamSharedNoiseModel_Sigma(2,measurementNoiseSigma);
for i=1:nCameras
camera = gtsamSimpleCamera(K,poses{i});
for j=1:3
zij = camera.project(points{j}); % you can add noise here if desired
zij = cameras{i}.project(points{j}); % you can add noise here if desired
graph.addMeasurement(zij, measurementNoise, symbol('x',i), symbol('l',j), K);
end
end
@ -54,10 +52,10 @@ end
%% Print the graph
graph.print(sprintf('\nFactor graph:\n'));
%% Initialize to noisy poses and points
%% Initialize to noisy cameras and points
initialEstimate = visualSLAMValues;
for i=1:size(poses,2)
initialEstimate.insertPose(symbol('x',i), poses{i});
for i=1:size(cameras,2)
initialEstimate.insertPose(symbol('x',i), cameras{i}.pose);
end
for j=1:size(points,2)
initialEstimate.insertPoint(symbol('l',j), points{j});
@ -79,10 +77,11 @@ for j=1:size(points,2)
covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P);
end
for i=1:size(poses,2)
for i=1:size(cameras,2)
P = marginals.marginalCovariance(symbol('x',i))
pose_i = result.pose(symbol('x',i))
plotPose3(pose_i,P,10);
end
axis([-20 20 -20 20 -1 15]);
axis equal
view(-37,40)