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