Ориентир SLAM с использованием маркеров AprilTag

Этот пример показывает, как объединить данные одометрии робота и наблюдаемые реперные маркеры под названием AprilTags, чтобы лучше оценить траекторию робота и знаковые положения в окружении.

Загрузить набор данных

Загрузите .mat файл, который содержит необработанные данные, записанные из rosbag на Robotics™ ClearPath Jackal™. Необработанные данные включают:

  • Изображения, сделанные с помощью Axis™ M1013 сетевой камеры в разрешении 640х480.

  • Данные одометрии предварительно обработаны и синхронизированы с данными изображения.

Rosbag импортируется в MATLAB™ и хранится как .mat файл. Чтобы увидеть, как данные извлекаются из rosbag и предварительно обрабатываются, exampleHelperExtractDataFromRosbag в конце этого примера приведен пример вспомогательного устройства. Для использования вспомогательной функции требуется ROS Toolbox.

filename = matlab.internal.examples.downloadSupportFile("spc/robotics","apriltag_odometry_slam_dataset.zip");
unzip(filename);

Построение графика положения из данных о датчике

В этом примере набор маркеров AprilTag печатается и случайным образом помещается в тестовое окружение. Теги рассматриваются как ориентиры в графике положения. Учитывая внутренность камеры и размер тега, можно преобразовать изображения с наблюдениями AprilTag в знаковые измерения точки с помощью readAprilTag (Computer Vision Toolbox) функция. Эти знаковые измерения относятся к текущей системе координат робота. Чтобы узнать собственные параметры камеры, следуйте шагам в примере Camera Calibration Using AprilTag Markers (Computer Vision Toolbox) или используйте шаблон шахматной карты с помощью приложения Camera Calibrator (Computer Vision Toolbox). Маркеры AprilTag, используемые в этом примере, взяты из tag46h11"семейство без повторяющихся идентификаторов. Размер отпечатанного тега составляет 136,65 мм.

tagFamily = "tag36h11";
tagSize = 136.65; % mm

load apriltag_odometry_slam_dataset/apriltag_odometry_slam_dataset.mat
load cameraParams.mat

После импорта синхронизированных данных о датчике и параметрах камеры создайте график положения оценок узлов из измерений в данных о датчике. График положения содержит оценки узлов, контуры ребер и замыкания циклов, которые оценивают положения робота.

% Create a pose graph object
pg = poseGraph3D;

При помощи реперных маркеров, таких как AprilTags, шаблон блока в изображении предоставляет уникальные идентификаторы для каждого знакового наблюдения. Эта ID-информация уменьшает потребность в сложных алгоритмах ассоциации данных при выполнении одновременной локализации и картографии (SLAM).

Создайте poseGraph3D объект. Создайте переменные для хранения предыдущего положения и идентификатора узла.

lastTform = [];
lastPoseNodeId = 1;

Создайте containers.Map структура данных для поддержания отображения между идентификаторами тегов и их идентификатором узла в графике положения.

tagToNodeIDMap = containers.Map('KeyType','double','ValueType','double');

% Offset transformations for AprilTag observations
% The odometry data recorded in the .mat file is measured from 'odom'
% frame (i.e. fixed world frame) to the 'laser' frame that moves with
% the robot. There is a fixed transformation between the 'laser'
% frame and the camera frame applied manually.
R1 = [0 0 1; -1 0 0; 0 -1 0];
Ta = blkdiag(R1,1); % The camera frame has z axis pointing forward and y axis pointing down
Tb = eye(4); T2(1,3) = -0.11; T(3,3) = 0.146; % Fixed translation of camera frame origin to 'laser' frame

Этот пример оценивает траекторию робота на основе знаковых измерений как из одометрии, так и из наблюдений AprilTag. Выполните итерацию данных датчика и выполните следующие шаги:

  • Добавьте данные одометрии в график положения с помощью addRelativePose функция. Вычислите относительные положения между каждой одометрией перед добавлением к графику положения.

  • Добавьте знаковые измерения из наблюдений AprilTag на изображениях с помощью readAprilTag функция. Поскольку изображение может содержать несколько тегов, выполните итерацию через все возвращенные идентификаторы. Добавьте ориентиры точек относительно текущего узла положения с помощью addPointLandMark функция.

  • Покажите изображение с маркерами вокруг наблюдений AprilTag. Изображение обновляется по мере итерации данных.

figure('Visible','on')
for i = 1:numel(imageData)
    
    % Add odometry data to pose graph
    T = odomData{i};
    if isempty(lastTform)
        nodePair = addRelativePose(pg,[0 0 0 1 0 0 0],[],lastPoseNodeId);
    else
        relPose = exampleHelperComputeRelativePose(lastTform,T);
        nodePair = addRelativePose(pg,relPose,[],lastPoseNodeId);
    end
    currPoseNodeId = nodePair(2);
    
    % Add landmark measurement based on AprilTag observation in the image.
    I = imageData{i};
    [id,loc,poseRigid3d,detectedFamily] = readAprilTag(I,tagFamily,cameraParams.Intrinsics,tagSize);
    
    for j = 1:numel(id)
        % Insert observation markers to image.
        markerRadius = 6;
        numCorners = size(loc,1);
        markerPosition = [loc(:,:,j),repmat(markerRadius,numCorners,1)];
        I = insertShape(I, "FilledCircle", markerPosition, "Color", "red", "Opacity", 1);

        t = poseRigid3d(j).Translation/1000; % change from mm to meter
        po = [t(:);1];
        p = Tb*Ta*po;
                
        if tagToNodeIDMap.isKey(id(j))
            lmkNodeId = tagToNodeIDMap(id(j));
            addPointLandmark(pg, p(1:3)', [], currPoseNodeId, lmkNodeId);
        else
            nodePair = addPointLandmark(pg, p(1:3)', [], currPoseNodeId);
            tagToNodeIDMap(id(j)) = nodePair(2);
        end
    end
    
    % Show the image with AprilTag observation markers.
    imshow(I)
    drawnow
            
    lastTform = T;
    lastPoseNodeId = currPoseNodeId;
end

Оптимизируйте график положения и смотрите результаты

После создания графика положения оптимизируйте его с помощью optimizePoseGraph функция.

pgUpd = optimizePoseGraph(pg);

Визуализируйте и начальный, и оптимизированный график положения. Оптимизированный график положения показывает эти улучшения:

  • Начальное положение робота относительно ориентиров было скорректировано.

  • Ориентиры на каждой стенке лучше выровнены.

  • Вертикальный дрейф в траектории робота по направлению z был скорректирован.

figure('Visible','on');
show(pg); axis equal; xlim([-10.0 5.0]); ylim([-2.0 12.0]);
title('Before Optimization XY View')

figure('Visible','on');
show(pgUpd); axis equal; xlim([-10.0 5.0]); ylim([-2.0 12.0]);
title('After Optimization XY View')

% Vertical drift.
figure('Visible','on');
subplot(2,1,1)
show(pg); xlim([-10.0 5.0]); view(0, 0)
title('Before Optimization XZ View')
subplot(2,1,2)
show(pgUpd); xlim([-10.0 5.0]); view(0, 0)
title('After Optimization XZ View')

Заключение

Окончательный график положения робота накладывается на синий отпечаток офисной области, чтобы показать истинную траекторию и предполагаемые знаковые местоположения. Если вы снова воспроизведете изображения, можно увидеть, что есть хорошая корреляция.

Обратите внимание, что существует один AprilTag, который не взят камерой, отмеченный красным кругом на следующих изображениях. Эти изображения показывают положение AprilTags для вашей ссылки.

Извлечение данных о датчике из rosbag

The exampleHelperExtractDataFromRosbag функция извлекает синхронизированное изображение и данные одометрии из rosbag, записанного на UGV робота Jackal (apriltag_odometry_slam_dataset.bag расположен в той же папке, что и .mat файл). Импорт из rosbag (ROS Toolbox) требует ROS Toolbox.

function [imageData, odomData] = exampleHelperExtractDataFromRosbag(bagName)
%exampleHelperExtractDataFromRosbag Extract synchronized image and odometry
%   data from a rosbag recorded on a Jackal UGV robot. ROS Toolbox license
%   is required to use this function.

%   Copyright 2020 The MathWorks, Inc.

bagSel = rosbag(bagName);
imgSel = bagSel.select('Topic','axis/image_raw_out/compressed'); % Image topic

% Needed for computing change between SE(2) poses
ss = stateSpaceSE2;
ss.WeightTheta = 1;

lastPoseSE2 = [];
lastT = [];

imageData = {};
odomData = {};

% Grab all the image messages
imgObjs = imgSel.readMessages;
for i = 1:numel(imgObjs)
    % Odom data is extracted from tf 
    if bagSel.canTransform('odom', 'laser', imgObjs{i}.Header.Stamp) % From odom frame to laser frame
        % ith odom reading is extracted at the time of the ith image
        Tstamped = bagSel.getTransform('odom', 'laser', imgObjs{i}.Header.Stamp);
        [T, poseSE2] = translateTransformStampedMsg(Tstamped);
    
        if isempty(lastT)
            takeThisImage = true;
        else
            takeThisImage = true;
            % Only accept a new pair of sensor measurements if the robot odom pose
            % has changed more than this threshold since last accepted one
            % Here we use the stateSpaceSE2's distance function to calculate the 2D pose difference
            if ss.distance(poseSE2, lastPoseSE2) < 0.06
                takeThisImage = false;
            end
        end

        if takeThisImage
            I = readImage(imgObjs{i}); % An alternative is to use the newer convenience function "rosReadImage"
            imageData{end+1} = I;
            odomData{end+1} = T;
            lastPoseSE2 = poseSE2;
            lastT = T;
        end
    end
end

end

function [ToutSE3, poseSE2] = translateTransformStampedMsg(Tin)
%translateTransformMsg Extract the 4x4 homogeneous transformation matrix,
%   ToutSE3, from a TransformStamped message object. This function also 
%   returns a second output, poseSE2, which is an SE(2) pose vector 
%   computed by projecting ToutSE2 to the XY plane. Note the formulation
%   used is approximate and relies on the assumption that the robot mostly
%   moves on the flat ground. 

%   Copyright 2020 The MathWorks, Inc.

    %Tin - TransformStamped message object
    x = Tin.Transform.Translation.X;
    y = Tin.Transform.Translation.Y;
    z = Tin.Transform.Translation.Z;
    
    qx = Tin.Transform.Rotation.X;
    qy = Tin.Transform.Rotation.Y;
    qz = Tin.Transform.Rotation.Z;
    qw = Tin.Transform.Rotation.W;
    q = [ qw, qx, qy, qz]; % Note the sequence for quaternion in MATLAB

    ToutSE3 = robotics.core.internal.SEHelpers.poseToTformSE3([x,y,z,q]);
    YPR = quat2eul(q);
    poseSE2 = [x,y,YPR(1)];
end
Для просмотра документации необходимо авторизоваться на сайте