このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。

ROS からのカメラ内部パラメーターのインポート

ROS カメラ キャリブレーション パッケージは、OpenCV カメラ キャリブレーション ツール [1] を使用してカメラの内部パラメーターを推定します。ROS でカメラをキャリブレーションした後、ROS のカメラ キャリブレーション パーサーを使用して、その内部パラメーターを YAML ファイルにインポートできます。undistortImage などの Computer Vision Toolbox™ 関数でキャリブレーションされたカメラを使用するには、YAML ファイルからカメラ パラメーターを読み取り、cameraIntrinsicsFromOpenCV を使用してカメラ パラメーターを cameraIntrinsics オブジェクトに変換しなければなりません。

メモ: 関数 cameraIntrinsicsFromOpenCV は、ROS plumb-bob 歪みモデルを使用するピンホール カメラ モデルについてのみ、カメラ内部パラメーターのインポートをサポートします。

ROS YAML ファイルからのカメラ内部パラメーターの読み取り

補助関数 helperReadYAML を使用して、cameraParams.yaml に保存されているカメラ パラメーターを読み取ります。

intrinsicsParams = helperReadYAML('cameraParams.yaml');

cameraIntrinsicsFromOpenCV を使用した cameraIntrinsics オブジェクトの作成

関数 cameraIntrinsicsFromOpenCV を使用して、カメラ行列と歪み係数から cameraIntrinsics オブジェクトを作成します。

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

intrinsicsObj = cameraIntrinsicsFromOpenCV(intrinsicMatrix,distortionCoefficients,imageSize);

イメージの歪み補正

インポートされたカメラ内部パラメーターを undistortImage で使用して、キャリブレーションされたカメラを使用してキャプチャされたイメージの歪みを補正します。

% 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})

サポート関数

helperReadYAML

関数 helperReadYAML は、ROS からエクスポートされた入力 YAML ファイルから単眼カメラのパラメーターを読み取ります。

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

helperReadYAMLMatrix

関数 helperReadYAMLMatrix は、ROS YAML ファイル内の行列の行、列、およびデータ フィールドを読み取ります。

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

helperReadYAMLLine

関数 helperReadYAMLLine は、ROS YAML ファイルの行を読み取ります。

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

参考文献

[1] http://wiki.ros.org/camera_calibration