diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m index 890f174b7..0abd9cc3c 100644 --- a/matlab/+gtsam/cylinderSampleProjection.m +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -17,18 +17,10 @@ camera = SimpleCamera(pose, K); %% 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; +visiblePointIdx = 1; for i = 1:cylinderNum pointNum = length(cylinders{i}.Points); @@ -70,8 +62,8 @@ for i = 1:cylinderNum 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 + if rayCylinderToPoint(1) > cylinders{k}.radius && ... + rayCylinderToPoint(2) > cylinders{k}.radius continue; else visible = false; @@ -83,10 +75,13 @@ for i = 1:cylinderNum end if visible - visiblePoints.data{pointCloudIndex} = sampledPoint3; - visiblePoints.Z{pointCloudIndex} = Z2d; - visiblePoints.index{i}{j} = pointCloudIndex; + visiblePoints.data{visiblePointIdx} = sampledPoint3; + visiblePoints.Z{visiblePointIdx} = Z2d; + visiblePoints.cylinderIdx{visiblePointIdx} = i; + visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex; + visiblePointIdx = visiblePointIdx + 1; end + end end diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m index 99cb83f87..ae02c879c 100644 --- a/matlab/+gtsam/cylinderSampleProjectionStereo.m +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -4,18 +4,10 @@ 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; +visiblePointIdx = 1; for i = 1:cylinderNum pointNum = length(cylinders{i}.Points); @@ -64,8 +56,8 @@ for i = 1:cylinderNum 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 + if rayCylinderToPoint(1) > cylinders{k}.radius && ... + rayCylinderToPoint(2) > cylinders{k}.radius continue; else visible = false; @@ -77,9 +69,11 @@ for i = 1:cylinderNum end if visible - visiblePoints.data{pointCloudIndex} = sampledPoint3; - visiblePoints.Z{pointCloudIndex} = Z; - visiblePoints.index{i}{j} = pointCloudIndex; + visiblePoints.data{visiblePointIdx} = sampledPoint3; + visiblePoints.Z{visiblePointIdx} = Z; + visiblePoints.cylinderIdx{visiblePointIdx} = i; + visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex; + visiblePointIdx = visiblePointIdx + 1; end end diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m index 0f209bb43..7cdd99fd3 100644 --- a/matlab/+gtsam/points2DTrackMonocular.m +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -28,10 +28,9 @@ end pts3d = cell(cameraPosesNum, 1); initialEstimate = Values; initialized = false; -for i = 1:cameraPosesNum - % add a constraint on the starting pose - cameraPose = cameraPoses{i}; +for i = 1:cameraPosesNum + cameraPose = cameraPoses{i}; pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders); if ~initialized @@ -39,12 +38,10 @@ for i = 1:cameraPosesNum initialized = true; end - for j = 1:length(pts3d{i}.Z) - if isempty(pts3d{i}.Z{j}) - continue; - end + measurementNum = length(pts3d{i}.Z); + for j = 1:measurementNum 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 @@ -70,23 +67,13 @@ marginals = Marginals(graph, initialEstimate); %% get all the points track information % currently throws the Indeterminant linear system exception -ptx = 1; 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}; - 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 + 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 diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m index 14b3b5cd2..0a27cf90b 100644 --- a/matlab/+gtsam/points2DTrackStereo.m +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -34,12 +34,10 @@ for i = 1:cameraPosesNum initialized = true; end - for j = 1:length(pts3d{i}.Z) - if isempty(pts3d{i}.Z{j}) - continue; - end + measurementNum = length(pts3d{i}.Z); + for j = 1:measurementNum 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 @@ -64,21 +62,10 @@ marginals = Marginals(graph, initialEstimate); %% get all the 2d points track information % currently throws the Indeterminant linear system exception -ptx = 1; 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; - end - end + pts2dTracksStereo.pt3d{ptx} = pts3d{k}.data{idx}; + pts2dTracksStereo.Z{ptx} = pts3d{k}.Z{idx}; + pts2dTracksStereo.cov{ptx} = marginals.marginalCovariance(symbol('p',pts3d{k}.overallIdx{visiblePointIdx})); end end diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m index 3a66bb498..1e5981614 100644 --- a/matlab/gtsam_examples/CameraFlyingExample.m +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -29,7 +29,7 @@ options.stereoK = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); % the image size of camera options.imageSize = Point2([640, 480]'); % use Monocular camera or Stereo camera -options.Mono = false; +options.Mono = true; %% test1: visibility test in monocular camera cylinders{1}.centroid = Point3(30, 50, 5); @@ -54,9 +54,6 @@ prjMonoResult = cylinderSampleProjection(options.monoK, pose, options.imageSize, %% test2: visibility test in stereo camera prjStereoResult = cylinderSampleProjectionStereo(options.stereoK, pose, options.imageSize, cylinders); - - - %% generate a set of cylinders and Samples cylinderNum = options.cylinderNum; cylinders = cell(cylinderNum, 1); @@ -93,9 +90,6 @@ for i = 1:options.poseNum cameraPoses{i} = camera.pose; end -%% plot all the projected points -%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); - %% set up camera and get measurements if options.Mono % use Monocular Camera @@ -107,6 +101,9 @@ else options.imageSize, cylinders); end +%% plot all the projected points +%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID); + % plot the 2D tracks % ToDo: plot the trajectories