intrinsicsParams = helperReadYAML('cameraParams.yaml');

imageSize = [intrinsicsParams.image_height intrinsicsParams.image_width];
intrinsicMatrix = intrinsicsParams.camera_matrix;
distortionCoefficients = intrinsicsParams.distortion_coefficients;

intrinsicsObj = cameraIntrinsicsFromOpenCV(intrinsicMatrix,distortionCoefficients,imageSize);

% Load the captured image.
imageName = fullfile(toolboxdir('vision'),'visiondata','calibration','stereo','left','left01.png');
I = imread(imageName);

% Undistort the image.
J = undistortImage(I,intrinsicsObj,'OutputView','full');

% Display the result.
figure
montage({I,J})

function cameraParams = helperReadYAML(filename)
% helperReadYAML reads a ROS YAML file, filename, and returns a structure 
% with these fields: image_width, image_height, camera_name,
% camera_matrix, distortion_model, distortion_coefficients,
% rectification_matrix, and projection_matrix. These fields are stored 
% in the YAML file colon separated from their values in different lines.

    f = fopen(filename,'r');
    stringFields = {'camera_name','distortion_model'};
    
    while ~feof(f)

        [name,value,isEmptyLine] = helperReadYAMLLine(f);
        if isEmptyLine
            continue
        end

        if ~isempty(value)
            % Convert all values to numbers except for known string
            % fields.
            if ~any(contains(name, stringFields))
                value = str2num(value); %#ok
            end
        else
            % An empty value in ROS YAML files indicates a matrix in
            % upcoming lines. Read the matrix from the upcoming lines.
            value = helperReadYAMLMatrix(f);
        end

        % Store post-processed value.
        cameraParams.(name) = value;
    end
    
    fclose(f);
end

function matrix = helperReadYAMLMatrix(f)
%   helperReadYAMLMatrix reads a matrix from the ROS YAML file. A matrix in
%   a ROS YAML file has three fields: rows, columns and data. rows and col
%   describe the matrix size. data is a continguous array of the matrix
%   elements in row-major order. This helper function assumes the presence
%   of all three fields of a matrix to return the correct matrix.

    numRows = 0;
    numCols = 0;
    data = [];

    % Read numRows, numCols and matrix data.
    while ~feof(f)
        [name,value,isEmptyLine] = helperReadYAMLLine(f);

        if isEmptyLine
            continue
        end

        switch name
            case 'rows'
                numRows = str2num(value); %#ok
            case 'cols'
                numCols = str2num(value); %#ok
            case 'data'
                data    = str2num(value); %#ok

                % Terminate the while loop as data is the last 
                % field of a matrix in the ROS YAML file.
                break
            otherwise
                % Terminate the while loop if any other field is
                % encountered.
                break
        end
    end

    if numel(data) == numRows*numCols
        % Reshape the matrix using row-major order.
        matrix = reshape(data,[numCols numRows])';
    end
end

function [name,value,isEmptyLine] = helperReadYAMLLine(f)

    % Read line from file.
    line = fgetl(f); 

    % Trim leading and trailing whitespaces.
    line = strtrim(line);

    if isempty(line) || line(1)=='#'
        % Empty line or comment.
        name = '';
        value = '';
        isEmptyLine = true;
    else
        % Split the line to get name and value.
        c = strsplit(line,':');
        assert(length(c)==2,'Unexpected file format')
        
        name = c{1};
        value = strtrim(c{2}); % Trim leading whitespace.
        isEmptyLine = false;
    end
end