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

Stereo camera visibility tests passed

release/4.3a0
lvzhaoyang 2015-01-13 23:36:19 -05:00
parent 4a5d94ea59
commit ea556c71d7
3 changed files with 52 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
matlab/+gtsam/cylinderSampleProjectionStereo.m
View File

@ -33,10 +33,10 @@ for i = 1:cylinderNum
end
% measurements
Z.du = K.fx() * K.baseline() / samplePoint3.z;
Z.uL = K.fx() * samplePoint3.x / samplePoint3.z + K.px();
Z.uR = uL + du;
Z.v = K.fy() / samplePoint3.z + K.py();
Z.du = K.fx() * K.baseline() / sampledPoint3local.z;
Z.uL = K.fx() * sampledPoint3local.x / sampledPoint3local.z + K.px();
Z.uR = Z.uL + Z.du;
Z.v = K.fy() / sampledPoint3local.z + K.py();
% ignore points not visible in the scene
if Z.uL < 0 || Z.uL >= imageSize.x || ...
@ -49,34 +49,39 @@ for i = 1:cylinderNum
% use a simple math hack to check occlusion:
% 1. All points in front of cylinders' surfaces are visible
% 2. For points behind the cylinders' surfaces, the cylinder
visible = true;
for k = 1:cylinderNum
rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
rayCameraToCylinder = pose.translation().between(cylinders{i}.centroid).vector();
rayCylinderToPoint = cylinders{i}.centroid.between(sampledPoint3).vector();
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
% Condition 1: all points in front of the cylinders'
% surfaces are visible
if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
visiblePoints.data{pointCloudIndex} = sampledPoint3;
visiblePoints.Z{pointCloudIndex} = Z;
visiblePoints.index{i}{j} = pointCloudIndex;
continue;
end
% Condition 2
projectedRay = dot(rayCameraToCylinder, rayCameraToPoint);
if projectedRay > 0
rayCylinderToProjected = norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
if rayCylinderToProjected(1) > cylinders{i}.radius && ...
rayCylinderToProjected(2) > cylinders{i}.radius
visiblePoints.data{pointCloudIndex} = sampledPoint3;
visiblePoints.Z{pointCloudIndex} = Z;
visiblePoints.index{i}{j} = pointCloudIndex;
continue;
else
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
continue;
else
visible = false;
break;
end
end
end
end
if visible
visiblePoints.data{pointCloudIndex} = sampledPoint3;
visiblePoints.Z{pointCloudIndex} = Z;
visiblePoints.index{i}{j} = pointCloudIndex;
end
end
end

36
matlab/+gtsam/points2DTrackStereo.m
View File

@ -10,12 +10,8 @@ import gtsam.*
graph = NonlinearFactorGraph;
%% create the noise factors
pointNoiseSigma = 0.1;
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
measurementNoiseSigma = 1.0;
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
pointPriorNoise = noiseModel.Isotropic.Sigma(3, pointNoiseSigma);
measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
stereoNoise = noiseModel.Isotropic.Sigma(3,1);
cameraPosesNum = length(cameraPoses);
@ -31,15 +27,15 @@ pts3d = cell(cameraPosesNum, 1);
initialEstimate = Values;
initialized = false;
for i = 1:cameraPosesNum
pts3d{i} = cylinderSampleProjectionStereo(K, cameraPose, imageSize, cylinders);
pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, imageSize, cylinders);
if ~initialized
graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise));
graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{i}, posePriorNoise));
initialized = true;
end
for j = 1:length(pts3d.pts{i}.Z)
if isempty(pts3d.pts{i}.Z{j})
for j = 1:length(pts3d{i}.Z)
if isempty(pts3d{i}.Z{j})
continue;
end
graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ...
@ -49,7 +45,7 @@ end
%% initialize cameras and points close to ground truth
for i = 1:cameraPosesNum
pose_i = camera.pose.retract(0.1*randn(6,1));
pose_i = cameraPoses{i}.retract(0.1*randn(6,1));
initialEstimate.insert(symbol('x', i), pose_i);
end
ptsIdx = 0;
@ -69,17 +65,19 @@ marginals = Marginals(graph, initialEstimate);
%% get all the 2d points track information
% currently throws the Indeterminant linear system exception
ptx = 1;
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};
pts2dTracksMono.pt3d{ptx} = pts3d{k}.data{idx};
pts2dTracksMono.Z{ptx} = pts3d{k}.Z{idx};
pts2dTracksMono.cov{ptx} = marginals.marginalCovariance(symbol('p',idx));
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;
ptx = ptx + 1;
end
end
end

14
matlab/gtsam_examples/CameraFlyingExample.m
View File

@ -31,7 +31,7 @@ options.imageSize = Point2([640, 480]');
% use Monocular camera or Stereo camera
options.Mono = false;
%% test1: visibility
%% test1: visibility test in monocular camera
cylinders{1}.centroid = Point3(30, 50, 5);
cylinders{2}.centroid = Point3(50, 50, 5);
cylinders{3}.centroid = Point3(70, 50, 5);
@ -44,16 +44,18 @@ for i = 1:3
cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
end
camera = SimpleCamera.Lookat(Point3(40, 50, 10), ...
camera = SimpleCamera.Lookat(Point3(10, 50, 10), ...
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
Point3([0,0,1]'), options.monoK);
pose = camera.pose;
pts3d = cylinderSampleProjection(options.monoK, pose, options.imageSize, cylinders);
prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize, cylinders);
%% test2: visibility test in stereo camera
prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders);
figID = 1;
figure(figID);
plotCylinderSamples(cylinders, options.fieldSize, figID);
%% generate a set of cylinders and Samples
cylinderNum = options.cylinderNum;