
%% Orientation of Lines within each Group of an Image Sequence
%
% Assumes each group contains lines with roughly approx (accounting for
% noise) orientations'. Note (x,y) = (col, row).
% Since the (x,y) = (col,row) axes is a left-handed coordinate system. The
% angle is formed relative the y-axis in order to form right-handed.
%
%   _________________ >   (x,col)
%  |\
%  |<\- - - (theta)
%  |  \
%  v   \
%
%  (y,row)
%
% Equation of line: alpha*X + beta*Y = gamma;  alpha = sin(theta)
%                                              beta  = cos(theta)
%
% tan(2*theta) =   sum(all lines) { Axy }
%                 ------------------------
%               sum(all lines) { Ayy - Axx }
%
% Axy  = sum(all pts) { (x - xbar)(y) } ; 
% xbar = (1/N) sum{all pts} x
% xbar = (1/N) sum{all pts} y
%
% Axx  = sum(all pts) { (x - xbar)(x) } ; 
% Ayy  = sum(all pts) { (y - ybar)(y) } ; 
%
% 
%
%References:
%1]Tang, Zhongwei, et al. "High-precision camera distortion measurements 
%   with a ?calibration harp?." JOSA A 29.10 (2012): 2134-2143.


%% Input Arguments

% pts - Coordinate matrix containing ordered list of all line points
%       grouped hiearchically by line and then group (image)
%       Nx2 matrix.

% lineSizeMat - Size of sets matrix. Columns correspond to group (image) index,
%               and rows correspond to line index. To deduce number of points 
%               within line segment with group and line index i and j, 
%               respectively then N = setSize(i,j)
%               GxL matrix. 


%% Output Arguments

% orientation - Orientation (slope) coefficients for each group of lines.
%               Gx2 matrix. [alpha1, beta1; ... alphaG, betaG]

% offsets - Optional line offsets (gamma parameters for each line/group pair)
%           Px1 matrix. (vectorized format of lineSizeMat)


%% Orientation of Lines within each Group of an Image Sequence

function [orientation, offsets] = line_orientation_group(pts, lineSizeMat)

%parse input parameters
[pts, lineSizeMat, nGroups, nMaxLines] = input_parser(pts, lineSizeMat);

%--------------------------------------------------
%		 <step 1>
%--------------------------------------------------
%description: iterate through each group of lines

orientation = nan(nGroups, 2);
offsets     = nan(nGroups,nMaxLines);

ptIdx = 1;
for i = 1:nGroups,
    
    nLines = nnz(lineSizeMat(i,:));
    
    Axx  = zeros(nLines,1);
    Ayy  = zeros(nLines,1);
    Axy  = zeros(nLines,1);
    Abar = zeros(nLines,2); %[xbar,ybar]
    
    for k = 1:nLines,
        
        nPts = lineSizeMat(i,k);
        linePts  = pts(ptIdx:ptIdx + nPts-1,:);
        
        Abar(k,:) = mean(linePts,1); 
        
        ctrLinePts = linePts - ones(nPts,1)*Abar(k,:);
        covLinePts = ctrLinePts'*linePts;
        
        Axx(k) = covLinePts(1,1);
        Ayy(k) = covLinePts(2,2);
        Axy(k) = covLinePts(1,2);
        
        ptIdx = ptIdx + nPts;
        
    end
    
    theta = 0.5*atan2(-2*sum(Axy),sum(Axx-Ayy));
    alpha = sin(theta);
    beta  = cos(theta);
    
    orientation(i,:) = [alpha, beta]; 
    
    offsets(i,1:nLines) = [alpha,beta]*Abar';    

end

if ptIdx-1 ~= size(pts,1)
    error('Did not process all points');
end

orientation(isnan(orientation)) = [];

offsets = transpose(offsets);
offsets = offsets(:);
offsets(isnan(offsets)) = [];

end% end of line orientation group


%% Input Parser

function [pts, lineSizeMat, nGroups, nMaxLines] = input_parser(pts, lineSizeMat)

% parse input arguments
p = inputParser;
p.addRequired('pts', @(x) ...
              validateattributes(x,{'numeric'},{'real','nonnan','2d','finite'}));
p.addRequired('lineSizeMat', @(x) ...
              validateattributes(x,{'numeric'},{'real','nonnan','2d','finite'}));   
p.parse(pts, lineSizeMat);
inputs       = p.Results;
pts          = inputs.pts;
lineSizeMat  = inputs.lineSizeMat;

[nGroups, nMaxLines] = size(lineSizeMat);

end %end of input parser

















