-
Rohan Kumar authoredRohan Kumar authored
Code owners
Assign users and groups as approvers for specific file changes. Learn more.
vslam_implementation.m 22.59 KiB
function worldPointSet = vslam_implementation(imagePath)
% Import dependencies
addpath('./Mathworks_VSLAM_Example/');
% Initialize image dataset
imds = imageDatastore(imagePath);
% Inspect the first image
currFrameIdx = 1;
currI = readimage(imds, currFrameIdx);
himage = imshow(currI);
% Process the image sequence
worldPointSet = ProcessImageSequence(imds);
end
function worldPointSetOutput = ProcessImageSequence(imds)
currFrameIdx = 1;
currI = readimage(imds, currFrameIdx);
%% Map Initilization
% Set random seed for reproducibility
rng(0);
% Create a cameraIntrinsics object to store the camera intrinsic parameters.
% The intrinsics for the dataset can be found at the following page:
% https://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats
% Note that the images in the dataset are already undistorted, hence there
% is no need to specify the distortion coefficients.
focalLength = [535.4, 539.2]; % in units of pixels
principalPoint = [320.1, 247.6]; % in units of pixels
imageSize = size(currI,[1 2]); % in units of pixels
intrinsics = cameraIntrinsics(focalLength, principalPoint, imageSize);
% Detect and extract ORB features
scaleFactor = 1.2;
numLevels = 8;
numPoints = 1000;
[preFeatures, prePoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels, numPoints);
currFrameIdx = currFrameIdx + 1;
firstI = currI; % Preserve the first frame
isMapInitialized = false;
% Map initialization loop
while ~isMapInitialized && currFrameIdx < numel(imds.Files)
currI = readimage(imds, currFrameIdx);
[currFeatures, currPoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels, numPoints);
currFrameIdx = currFrameIdx + 1;
% Find putative feature matches
indexPairs = matchFeatures(preFeatures, currFeatures, Unique=true, ...
MaxRatio=0.9, MatchThreshold=40);
preMatchedPoints = prePoints(indexPairs(:,1),:);
currMatchedPoints = currPoints(indexPairs(:,2),:);
% If not enough matches are found, check the next frame
minMatches = 100;
if size(indexPairs, 1) < minMatches
continue
end