classdef poseGraph < nav.algs.internal.PoseGraphBase
%poseGraph Create a 2D pose graph representation
%   POSEGRAPH = poseGraph() creates a 2D pose graph object
%
%   POSEGRAPH = poseGraph('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.
%
%   POSEGRAPH 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
%
%   POSEGRAPH 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 poseGraph3D, optimizePoseGraph

%   Copyright 2017-2020 The MathWorks, Inc.

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

        PoseDeltaLength = 3

        TformSize = [3 3]

        InfoMatSize = [3 3]

        CompactInfoMatLength = 6
        
        DefaultEdgeInfoMat = eye(3)
    end

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

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

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

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


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

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

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

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

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

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

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

    end





end
