Compute First features, then matching

+logic/+reconstruct3D/denseMatching.m

@@ -0,0 +1,60 @@
+function pointCloudDense = denseMatching(pointCloudInstance, imagesOriginal, cameraParams)
+% TODO: add description
+
+% === 4. Dense reconstruction ===
+if log
+    fprintf('Dense reconstruction\n');
+end
+% Create a dense point cloud from the triangulated points and camera poses.
+numImagesDenseRec = 2;
+for i = 1:(numImagesDenseRec-1)
+    if log
+        fprintf('Dense reconstruction of image %d of %d \r', i, numImages-1);
+    end
+    image1 = imagesOriginal{i};
+    image2 = imagesOriginal{i+1};
+    % figure
+    % imshowpair(image1, image2, 'montage');
+
+    % Compute stereo parameters and rectify the stereo images.
+    relPose = rigidtform3d(poses(vSet, i).AbsolutePose.A \ poses(vSet, i+1).AbsolutePose.A);
+    stereoParams = stereoParameters(cameraParams, cameraParams, relPose);
+    %[image1Rect, image2Rect] = rectifyStereoImages(rgb2gray(image1), rgb2gray(image2), stereoParams);
+    [image1Rect, image2Rect, ~, camMatrix1] = rectifyStereoImages(image1, image2, stereoParams);
+    figure
+    imshowpair(image1Rect, image2Rect, 'montage');
+
+    % Compute disparity.
+    disparityRange = [0 128];
+    disparityMap = disparitySGM(rgb2gray(image1Rect), rgb2gray(image2Rect), DisparityRange=disparityRange);
+    figure
+    imshow(disparityMap, disparityRange);
+    colormap jet
+    colorbar
+
+
+    % Reconstruct the 3-D world coordinates of points corresponding to each pixel from the disparity map.
+    xyzPoints = reconstructScene(disparityMap, stereoParams);
+
+    % Filter points that are too far away and transform the MxNx3 matrix into a Nx3 matrix
+    roiBorder = 50;  % TODO: make this a parameter
+    roi = zeros(size(disparityMap));
+    roi(roiBorder:end-roiBorder, roiBorder:end-roiBorder) = 1;
+    xyzPoints = reshape(xyzPoints, [], 3);
+    roi = reshape(roi, [], 1);
+    % validIdx = xyzPoints(:, 3) < 1.5*maxZ & xyzPoints(:, 3) > 0.5*minZ & roi;
+    validIdx = xyzPoints(:, 3) < maxZ & roi;
+    xyzPoints = xyzPoints(validIdx, :);
+    xyzPointsInImage1 = (camMatrix1 * [xyzPoints, ones(size(xyzPoints, 1), 1)]')';
+    xyzPointsInImage1 = floor(xyzPointsInImage1(:, 2:-1:1) ./ xyzPointsInImage1(:, 3));
+    pixelColors = impixel(image1Rect, xyzPointsInImage1(: , 1), xyzPointsInImage1(: , 2));
+
+
+    % create a pointCloud object and assing the color of the original image at the corresponding pixel
+    size(xyzPoints)
+    pointCloudInstance = pccat([pointCloudInstance, pointCloud(xyzPoints, Color=pixelColors)]);
+end
+if log
+    fprintf('\nDense reconstruction finished after %.2f seconds.\n', toc);
+end
+end

+logic/+reconstruct3D/extractFeatures.m

@@ -0,0 +1,43 @@
+function [points, features] = extractFeatures(image, ~, varargin)
+% EXTRACTFEATURES Extracts features from the image
+%   [points, features] = EXTRACTFEATURES(image, prevFeatures, method, numOctaves, roiBorder)
+%   extracts features from the image using the specified method. 
+%   Inputs:
+%      image           - The image from which the features should be extracted
+%      prevFeatures    - The features from the previous image
+%      method = 'SURF' - The method used to extract the features. Can be 'SURF',
+%                       'FAST', 'Harris' or 'BRISK'
+%      numOctaves = 20 - The number of octaves used for SURF
+%      roiBorder = 20  - The border around the image that is not used for SURF
+%   Outputs:
+%      points          - The points of the features
+%      features        - The features
+
+p = inputParser;
+p.addOptional('method', 'SURF', @(x) any(validatestring(x, {'SURF', 'FAST', 'Harris', 'BRISK'})));
+p.addOptional('numOctaves', 20);
+p.addOptional('roiBorder', 20);
+p.parse(varargin{:});
+method = p.Results.method;
+numOctaves = p.Results.numOctaves;
+roiBorder = p.Results.roiBorder;
+
+% Detect features in the images 
+% https://de.mathworks.com/help/vision/ug/point-feature-types.html
+if strcmp(p.Results.method, 'SURF')
+    roi = [roiBorder, roiBorder, size(image, 2) - 2 * roiBorder, size(image, 1) - 2 * roiBorder];
+    points = detectSURFFeatures((image), NumOctaves=numOctaves, ROI=roi);
+    features = extractFeatures(image, points);
+elseif strcmp(p.Results.method, 'FAST')
+    points = detectFASTFeatures(image);
+    features = extractFeatures(image, points);
+elseif strcmp(p.Results.method, 'Harris')
+    points = detectHarrisFeatures(image);
+    features = extractFeatures(image, points);
+elseif strcmp(p.Results.method, 'BRISK')
+    points = detectBRISKFeatures(image);
+    features = extractFeatures(image, points);
+end
+
+
+end

+logic/reconstruct3DMultiview.m

@@ -25,9 +25,10 @@
 p = inputParser;
 p.addOptional('log', true);
 %% Preprocessing Params
-p.addOptional('scalingFactor', 0.5);
+p.addOptional('scalingFactor', 0.25);
 %% Reconstruction Params
 % Feature extraction parameters
+p.addOptional('featureExtractionMethod', 'SURF');
 p.addOptional('numOctaves', 20);
 p.addOptional('roiBorder', 20);
 % Epipolar geometry parameters
@@ -46,6 +47,7 @@
 p.parse(varargin{:});
 log = p.Results.log;
 scalingFactor = p.Results.scalingFactor;
+featureExtractionMethod = p.Results.featureExtractionMethod;
 numOctaves = p.Results.numOctaves;
 roiBorder = p.Results.roiBorder;
 eMaxDistance = p.Results.eMaxDistance;
@@ -60,7 +62,7 @@
 % =========================
 
 numImages = length(images);
-
+tic;
 if log
     fprintf('Starting preprocessing\n');
 end
@@ -80,80 +82,96 @@
 images = logic.presort(images, featureLength=presortFeatures, featureType=presort, normalize=presortNormalize, sortNearestNeighbors=presortNearestNeighbors);
 
 if log
-    fprintf('\nPreprocessing finished.\n');
+    fprintf('\nPreprocessing finished in %f seconds.\n', toc);
 end
 % =========================
 
-%% === 2. Feature Extraction First Image ===
-image1 = images{1};
+%% === 2. Extract Features ===
+% We first extract the features/points of all the images and save them on a cell
+% array. This is done to avoid recomputing the features when matching the
+% images.
+if log
+    fprintf('Extracting features\n');
+end
+features = cell(numImages, 1);
+points = cell(numImages, 1);
+for i = 1:numImages
+    if log
+        fprintf('Extracting features of image %d of %d\r', i, numImages);
+    end
+    currentImage = images{i};
+    [points{i}, features{i}] = logic.reconstruct3D.extractFeatures(currentImage, ...
+                                                                   method=featureExtractionMethod, ...
+                                                                   numOctaves=numOctaves, roiBorder=roiBorder);
+end
+if log
+    fprintf('\nFeature extraction finished in %f seconds.\n', toc);
+end
+
+% === 3. Feature Matching and triangulation ===
 % The object vSet contains the view set that stores the views (camera pose, feature vectors and points) and the connections
 % (relative poses and point matches) between the views.
 if log
-    tic;
-    fprintf("Generating view set and finding features of first picture\n");
+    fprintf("Generating view set\n");
 end
-[vSet, prevFeatures, prevPoints] = logic.reconstruct3D.createViewSet(image1, numOctaves=numOctaves, roiBorder=roiBorder);
+% Create an empty imageviewset object to manage the data associated with each view.
+% Add the first view. Place the camera associated with the first view and the origin, oriented along the Z-axis.
+vSet = imageviewset;
+vSet = addView(vSet, 1, rigidtform3d, Points=points{1});
 
-% === 3. Feature Extraction and matching remaining Images ===
 for i = 2:numImages
     if log
-        fprintf('Matching points of image %d of %d\r', i, numImages);
+        fprintf('Matching points of image %d of %d and triangulation with previous images. \r', i, numImages);
     end
-    % Load the current image and extract common features to all previous views.
-    currentImage = images{i};
-
-    [matchedPoints1, matchedPoints2, currPoints, currFeatures, indexPairs] = logic.reconstruct3D.extractCommonFeaturesMultiView(currentImage, prevFeatures, prevPoints, ...
-                                                                                                                                numOctaves=numOctaves, roiBorder=roiBorder);
+    % Match the features between the previous and the current image.
+    indexPairs   = matchFeatures(features{i-1}, features{i}, 'Unique', true);
+    matchedPointsPrev = points{i-1}(indexPairs(:, 1));
+    matchedPointsCurr = points{i}(indexPairs(:, 2));
 
     % Estimate the camera pose of current view relative to the previous view.
     % The pose is computed up to scale, meaning that the distance between
     % the cameras in the previous view and the current view is set to 1.
     % This will be corrected by the bundle adjustment.
-    [E, relPose, status, inlierIdx] = logic.reconstruct3D.getEpipolarGeometry(matchedPoints1, matchedPoints2, cameraParams, ...
+    [E, relPose, status, inlierIdx] = logic.reconstruct3D.getEpipolarGeometry(matchedPointsPrev, matchedPointsCurr, cameraParams, ...
                                                                               eMaxDistance=eMaxDistance, eConfidence=eConfidence, eMaxNumTrials=eMaxNumTrials, eValidPointFraction=eValidPointFraction);
 
-    % Get the table containing the previous camera pose.
+
+                                                                              % Get the table containing the previous camera pose.
     prevPose = poses(vSet, i-1).AbsolutePose;
-
     % Compute the current camera pose in the global coordinate system
     % relative to the first view.
-    if length(relPose) > 1  % TODO: BUG sometimes two poses are returned. This is a workaround
+    if length(relPose) > 1  % TODO: BUG sometimes two poses are returned. This is a workaround.
         size(relPose)
         relPose = relPose(1);
     end
     currPose = rigidtform3d(prevPose.A * relPose.A);
 
     % Add the current view to the view set.
-    vSet = addView(vSet, i, currPose, Points=currPoints);
-
+    vSet = addView(vSet, i, currPose, Points=points{i});
     % Store the point matches between the previous and the current views.
     vSet = addConnection(vSet, i-1, i, relPose, Matches=indexPairs(inlierIdx, :));
-
-    % Find point tracks across all views.
-    tracks = findTracks(vSet);
-
+    tracks = findTracks(vSet);  % Find point tracks across all views.
     % Get the table containing camera poses for all views.
     camPoses = poses(vSet);
 
+    % Triangulate initial locations for the 3-D world points and do bundle adjustment.
     [pointCloudInstance, camPoses] = logic.reconstruct3D.getTriangulatedPointsMultiView(tracks, camPoses, cameraParams, ...
                                                                                         maxReprojectionError=maxReprojectionError);
 
     % Store the refined camera poses.
     vSet = updateView(vSet, camPoses);
+end
+% We will use this to filter out points that are too far away.
+% after doing dense reconstruction.
 
-    prevFeatures = currFeatures;
-    prevPoints = currPoints;
 
+if log
+    fprintf('\n3D reconstruction finished after %.2f seconds.\n', toc);
 end
 
 % Rotate the point cloud
 R = [1 0 0; 0 0 1; 0 -1 0];
 tform = affinetform3d([R, zeros(3, 1); zeros(1, 3), 1]);
 [pointCloudInstance, camPoses] = logic.reconstruct3D.transformScene(pointCloudInstance, camPoses, tform);
 
-% TODO: Add an extra step to generate more features once we get the full 3D reconstruction
-
-if log
-    fprintf('\n3D reconstruction finished after %.2f seconds.\n', toc);
-end
 end

+plotting/plotPointCloud.m

@@ -14,16 +14,18 @@ function plotPointCloud(pointCloudInstance, camPoses, varargin)
 p.addOptional('usePcViewer', false, @islogical);
 p.addOptional('cameraSizePlotSize', 0.2, @isnumeric);
 p.addOptional('pcMarkerSize', 45, @isnumeric);
+p.addOptional('pcViewerMarkerSize', 5, @isnumeric);
 p.parse(varargin{:});
 usePcViewer = p.Results.usePcViewer;
 cameraSizePlotSize = p.Results.cameraSizePlotSize;
 pcMarkerSize = p.Results.pcMarkerSize;
+pcViewerMarkerSize = p.Results.pcViewerMarkerSize;
 
-hold on;
 
 if usePcViewer
-    pcviewer(pointCloudInstance, PointSize=1);
+    pcviewer(pointCloudInstance, PointSize=pcViewerMarkerSize);
 else
+    hold on;
     pcshow(pointCloudInstance, VerticalAxisDir='Down', MarkerSize=pcMarkerSize);
     % Show cameras poses
     if ~isempty(camPoses)

test_multiview.m

@@ -5,7 +5,7 @@
 dataDir = "test/delivery_area_dslr_undistorted";
 
 if exist('images','var') == 0  % Load the images if they are not already loaded yet
-    images = util.loadImages(dataDir + "/images", log=true, numImages=15);
+    images = util.loadImages(dataDir + "/images", log=true, numImages=3);
 end
 
 % Load the camera parameters

test_plot.m

@@ -0,0 +1,7 @@
+% Plot the point cloud unscaled as well as the points of the door frame
+figure
+plotting.plotPointCloud(denoisedPointCloud, camPoses, cameraSizePlotSize=0.2, pcMarkerSize=10, usePcViewer=true)
+xlabel('X [Unknown]');
+ylabel('Y [Unknown]');
+zlabel('Z [Unknown]');
+hold off

test_scaling_from_two_points.m

@@ -29,7 +29,7 @@
 
 % Plot the point cloud unscaled as well as the points of the door frame
 figure
-plotting.plotPointCloud(denoisedPointCloud, camPoses, cameraSizePlotSize=0.2, pcMarkerSize=20)
+plotting.plotPointCloud(denoisedPointCloud, camPoses, cameraSizePlotSize=0.2, pcMarkerSize=10)
 plotting.plotPointCloud(pointCloud(markedPoints, Color=[0,0,1]), [], pcMarkerSize=500);
 xlabel('X [Unknown]');
 ylabel('Y [Unknown]');
@@ -38,7 +38,7 @@
 
 % Plot the point cloud scaled
 figure
-plotting.plotPointCloud(denoisedPointCloudScaled, camPosesScaled, cameraSizePlotSize=0.2*scalingFactor, pcMarkerSize=20*scalingFactor)
+plotting.plotPointCloud(denoisedPointCloudScaled, camPosesScaled, cameraSizePlotSize=0.2*scalingFactor, pcMarkerSize=10)
 xlabel('X [m]');
 ylabel('Y [m]');
 zlabel('Z [m]');