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

22
matlab/+gtsam/cylinderSampleProjectionStereo.m
View File

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

33
matlab/+gtsam/points2DTrackMonocular.m
View File

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

25
matlab/+gtsam/points2DTrackStereo.m
View File

@ -34,12 +34,10 @@ for i = 1:cameraPosesNum
initialized = true; initialized = true;
end end
for j = 1:length(pts3d{i}.Z) measurementNum = length(pts3d{i}.Z);
if isempty(pts3d{i}.Z{j}) for j = 1:measurementNum
continue;
end
graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ... 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
end end
@ -64,21 +62,10 @@ marginals = Marginals(graph, initialEstimate);
%% get all the 2d points track information %% get all the 2d points track information
% currently throws the Indeterminant linear system exception % currently throws the Indeterminant linear system exception
ptx = 1;
for k = 1:cameraPosesNum for k = 1:cameraPosesNum
for i = 1:length(cylinders) pts2dTracksStereo.pt3d{ptx} = pts3d{k}.data{idx};
for j = 1:length(cylinders{i}.Points) pts2dTracksStereo.Z{ptx} = pts3d{k}.Z{idx};
if isempty(pts3d{k}.index{i}{j}) pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx}));
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
end end
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 % the image size of camera
options.imageSize = Point2([640, 480]'); options.imageSize = Point2([640, 480]');
% use Monocular camera or Stereo camera % use Monocular camera or Stereo camera
options.Mono = false; options.Mono = true;
%% test1: visibility test in monocular camera %% test1: visibility test in monocular camera
cylinders{1}.centroid = Point3(30, 50, 5); cylinders{1}.centroid = Point3(30, 50, 5);
@ -54,9 +54,6 @@ prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize,
%% test2: visibility test in stereo camera %% test2: visibility test in stereo camera
prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders); prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders);
%% generate a set of cylinders and Samples %% generate a set of cylinders and Samples
cylinderNum = options.cylinderNum; cylinderNum = options.cylinderNum;
cylinders = cell(cylinderNum, 1); cylinders = cell(cylinderNum, 1);
@ -93,9 +90,6 @@ for i = 1:options.poseNum
cameraPoses{i} = camera.pose; cameraPoses{i} = camera.pose;
end end
%% plot all the projected points
%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID);
%% set up camera and get measurements %% set up camera and get measurements
if options.Mono if options.Mono
% use Monocular Camera % use Monocular Camera
@ -107,6 +101,9 @@ else
options.imageSize, cylinders); options.imageSize, cylinders);
end end
%% plot all the projected points
%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID);
% plot the 2D tracks % plot the 2D tracks
% ToDo: plot the trajectories % ToDo: plot the trajectories