Skip to content
Snippets Groups Projects
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