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

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;

lastTform = [];
lastPoseNodeId = 1;

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

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

pgUpd = optimizePoseGraph(pg);

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')

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