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

use circle generator to replace the random data generator. This can fix the indeterminant system error.

release/4.3a0
lvzhaoyang 2015-01-13 01:32:59 -05:00
parent 10dc767eda
commit 6ab95f60c2
1 changed files with 38 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
matlab/gtsam_examples/CameraFlyingExample.m
View File

@ -3,15 +3,26 @@ clc;
clf; clf;
import gtsam.* import gtsam.*
%% define the options
options.fieldSize = Point2([100, 100]');
options.cylinderNum = 10;
options.poseNum = 20;
options.monoK = Cal3_S2(525,525,0,320,240);
options.stereoK = Cal3_S2Stereo(721,721,0.0,609,172,0.53715); % read from the VO calibration file
options.imageSize = Point2([640, 480]');
%% generate a set of cylinders and Samples %% generate a set of cylinders and Samples
fieldSize = Point2([100, 100]'); cylinderNum = options.cylinderNum;
cylinderNum = 10;
cylinders = cell(cylinderNum, 1); cylinders = cell(cylinderNum, 1);
% ToDo: it seems random generated cylinders doesn't work that well % It seems random generated cylinders doesn't work that well
% use fixed parameters instead % Now it set up a circle of cylinders
theta = 0;
for i = 1:cylinderNum for i = 1:cylinderNum
baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]'); theta = theta + 2*pi / 10;
x = 20 * cos(theta) + options.fieldSize.x/2;
y = 20 * sin(theta) + options.fieldSize.y/2;
baseCentroid = Point2([x, y]');
cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1);
end end
@ -19,23 +30,21 @@ end
% now is plotting on a 100 * 100 field % now is plotting on a 100 * 100 field
figID = 1; figID = 1;
figure(figID); figure(figID);
plotCylinderSamples(cylinders, fieldSize, figID); plotCylinderSamples(cylinders, options.fieldSize, figID);
%% generate camera trajectories %% generate camera trajectories
K = Cal3_S2(525,525,0,320,240); K = Cal3_S2(525,525,0,320,240);
imageSize = Point2([640, 480]'); imageSize = Point2([640, 480]');
poseNum = 10; cameras = cell(options.poseNum, 1);
cameras = cell(poseNum, 1); % Generate ground truth trajectory r.w.t. the field center
trans = Point3(); theta = 0;
% To ensure there are landmarks in view, look at one randomly chosen cylinder r = 30;
% each time. for i = 1:options.poseNum
for i = 1:poseNum theta = (i-1)*2*pi/options.poseNum;
cylinderIdx = max(min(round(cylinderNum*rand), 10), 1); t = Point3([30*cos(theta), 30*sin(theta), 10]');
cameras{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ... cameras{i} = SimpleCamera.Lookat(t, ...
Point3([0,0,1]'), K); Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), K);
incT = Point3(5*rand, 5*rand, 5*rand);
trans = trans.compose(incT);
end end
%% visibility validation %% visibility validation
@ -46,25 +55,23 @@ visiblePoints3 = cylinderSampleProjection(cameras{1}, imageSize, cylinders);
%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); %plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID);
%% setp up monocular camera and get measurements %% setp up monocular camera and get measurements
%pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders); pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders);
%% set up stereo camera and get measurements %% set up stereo camera and get measurements
% load stereo calibration % load stereo calibration
calib = dlmread(findExampleDataFile('VO_calibration.txt')); calib = dlmread(findExampleDataFile('VO_calibration.txt'));
KStereo = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6)); KStereo = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6));
poseNum = 10; camerasStereo = cell(options.poseNum, 1);
camerasStereo = cell(poseNum, 1); % for i = 1:options.poseNum
trans = Point3(); % cylinderIdx = max(min(round(cylinderNum*rand), 10), 1);
for i = 1:poseNum % camerasStereo{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ...
cylinderIdx = max(min(round(cylinderNum*rand), 10), 1); % Point3([0,0,1]'), KStereo);
camerasStereo{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ... %
Point3([0,0,1]'), KStereo); % incT = Point3(5*rand, 5*rand, 5*rand);
% trans = trans.compose(incT);
incT = Point3(5*rand, 5*rand, 5*rand); % end
trans = trans.compose(incT);
end
pts2dTracksStereo = points2DTrackStereo(camerasStereo, imageSize, cylinders); %pts2dTracksStereo = points2DTrackStereo(camerasStereo, imageSize, cylinders);
% plot the 2D tracks % plot the 2D tracks