diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m index 917068da0..99cb83f87 100644 --- a/matlab/+gtsam/cylinderSampleProjectionStereo.m +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -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 diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index b3fea3d58..14b3b5cd2 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -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 diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 4b379d7a4..3a66bb498 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -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;