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

remove dulipcate points in stereo camera set up

release/4.3a0
lvzhaoyang 2015-01-15 01:19:21 -05:00
parent 27b3b5ebed
commit 26df490c55
5 changed files with 95 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
matlab/+gtsam/plotCylinderSamples.m
View File

@ -1,4 +1,4 @@
function plotCylinderSamples(cylinders, fieldSize, figID)
function plotCylinderSamples(cylinders, options, figID)
% plot the cylinders on the given field
% @author: Zhaoyang Lv
@ -24,7 +24,7 @@ function plotCylinderSamples(cylinders, fieldSize, figID)
end
axis equal
axis([0, fieldSize.x, 0, fieldSize.y, 0, 20]);
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]);
grid on

7
matlab/+gtsam/plotProjectedCylinderSamples.m
View File

@ -1,5 +1,5 @@
function plotProjectedCylinderSamples(pts3d, covariance, figID)
% plot the visible projected points on the cylinders
function plotProjectedCylinderSamples(pts3d, covariance, options, figID)
% plot the visible points on the cylinders
% author: Zhaoyang Lv
import gtsam.*
@ -11,7 +11,8 @@ function plotProjectedCylinderSamples(pts3d, covariance, figID)
pointsNum = length(pts3d);
for i = 1:pointsNum
plotPoint3(pts3d{i}, 'green', covariance{i});
%gtsam.plotPoint3(p, 'g', covariance{i});
plotPoint3(pts3d{i}, 'r', covariance{i});
hold on
end

50
matlab/+gtsam/points2DTrackMonocular.m
View File

@ -59,6 +59,13 @@ for i = 1:cameraPosesNum
end
for i = 1:pointsNum
% add in values
point_j = points3d{i}.data.retract(0.1*randn(3,1));
initialEstimate.insert(symbol('p', i), point_j);
if ~points3d{i}.visiblity
continue;
end
% single measurement. not added to graph
factorNum = length(points3d{i}.Z);
if factorNum > 1
@ -68,41 +75,40 @@ for i = 1:pointsNum
measurementNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K) );
end
end
% add in values
point_j = points3d{i}.data.retract(0.1*randn(3,1));
initialEstimate.insert(symbol('p', i), point_j);
end
%% Print the graph
graph.print(sprintf('\nFactor graph:\n'));
%% linearize the graph
% currently throws the Indeterminant linear system exception
marginals = Marginals(graph, initialEstimate);
%% get all the points track information
% currently throws the Indeterminant linear system exception
for i = 1:pointsNum
if points3d{i}.visiblity
pts2dTracksMono.pt3d{i} = points3d{i}.data;
pts2dTracksMono.Z = points3d{i}.Z;
if ~points3d{i}.visiblity
continue;
end
pts2dTracksMono.pt3d{end+1} = points3d{i}.data;
pts2dTracksMono.Z{end+1} = points3d{i}.Z;
if length(points3d{i}.Z) == 1
%pts2dTracksMono.cov{i} singular matrix
else
pts2dTracksMono.cov{i} = marginals.marginalCovariance(symbol('p', i));
end
if length(points3d{i}.Z) == 1
%pts2dTracksMono.cov{i} singular matrix
else
pts2dTracksMono.cov{end+1} = marginals.marginalCovariance(symbol('p', i));
end
end
for k = 1:cameraPosesNum
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
end
% for k = 1:cameraPosesNum
% 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
% end
%% plot the result with covariance ellipses
hold on;

82
matlab/+gtsam/points2DTrackStereo.m
View File

@ -19,64 +19,100 @@ cameraPosesNum = length(cameraPoses);
%% add measurements and initial camera & points values
pointsNum = 0;
cylinderNum = length(cylinders);
points3d = cell(0);
for i = 1:cylinderNum
cylinderPointsNum = length(cylinders{i}.Points);
pointsNum = pointsNum + length(cylinders{i}.Points);
for j = 1:cylinderPointsNum
points3d{end+1}.data = cylinders{i}.Points{j};
points3d{end}.Z = cell(0);
points3d{end}.cameraConstraint = cell(0);
points3d{end}.visiblity = false;
end
end
graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
pts3d = cell(cameraPosesNum, 1);
initialEstimate = Values;
initialized = false;
for i = 1:cameraPosesNum
pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, imageSize, cylinders);
if ~initialized
graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{i}, posePriorNoise));
initialized = true;
if isempty(pts3d{i}.Z)
continue;
end
measurementNum = length(pts3d{i}.Z);
if measurementNum < 1
continue;
end
for j = 1:measurementNum
index = pts3d{i}.overallIdx{j};
points3d{index}.Z{end+1} = pts3d{i}.Z{j};
points3d{index}.cameraConstraint{end+1} = i;
points3d{index}.visiblity = true;
end
for j = 1:length(pts3d{i}.Z)
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', pts3d{i}.overallIdx{j}), K));
end
end
%% initialize cameras and points close to ground truth
%% initialize graph and values
for i = 1:cameraPosesNum
pose_i = cameraPoses{i}.retract(0.1*randn(6,1));
initialEstimate.insert(symbol('x', i), pose_i);
end
ptsIdx = 0;
for i = 1:length(cylinders)
for j = 1:length(cylinders{i}.Points)
ptsIdx = ptsIdx + 1;
point_j = cylinders{i}.Points{j}.retract(0.1*randn(3,1));
initialEstimate.insert(symbol('p', ptsIdx), point_j);
for i = 1:pointsNum
point_j = points3d{i}.data.retract(0.1*randn(3,1));
initialEstimate.insert(symbol('p', i), point_j);
if ~points3d{i}.visiblity
continue;
end
factorNum = length(points3d{i}.Z);
for j = 1:factorNum
cameraIdx = points3d{i}.cameraConstraint{j};
graph.add(GenericStereoFactor3D(StereoPoint2(points3d{i}.Z{j}.uL, points3d{i}.Z{j}.uR, points3d{i}.Z{j}.v), ...
stereoNoise, symbol('x', cameraIdx), symbol('p', points3d{i}.cameraConstraint{j}), K));
end
end
%% Print the graph
graph.print(sprintf('\nFactor graph:\n'));
%% linearize the graph
marginals = Marginals(graph, initialEstimate);
%% get all the 2d points track information
% currently throws the Indeterminant linear system exception
for k = 1:cameraPosesNum
num = length(pts3d{k}.data);
if num < 1
pts2dTracksStereo.pt3d = cell(0);
pts2dTracksStereo.Z = cell(0);
pts2dTracksStereo.cov = cell(0);
for i = 1:pointsNum
if ~points3d{i}.visiblity
continue;
end
for i = 1:num
pts2dTracksStereo.pt3d{i} = pts3d{k}.data{i};
pts2dTracksStereo.Z{i} = pts3d{k}.Z{i};
pts2dTracksStereo.cov{i} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{i}));
end
pts2dTracksStereo.pt3d{end+1} = points3d{i}.data;
pts2dTracksStereo.Z{end+1} = points3d{i}.Z;
pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i));
end
% for k = 1:cameraPosesNum
% if isempty(pts3d{k}.data)
% continue;
% end
%
% for i = 1:length(pts3d{k}.data)
% pts2dTracksStereo.pt3d{end+1} = pts3d{k}.data{i};
% pts2dTracksStereo.Z{end+1} = pts3d{k}.Z{i};
% pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{i}));
% end
% end
%% plot the result with covariance ellipses
hold on;
%plot3DPoints(initialEstimate, [], marginals);

4
matlab/gtsam_examples/CameraFlyingExample.m
View File

@ -87,7 +87,7 @@ end
% now is plotting on a 100 * 100 field
figID = 1;
figure(figID);
plotCylinderSamples(cylinders, options.fieldSize, figID);
plotCylinderSamples(cylinders, options, figID);
% %% generate ground truth camera trajectories: a circle
% KMono = Cal3_S2(525,525,0,320,240);
@ -129,7 +129,7 @@ else
options.imageSize, cylinders);
figID = 2;
plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, figID);
plotProjectedCylinderSamples(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, options, figID);
end