classdef poseGraph3D < nav.algs.internal.PoseGraphBase
%POSEGRAPH3D Create a 3D pose graph representation
%   POSEGRAPH = poseGraph3D() creates a 3D pose graph object
%
%   POSEGRAPH = poseGraph3D('MaxNumEdges', _, 'MaxNumNodes', _)
%   specifies upper bound on the Number of edges and nodes allowed in
%   POSEGRAPH when generating code. This limit is only required when
%   generating code.
%
%   POSEGRAPH3D properties:
%      NumNodes            - Number of nodes in pose graph
%      NumEdges            - Number of edges in pose graph
%      NumLoopClosureEdges - Number of loop closure edges in pose graph
%      LoopClosureEdgeIDs  - Loop closure edge IDs in pose graph
%
%   POSEGRAPH3D methods:
%      addRelativePose     - Add a new relative pose to pose graph
%      edges               - Get edges from pose graph
%      nodes               - Get nodes from pose graph
%      edgeConstraints     - Get edge constraints from pose graph
%      edgeResidualErrors  - Compute residual error for each edge
%      findEdgeID          - Find edge ID given an edge
%      removeEdges         - Remove edges from pose graph
%      copy                - Create a copy of the object
%      show                - Plot pose graph in MATLAB figure
%
%See also poseGraph, optimizePoseGraph

%   Copyright 2017-2020 The MathWorks, Inc.

%#codegen
    properties (Constant, Access = ?nav.algs.internal.InternalAccess)
        PoseLength = 7

        PoseDeltaLength = 6

        TformSize = [4 4]

        InfoMatSize = [6 6]

        CompactInfoMatLength = 21
        
        DefaultEdgeInfoMat = eye(6)
    end

    methods
        function obj = poseGraph3D(varargin)
        %POSEGRAPH3D Constructor
            obj@nav.algs.internal.PoseGraphBase(varargin{:});
        end

    end

    methods (Access = protected)
        function defaultObj = getDefaultObject(~,maxNumEdges,maxNumNodes)
        %getDefaultObject
            if coder.target('MATLAB')
                defaultObj = poseGraph3D;
            else
                % Codegen requires maxNumEdges and nodes to be set which
                % initializing an empty graph
                if nargin < 3
                    defaultObj = poseGraph3D('MaxNumEdges',8000,'MaxNumNodes',8001);
                else
                    defaultObj = poseGraph3D('MaxNumEdges',maxNumEdges,'MaxNumNodes',maxNumNodes);
                end
            end
        end

        function hLine = drawEdge(~, ax, hLine, n1, n2, varargin)
        %drawEdge
            if isempty(hLine)
                hLine = plot3(ax, [n1(1,1), n2(1,1)], [n1(1,2), n2(1,2)], [n1(1,3), n2(1,3)], varargin{:});
            else
                hLine.XData = [hLine.XData, nan, n1(1), n2(1)];
                hLine.YData = [hLine.YData, nan, n1(2), n2(2)];
                hLine.ZData = [hLine.ZData, nan, n1(3), n2(3)];
            end
        end

        function drawID(~, ax, loc, str, varargin)
        %drawID
            text(ax, loc(:,1), loc(:,2), loc(:,3), str, varargin{:});
        end

    end

    methods (Access = ?nav.algs.internal.InternalAccess)

        function [tformNew, infoMatNew] = invertConstraint(~, tform, infoMat)
        %invertConstraint
            [tformNew, infoMatNew] = nav.algs.internal.PoseGraphHelpers.invertConstraintSE3(tform, infoMat);
        end

        function pose = tformToPose(~, T)
        %tformToPose
            pose = robotics.core.internal.SEHelpers.tformToPoseSE3(T);
        end

        function T = poseToTform(~, pose)
        %poseToTform
            T = robotics.core.internal.SEHelpers.poseToTformSE3(pose);
        end

        function T = tforminv(~, pose)
        %poseToTform
            T = robotics.core.internal.SEHelpers.tforminvSE3(pose);
        end

        function infoMat = deserializeInformationMatrix(~, im)
        %deserializeInformationMatrix
            infoMat = robotics.core.internal.SEHelpers.deserializeInformationMatrixSE3(im);
        end

        function im = serializeInformationMatrix(~, infoMat)
        %serializeInformationMatrix
            im = robotics.core.internal.SEHelpers.serializeInformationMatrixSE3(infoMat);
        end
        
        function tform = multiplyTform(~,T1,T2)
            tform = T1*T2;
        end

    end

end
