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

update the stereo model and occlusion detection

release/4.3a0
lvzhaoyang 2015-01-13 21:21:48 -05:00
parent d62cb440db
commit da06689677
2 changed files with 105 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
matlab/+gtsam/cylinderSampleProjectionStereo.m Normal file
View File

@ -0,0 +1,84 @@
function [visiblePoints] = cylinderSampleProjectionStereo(K, pose, imageSize, cylinders)
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;
for i = 1:cylinderNum
pointNum = length(cylinders{i}.Points);
% to check point visibility
for j = 1:pointNum
pointCloudIndex = pointCloudIndex + 1;
% For Cheirality Exception
sampledPoint3 = cylinders{i}.Points{j};
sampledPoint3local = pose.transform_to(sampledPoint3);
if sampledPoint3local.z < 0
continue;
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();
% ignore points not visible in the scene
if Z.uL < 0 || Z.uL >= imageSize.x || ...
Z.uR < 0 || Z.uR >= imageSize.x || ...
Z.v < 0 || Z.v >= imageSize.y
continue;
end
% ignore points occluded
% 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
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();
% 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;
end
end
end
end
end
end

46
matlab/+gtsam/points2DTrackStereo.m
View File

@ -1,4 +1,4 @@
function pts2dTracksStereo = points2DTrackStereo(cameras, imageSize, cylinders)
function pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders)
% Assess how accurately we can reconstruct points from a particular monocular camera setup.
% After creation of the factor graph for each track, linearize it around ground truth.
% There is no optimization
@ -16,8 +16,9 @@ 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(cameras);
cameraPosesNum = length(cameraPoses);
%% add measurements and initial camera & points values
pointsNum = 0;
@ -26,25 +27,14 @@ for i = 1:cylinderNum
pointsNum = pointsNum + length(cylinders{i}.Points);
end
pts3d = {};
pts3d = cell(cameraPosesNum, 1);
initialEstimate = Values;
initialized = false;
for i = 1:cameraPosesNum
% add a constraint on the starting pose
camera = cameras{i};
for i = 1:cameraPosesNum
pts3d{i} = cylinderSampleProjectionStereo(K, cameraPose, imageSize, cylinders);
pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders);
pts3d.camera{i} = camera;
if ~initialized
graph.add(PriorFactorPose3(symbol('x', 1), camera.pose, posePriorNoise));
k = 0;
if ~isempty(pts3d.pts{i}.data{1+k})
graph.add(PriorFactorPoint3(symbol('p', 1), ...
pts3d.pts{i}.data{1+k}, pointPriorNoise));
else
k = k+1;
end
initialized = true;
end
@ -52,10 +42,9 @@ for i = 1:cameraPosesNum
if isempty(pts3d.pts{i}.Z{j})
continue;
end
graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ...
measurementNoise, symbol('x', i), symbol('p', j), camera.calibration) );
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));
end
end
%% initialize cameras and points close to ground truth
@ -79,12 +68,19 @@ marginals = Marginals(graph, initialEstimate);
%% get all the 2d points track information
% currently throws the Indeterminant linear system exception
ptIdx = 0;
for i = 1:pointsNum
if isempty(pts3d.pts{i})
continue;
end
pts2dTracksMono.cov{ptIdx} = marginals.marginalCovariance(symbol('p',i));
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));
ptx = ptx + 1;
end
end
end