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

remove the redudant empty cells

release/4.3a0
lvzhaoyang 2015-01-14 00:08:35 -05:00
parent ea556c71d7
commit 2378d59632
5 changed files with 37 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
matlab/+gtsam/cylinderSampleProjection.m
View File

@ -17,18 +17,10 @@ camera = SimpleCamera(pose, K);
%% memory allocation
cylinderNum = length(cylinders);
visiblePoints.index = cell(cylinderNum,1);
pointCloudNum = 0;
for i = 1:cylinderNum
pointCloudNum = pointCloudNum + length(cylinders{i}.Points);
visiblePoints.index{i} = cell(pointCloudNum,1);
end
visiblePoints.data = cell(pointCloudNum,1);
visiblePoints.Z = cell(pointCloudNum, 1);
%% check visiblity of points on each cylinder
pointCloudIndex = 0;
visiblePointIdx = 1;
for i = 1:cylinderNum
pointNum = length(cylinders{i}.Points);
@ -70,8 +62,8 @@ for i = 1:cylinderNum
projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
if projectedRay > 0
%rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
if rayCylinderToPoint(1) > cylinders{i}.radius && ...
rayCylinderToPoint(2) > cylinders{i}.radius
if rayCylinderToPoint(1) > cylinders{k}.radius && ...
rayCylinderToPoint(2) > cylinders{k}.radius
continue;
else
visible = false;
@ -83,10 +75,13 @@ for i = 1:cylinderNum
end
if visible
visiblePoints.data{pointCloudIndex} = sampledPoint3;
visiblePoints.Z{pointCloudIndex} = Z2d;
visiblePoints.index{i}{j} = pointCloudIndex;
visiblePoints.data{visiblePointIdx} = sampledPoint3;
visiblePoints.Z{visiblePointIdx} = Z2d;
visiblePoints.cylinderIdx{visiblePointIdx} = i;
visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex;
visiblePointIdx = visiblePointIdx + 1;
end
end
end

22
matlab/+gtsam/cylinderSampleProjectionStereo.m
View File

@ -4,18 +4,10 @@ import gtsam.*
%% memory allocation
cylinderNum = length(cylinders);
visiblePoints.index = cell(cylinderNum,1);
pointCloudNum = 0;
for i = 1:cylinderNum
pointCloudNum = pointCloudNum + length(cylinders{i}.Points);
visiblePoints.index{i} = cell(pointCloudNum,1);
end
visiblePoints.data = cell(pointCloudNum,1);
visiblePoints.Z = cell(pointCloudNum, 1);
%% check visiblity of points on each cylinder
pointCloudIndex = 0;
visiblePointIdx = 1;
for i = 1:cylinderNum
pointNum = length(cylinders{i}.Points);
@ -64,8 +56,8 @@ for i = 1:cylinderNum
projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
if projectedRay > 0
%rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
if rayCylinderToPoint(1) > cylinders{i}.radius && ...
rayCylinderToPoint(2) > cylinders{i}.radius
if rayCylinderToPoint(1) > cylinders{k}.radius && ...
rayCylinderToPoint(2) > cylinders{k}.radius
continue;
else
visible = false;
@ -77,9 +69,11 @@ for i = 1:cylinderNum
end
if visible
visiblePoints.data{pointCloudIndex} = sampledPoint3;
visiblePoints.Z{pointCloudIndex} = Z;
visiblePoints.index{i}{j} = pointCloudIndex;
visiblePoints.data{visiblePointIdx} = sampledPoint3;
visiblePoints.Z{visiblePointIdx} = Z;
visiblePoints.cylinderIdx{visiblePointIdx} = i;
visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex;
visiblePointIdx = visiblePointIdx + 1;
end
end

31
matlab/+gtsam/points2DTrackMonocular.m
View File

@ -29,9 +29,8 @@ pts3d = cell(cameraPosesNum, 1);
initialEstimate = Values;
initialized = false;
for i = 1:cameraPosesNum
% add a constraint on the starting pose
cameraPose = cameraPoses{i};
cameraPose = cameraPoses{i};
pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders);
if ~initialized
@ -39,12 +38,10 @@ for i = 1:cameraPosesNum
initialized = true;
end
for j = 1:length(pts3d{i}.Z)
if isempty(pts3d{i}.Z{j})
continue;
end
measurementNum = length(pts3d{i}.Z);
for j = 1:measurementNum
graph.add(GenericProjectionFactorCal3_S2(pts3d{i}.Z{j}, ...
measurementNoise, symbol('x', i), symbol('p', j), K) );
measurementNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K) );
end
end
@ -70,23 +67,13 @@ marginals = Marginals(graph, initialEstimate);
%% get all the points track information
% currently throws the Indeterminant linear system exception
ptx = 1;
for k = 1:cameraPosesNum
for i = 1:length(cylinders)
for j = 1:length(cylinders{i}.Points)
if isempty(pts3d{k}.index{i}{j})
continue;
num = length(pts3d{k}.data);
for i = 1:num
pts2dTracksMono.pt3d{i} = pts3d{k}.data{i};
pts2dTracksMono.Z{i} = pts3d{k}.Z{i};
pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx}));
end
idx = pts3d{k}.index{i}{j};
pts2dTracksMono.pt3d{ptx} = pts3d{k}.data{idx};
pts2dTracksMono.Z{ptx} = pts3d{k}.Z{idx};
pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx));
ptx = ptx + 1;
end
end
end
%% plot the result with covariance ellipses

21
matlab/+gtsam/points2DTrackStereo.m
View File

@ -34,12 +34,10 @@ for i = 1:cameraPosesNum
initialized = true;
end
for j = 1:length(pts3d{i}.Z)
if isempty(pts3d{i}.Z{j})
continue;
end
measurementNum = length(pts3d{i}.Z);
for j = 1:measurementNum
graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ...
stereoNoise, symbol('x', i), symbol('p', j), K));
stereoNoise, symbol('x', i), symbol('p', pts3d{i}.overallIdx{j}), K));
end
end
@ -64,21 +62,10 @@ marginals = Marginals(graph, initialEstimate);
%% get all the 2d points track information
% currently throws the Indeterminant linear system exception
ptx = 1;
for k = 1:cameraPosesNum
for i = 1:length(cylinders)
for j = 1:length(cylinders{i}.Points)
if isempty(pts3d{k}.index{i}{j})
continue;
end
idx = pts3d{k}.index{i}{j};
pts2dTracksStereo.pt3d{ptx} = pts3d{k}.data{idx};
pts2dTracksStereo.Z{ptx} = pts3d{k}.Z{idx};
pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',idx));
ptx = ptx + 1;
end
end
pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx}));
end
end

11
matlab/gtsam_examples/CameraFlyingExample.m
View File

@ -29,7 +29,7 @@ options.stereoK = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
% the image size of camera
options.imageSize = Point2([640, 480]');
% use Monocular camera or Stereo camera
options.Mono = false;
options.Mono = true;
%% test1: visibility test in monocular camera
cylinders{1}.centroid = Point3(30, 50, 5);
@ -54,9 +54,6 @@ prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize,
%% test2: visibility test in stereo camera
prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders);
%% generate a set of cylinders and Samples
cylinderNum = options.cylinderNum;
cylinders = cell(cylinderNum, 1);
@ -93,9 +90,6 @@ for i = 1:options.poseNum
cameraPoses{i} = camera.pose;
end
%% plot all the projected points
%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID);
%% set up camera and get measurements
if options.Mono
% use Monocular Camera
@ -107,6 +101,9 @@ else
options.imageSize, cylinders);
end
%% plot all the projected points
%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID);
% plot the 2D tracks
% ToDo: plot the trajectories