Знаменательный SLAM Используя маркеры AprilTag

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

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

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

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

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

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) функция. Эти знаменательные измерения относительно текущей системы координат робота. Чтобы узнать внутренние параметры вашей камеры, выполните шаги в Калибровке фотоаппарата Используя Маркеры AprilTag (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

exampleHelperExtractDataFromRosbag функционируйте извлекает изображение sychronized и данные об одометрии из rosbag, зарегистрированного на Шакале робот UGV (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
Для просмотра документации необходимо авторизоваться на сайте