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

change monocular set up and add stereo test. Still under test

release/4.3a0
lvzhaoyang 2015-01-12 23:27:50 -05:00
parent 678aabce3e
commit 10dc767eda
1 changed files with 27 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
matlab/gtsam_examples/CameraFlyingExample.m
View File

@ -1,12 +1,15 @@
clear all; clear all;
clc; clc;
clf;
import gtsam.* import gtsam.*
%% generate a set of cylinders and Samples %% generate a set of cylinders and Samples
fieldSize = Point2([100, 100]'); fieldSize = Point2([100, 100]');
cylinder_num = 10; cylinderNum = 10;
cylinders = cell(cylinder_num, 1); cylinders = cell(cylinderNum, 1);
% ToDo: it seems random generated cylinders doesn't work that well
% use fixed parameters instead
for i = 1:cylinderNum for i = 1:cylinderNum
baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]'); baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]');
cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1);
@ -18,7 +21,6 @@ figID = 1;
figure(figID); figure(figID);
plotCylinderSamples(cylinders, fieldSize, figID); plotCylinderSamples(cylinders, 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]');
@ -28,7 +30,8 @@ trans = Point3();
% To ensure there are landmarks in view, look at one randomly chosen cylinder % To ensure there are landmarks in view, look at one randomly chosen cylinder
% each time. % each time.
for i = 1:poseNum for i = 1:poseNum
camera = SimpleCamera.Lookat(trans, cylinders{round(cylinderNum*rand)}.centroid, ... cylinderIdx = max(min(round(cylinderNum*rand), 10), 1);
cameras{i} = SimpleCamera.Lookat(trans, cylinders{cylinderIdx}.centroid, ...
Point3([0,0,1]'), K); Point3([0,0,1]'), K);
incT = Point3(5*rand, 5*rand, 5*rand); incT = Point3(5*rand, 5*rand, 5*rand);
@ -36,17 +39,34 @@ for i = 1:poseNum
end end
%% visibility validation %% visibility validation
visiblePoints3 = cylinderSampleProjection(camera, imageSize, cylinders); % for a simple test, it will be removed later
visiblePoints3 = cylinderSampleProjection(cameras{1}, imageSize, cylinders);
%% plot all the projected points %% plot all the projected points
%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(K, cameraPoses, imageSize, cylinders); %pts2dTracksMono = points2DTrackMonocular(cameras, imageSize, cylinders);
%% set up stereo camera and get measurements %% set up stereo camera and get measurements
%pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders); % load stereo calibration
calib = dlmread(findExampleDataFile('VO_calibration.txt'));
KStereo = Cal3_S2Stereo(calib(1), calib(2), calib(3), calib(4), calib(5), calib(6));
poseNum = 10;
camerasStereo = cell(poseNum, 1);
trans = Point3();
for i = 1:poseNum
cylinderIdx = max(min(round(cylinderNum*rand), 10), 1);
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);
end
pts2dTracksStereo = points2DTrackStereo(camerasStereo, imageSize, cylinders);
% plot the 2D tracks
% ToDo: plot the trajectories % ToDo: plot the trajectories
%plot3DTrajectory(); %plot3DTrajectory();