Этот пример показывает, как объединить данные одометрии робота и наблюдаемые реперные маркеры под названием 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 для вашей ссылки.
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