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

an update of function 1&2 cameraFlyingExample. function3&4 in construction

release/4.3a0
lvzhaoyang 2015-01-11 23:22:42 -05:00
parent 377c462818
commit b45e81725b
1 changed files with 28 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
matlab/gtsam_examples/CameraFlyingExample.m
View File

@ -2,7 +2,7 @@ clear all;
clc; clc;
import gtsam.* import gtsam.*
cylinder_num = 20; cylinder_num = 10;
cylinders = cell(cylinder_num, 1); cylinders = cell(cylinder_num, 1);
% generate a set of cylinders % generate a set of cylinders
@ -11,21 +11,43 @@ fieldSize = Point2([100, 100]');
% random generate cylinders on the fields % random generate cylinders on the fields
for i = 1:cylinder_num for i = 1:cylinder_num
baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]'); baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]');
cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 30); cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1);
end end
% plot all the cylinders and sampled points % plot all the cylinders and sampled points
% now is plotting on a 100 * 100 field % now is plotting on a 100 * 100 field
plotCylinderSamples(cylinders, fieldSize); figID = 1;
figure(figID);
plotCylinderSamples(cylinders, fieldSize, figID);
% visibility validation % visibility validation
% generate camera trajectories
K = Cal3_S2(525,525,0,320,240); K = Cal3_S2(525,525,0,320,240);
cameraPose = Pose3(); % now set to be default imageSize = Point2([640, 480]');
poseNum = 10;
% the projections of all visible 3D points cameraPoses = cell(poseNum, 1);
% visiblePoints3 = cylinderSampleProjection(K, cameraPose, cylinders); cameraPoses{1} = Pose3();
cameras = cell(poseNum, 1);
for i = 2:poseNum
incRot = Rot3.RzRyRx(0,0,pi/4);
incT = Point3(5*rand, 5*rand, 5*rand);
cameraPoses{i} = cameraPoses{i-1}.compose(Pose3(incRot, incT));
end
[visiblePoints3, ~] = cylinderSampleProjection(K, cameraPoses{1}, imageSize, cylinders);
plotPose3(cameraPoses{1}, 5 )
% plot all the projected points
plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID);
pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders);
%pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders);
% ToDo: plot the trajectories
%plot3DTrajectory();