Импортируйте параметры стереофотоаппарата из ROS

Скрипт Open Live Script

Пакет калибровки фотоаппарата ROS оценивает параметры стереофотоаппарата с помощью инструментов калибровки фотоаппарата OpenCV [1]. После калибровки стереофотоаппарата в ROS можно экспортировать его параметры камеры в файл INI с помощью синтаксического анализатора калибровки фотоаппарата. Использовать калиброванный стереофотоаппарат с функциями Computer Vision Toolbox™, такими как rectifyStereoImages, необходимо считать параметры камеры из файла INI и преобразовать их в stereoParameters объект с помощью stereoParametersFromOpenCV.

Примечание: stereoParametersFromOpenCV функционируйте поддержки, импортирующие параметры стереофотоаппарата только для тех моделей камеры с точечной диафрагмой, которые используют ROS plumb-bob модель искажения.

Считайте параметры стереофотоаппарата из файла ROS INI

Считайте параметры стереофотоаппарата, сохраненные в stereoParams.ini использование помощника функционирует helperReadINI. 

stereoParamsINI = helperReadINI("stereoParams.ini");

Вычислите базовые параметры стереофотоаппарата

Базовые параметры стереофотоаппарата описывают относительный перевод и вращение этих двух камер в паре стереофотоаппарата. Относительное вращение и перевод камеры 2 относительно камеры 1 требуются, чтобы создавать stereoParameters объект с помощью stereoParametersFromOpenCV. Можно вычислить их из матриц исправления и проекции, считанных из файла [2] ROS INI.

Извлеките два параметра камеры из stereoParams структура.

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

Извлеките перевод камеры 2 относительно камеры 1 из последнего столбца матрицы проекции.

translationOfCamera2 = cameraParams2.projection(:,end);

Вращение камеры 2 относительно камеры 1, R21, выведен из матриц исправления стереопары R1 и R2. Матрицы исправления являются матрицами вращения, которые выравнивают систему координат камеры к идеальной плоскости стереоизображения, таким образом, что epipolar линии в обоих стереоизображениях параллельны. Вычислите вращение камеры 2 относительно камеры 1 как R21= R2*R1T.

rotationOfCamera2 = cameraParams2.rectification*cameraParams1.rectification';

Создайте stereoParameters Объект с помощью stereoParametersFromOpenCV

Извлеките внутренние матрицы и коэффициенты искажения этих двух камер от stereoParams структура.

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

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

Получите размер изображения из image поле stereoParams структура.

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

Используйте stereoParametersFromOpenCV создать stereoParameters объект от параметров стереофотоаппарата ROS. 

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 axes object contains an object of type image.

Вспомогательные Функции

helperReadINI

helperReadINI функционируйте читает параметры камеры из ее файла входа INI, который был экспортирован от ROS.

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

Документация Computer Vision Toolbox

Поддержка

Для просмотра документации необходимо авторизоваться на сайте