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

stereo view works fine. but monocular camera still sufferes from the indeterminant system problem

release/4.3a0
lvzhaoyang 2015-01-14 00:19:17 -05:00
parent 2378d59632
commit 86f580b9ae
2 changed files with 18 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
matlab/+gtsam/points2DTrackStereo.m
View File

@ -63,9 +63,19 @@ marginals = Marginals(graph, initialEstimate);
%% get all the 2d points track information %% get all the 2d points track information
% currently throws the Indeterminant linear system exception % currently throws the Indeterminant linear system exception
for k = 1:cameraPosesNum for k = 1:cameraPosesNum
pts2dTracksStereo.pt3d{ptx} = pts3d{k}.data{idx}; num = length(pts3d{k}.data);
pts2dTracksStereo.Z{ptx} = pts3d{k}.Z{idx}; for i = 1:num
pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx})); pts2dTracksStereo.pt3d{i} = pts3d{k}.data{i};
pts2dTracksStereo.Z{i} = pts3d{k}.Z{i};
pts2dTracksStereo.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{i}));
end
end end
%% plot the result with covariance ellipses
hold on;
%plot3DPoints(initialEstimate, [], marginals);
%plot3DTrajectory(initialEstimate, '*', 1, 8, marginals);
plot3DTrajectory(initialEstimate, '*', 1, 8, marginals);
view(3);
end end

7
matlab/gtsam_examples/CameraFlyingExample.m
View File

@ -20,6 +20,8 @@ import gtsam.*
options.fieldSize = Point2([100, 100]'); options.fieldSize = Point2([100, 100]');
% the number of cylinders % the number of cylinders
options.cylinderNum = 10; options.cylinderNum = 10;
% point density on cylinder
options.density = 1;
% The number of camera poses % The number of camera poses
options.poseNum = 20; options.poseNum = 20;
% Monocular Camera Calibration % Monocular Camera Calibration
@ -29,7 +31,8 @@ options.stereoK = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
% the image size of camera % the image size of camera
options.imageSize = Point2([640, 480]'); options.imageSize = Point2([640, 480]');
% use Monocular camera or Stereo camera % use Monocular camera or Stereo camera
options.Mono = true; options.Mono = false;
%% test1: visibility test in monocular camera %% test1: visibility test in monocular camera
cylinders{1}.centroid = Point3(30, 50, 5); cylinders{1}.centroid = Point3(30, 50, 5);
@ -66,7 +69,7 @@ for i = 1:cylinderNum
x = 10 * cos(theta) + options.fieldSize.x/2; x = 10 * cos(theta) + options.fieldSize.x/2;
y = 10 * sin(theta) + options.fieldSize.y/2; y = 10 * sin(theta) + options.fieldSize.y/2;
baseCentroid = Point2([x, y]'); baseCentroid = Point2([x, y]');
cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, options.density);
end end
%% plot all the cylinders and sampled points %% plot all the cylinders and sampled points