ROS からのステレオカメラパラメーターのインポート

ライブスクリプトを開く

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

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

ROS INI ファイルからのステレオカメラパラメーターの読み取り

補助関数 helperReadINI を使用して、Params.ini に保存されているステレオカメラパラメーターを読み取ります。

stereoParamsINI = helperReadINI("stereoParams.ini");

ステレオカメラのベースラインパラメーターの計算

ステレオカメラのベースラインパラメーターは、ステレオカメラペア内の 2 つのカメラの相対的な並進と回転を記述します。stereoParametersFromOpenCVを使用してstereoParametersオブジェクトを作成するには、カメラ 1 に対するカメラ 2 の相対的な回転と並進が必要です。これらは ROS INI ファイル [2] から読み取った平行化行列と射影行列から計算できます。

stereoParams 構造体から 2 つのカメラパラメーターを抽出します。

cameraParams1 = stereoParamsINI.narrow_stereo_left;
cameraParams2 = stereoParamsINI.narrow_stereo_right;

射影行列の最後の列から、カメラ 1 に対するカメラ 2 の並進を抽出します。

translationOfCamera2 = cameraParams2.projection(:,end);

カメラ 1 に対するカメラ 2 の回転 $R_{21}$ は、 $R_{1}$ と $R_{2}$ のステレオペアの平行化行列から導出されます。平行化行列は、両方のステレオイメージのエピポーラ線が平行になるように、カメラ座標系を理想的なステレオイメージ平面に合わせる回転行列です。カメラ 1 に対するカメラ 2 の回転を $R_{21}$ = $R_{2}$ * $R_{1}^{T}$ として計算します。

rotationOfCamera2 = cameraParams2.rectification*cameraParams1.rectification';

`stereoParametersFromOpenCV` を使用した `stereoParameters` オブジェクトの作成

stereoParams 構造体から 2 つのカメラの内部パラメーター行列と歪み係数を抽出します。

intrinsicMatrix1 = cameraParams1.camera_matrix;
intrinsicMatrix2 = cameraParams2.camera_matrix;

distortionCoefficients1 = cameraParams1.distortion;
distortionCoefficients2 = cameraParams2.distortion;

stereoParams 構造体の image フィールドからイメージサイズを取得します。

imageSize = [stereoParamsINI.image.height stereoParamsINI.image.width];

stereoParametersFromOpenCVを使用して、ROS ステレオカメラパラメーターからstereoParametersオブジェクトを作成します。

stereoParametersObj = stereoParametersFromOpenCV(intrinsicMatrix1, ...
    distortionCoefficients1,intrinsicMatrix2,distortionCoefficients2, ...
    rotationOfCamera2,translationOfCamera2,imageSize);

ステレオイメージのペアの平行化

rectifyStereoImagesでインポートしたステレオパラメーターを使用し、キャリブレーション済みのステレオカメラを使用してキャプチャされたイメージペアを平行化します。

% Load the image pair.
imageDir = fullfile(toolboxdir("vision"),"visiondata","calibration","stereo");
leftImages = imageDatastore(fullfile(imageDir,"left"));
rightImages = imageDatastore(fullfile(imageDir,"right"));
I1 = readimage(leftImages,1);
I2 = readimage(rightImages,1);

% Rectify the image pair.
[J1,J2] = rectifyStereoImages(I1,I2,stereoParametersObj,OutputView="full");

% Display the results.
figure
J = stereoAnaglyph(J1,J2);
imshow(J)

Figure contains an axes object. The hidden axes object contains an object of type image.

サポート関数

helperReadINI

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

function cameraParams = helperReadINI(filename)
% helperReadINI reads a ROS INI file, filename, and returns a structure with
% these fields: image, <camera_name1>, <camera_name2>. image is a
% structure describing the height and width of the image captured by the
% cameras of the stereo pair. The fields <camera_name1> and <camera_name2>
% are structures named after the camera names present in the INI file, and they contain
% these fields: camera_matrix, distortion, rectification_matrix,
% and projection_matrix. These fields are stored in the INI file with their
% values placed in a new line followed by their name.

    f = fopen(filename,"r");
    sectionName = '';
    
    while ~feof(f)
        % Read line from file.
        line = fgetl(f);

        % Trim leading and trailing whitespaces.
        line = strtrim(line);
        
        if isempty(line) || line(1)=='#'
            % Skip empty line and comments.
            continue
        elseif line(1) == '[' && line(end) == ']'
            % Identify section names and continue reading.
            sectionName = line(2:end-1);
            sectionName = strrep(sectionName,'/','_');
            continue
        end

        % Replace blankspaces with underscores to create valid MATLAB variable
        % name.
        name = line;
        name(name == ' ') = '_';
        
        % Read the value data in upcoming lines.
        value = [];
        while ~feof(f)
            line = fgetl(f);
            line = strtrim(line);

            if isempty(line)
                % A empty line indicates end of value data.
                break
            elseif line(1)=='#'
                % Skip comment lines.
                continue
            end
            line = str2num(line); %#ok
            value = [value; line]; %#ok
        end
    
        % Store post-processed value.
        if isempty(sectionName)
            cameraParams.(name) = value;
        else
            cameraParams.(sectionName).(name) = value;
        end
    end
    
    fclose(f);
end

参考文献

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

[2] http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CameraInfo.html

ROS からのステレオ カメラ パラメーターのインポート

ROS INI ファイルからのステレオ カメラ パラメーターの読み取り

ステレオ カメラのベースライン パラメーターの計算

stereoParametersFromOpenCV を使用した stereoParameters オブジェクトの作成

ステレオ イメージのペアの平行化