57 lines
1.3 KiB
Matlab
57 lines
1.3 KiB
Matlab
clear all;
|
|
clc;
|
|
import gtsam.*
|
|
|
|
cylinder_num = 10;
|
|
cylinders = cell(cylinder_num, 1);
|
|
|
|
% generate a set of cylinders
|
|
fieldSize = Point2([100, 100]');
|
|
|
|
% random generate cylinders on the fields
|
|
for i = 1:cylinder_num
|
|
baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]');
|
|
cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1);
|
|
end
|
|
|
|
% plot all the cylinders and sampled points
|
|
% now is plotting on a 100 * 100 field
|
|
figID = 1;
|
|
figure(figID);
|
|
plotCylinderSamples(cylinders, fieldSize, figID);
|
|
|
|
% visibility validation
|
|
|
|
% generate camera trajectories
|
|
K = Cal3_S2(525,525,0,320,240);
|
|
imageSize = Point2([640, 480]');
|
|
poseNum = 10;
|
|
cameraPoses = cell(poseNum, 1);
|
|
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();
|
|
|
|
|
|
|
|
|
|
|
|
|