function M = essentialfactory(k, strSigned)
% Manifold structure to optimize over the space of essential matrices.
%
% function M = essentialfactory(k)
% function M = essentialfactory(k, 'signed')
% function M = essentialfactory(k, 'unsigned')
%
%
% Quotient representation of the essential manifold: deals with the
% representation of the space of essential matrices M_rE. These are used in
% computer vision to represent the epipolar constraint between projected
% points in two perspective views.
%
% The space is represented as the quotient (SO(3)^2/SO(2)).
% See the following references for details:
%
%   R. Tron, K. Daniilidis,
%   "On the quotient representation of the essential manifold"
%   IEEE Conference on Computer Vision and Pattern Recognition, 2014
%
% For computational purposes, each essential matrix is represented as a
% [3x6] matrix where each [3x3] block is a rotation.
%
% The metric used is the one induced by the submersion of M_rE in SO(3)^2.
%
% Tangent vectors are represented in the Lie algebra of SO(3)^2, i.e., as
% [3x6] matrices where each [3x3] block is a skew-symmetric matrix.
% Use the function tangent2ambient(X, H) to switch from the Lie algebra
% representation to the embedding space representation in R^(3x6).
%
% By default, k = 1, and the geometry is 'signed'.
%
% Optional arguments:
%   "signed"    selects the signed version of the manifold
%   "unsigned"  selects the unsigned version of the manifold
%
% See also rotationsfactory

% Please cite the Manopt paper as well as the research paper:
%     @InProceedings{tron2014essential,
%       Title        = {On the quotient representation of the essential manifold},
%       Author       = {Tron, R. and Daniilidis, K.},
%       Booktitle    = {IEEE Conference on Computer Vision and Pattern Recognition},
%       Year         = {2014},
%       Organization = {{IEEE CVPR}}
%     }


% This file is part of Manopt: www.manopt.org.
% Original author: Roberto Tron, Aug. 8, 2014
% Contributors: Bamdev Mishra, May 15, 2015.
% Change log:
%   Jan. 4, 2021 (NB):
%       Compatibility with Octave 6.1.0: moved quite a few nested functions
%       to end-of-file functions, and made tangent2ambient accessible as
%       M.tangent2ambient (which should have been the case already).
%   July 5, 2024 (NB):
%       Added missing M.tangent2ambient_is_identity = false. Also retr2.

% RT: General implementation note: to streamline component-wise
% computations, in tangentProjection and exponential,
% we flatten out the arguments into [3 x 3 x 2K] arrays, compute the
% components all together, and then sharp the result again into [3 x 6 x K]
% arrays.


    % Optional parameters to switch between the signed and unsigned
    % versions of the manifold.
    if ~exist('strSigned', 'var') || isempty(strSigned)
        strSigned = 'signed';
    end
    switch(strSigned)
        case 'signed'
            flagSigned = true;
        case 'unsigned'
            flagSigned = false;
        otherwise
            error('Second argument can be either empty, ''signed'', or ''unsigned''.');
    end

    
    if ~exist('k', 'var') || isempty(k)
        k = 1;
    end
    
    if k == 1
        M.name = @() sprintf('Quotient representation of the essential manifold, %s', strSigned);
    elseif k > 1 && k == round(k)
        M.name = @() sprintf('Product of %d quotient representations of the essential manifold, %s', k, strSigned);
    else
        error('k must be an integer no less than 1.');
    end
    
    M.dim = @() k*5;
    
    M.inner = @(x, d1, d2) d1(:).'*d2(:);
    
    M.norm = @(x, d) norm(d(:));
    
    M.typicaldist = @() pi*sqrt(2*k);
    
    M.proj = @tangentProjection;
    function HProjHoriz=tangentProjection(X,H)
        % Project H on the tangent space of SO(3)^2
        HProj = essential_sharp(multiskew(multiprod(multitransp(essential_flat(X)), essential_flat(H))));
        
        % Compute projection on vertical component
        p = vertproj(X, HProj);
        
        HProjHoriz = HProj - multiprod(p/2,[essential_hat3(permute(X(3,1:3,:),[2 3 1])) essential_hat3(permute(X(3,4:6,:),[2 3 1]))]);% BM: okay
    end
    
    
    M.tangent = @(X, H) essential_sharp(multiskew(essential_flat(H)));
    
    M.egrad2rgrad=@egrad2rgrad;
    function rgrad = egrad2rgrad(X, egrad)
        rgrad = M.proj(X, egrad);
    end
    
    M.ehess2rhess = @ehess2rhess;
    function rhess = ehess2rhess(X, egrad, ehess, S)
        % Reminder: S contains skew-symmetric matrices. The actual
        % direction that the point X is moved along is X*S.
        RA = p1(X);
        RB = p2(X);
        SA = p1(S);
        SB = p2(S);
        
        G = egrad; 
        GA = p1(G);
        GB = p2(G);
        
        H = ehess; 
        
        % RT: We now compute the connection, i.e. the part of the derivative
        % given by the curvature of the space (as opposed to a simple
        % Euclidean derivative).
        
        % The following is the vectorized version of connection=-[multisym(GA'*RA)*SA multisym(GB'*RB)*SB];
        connection = tangent2ambient(X,-cat(2,...
            multiprod(multisym(multiprod(multitransp(GA), RA)), SA),...
            multiprod(multisym(multiprod(multitransp(GB), RB)), SB)));
        rhess = M.proj(X,H + connection);
    end
    
    
    
    M.exp = @exponential;
    function Y = exponential(X, U, t)
        if nargin == 3
            U = t*U;
        end
        
        UFlat = essential_flat(U);
        exptUFlat = rot3_exp(UFlat);
        Y = essential_sharp(multiprod(essential_flat(X), exptUFlat));
    end
    
    M.retr = @exponential;
    M.retr2 = @exponential;
    
    M.log = @logarithm; 
    function U = logarithm(X, Y)
        
        QX = [X(:,1:3,:);X(:,4:6,:)];
        QY = [Y(:,1:3,:);Y(:,4:6,:)];
        QYr = essential_closestRepresentative(QX,QY,'flagSigned',flagSigned);
        Yr = [QYr(1:3,:,:) QYr(4:6,:,:)];
        U = zeros(size(X));
        U(:,1:3,:) = rot3_log(multiprod(multitransp(X(:,1:3,:)),Yr(:,1:3,:)));
        U(:,4:6,:) = rot3_log(multiprod(multitransp(X(:,4:6,:)),Yr(:,4:6,:)));
    end
    
    M.hash = @(X) ['z' hashmd5(X(:))];
    
    M.rand = @() randessential(k);
    function Q = randessential(N)
        % Generates random essential matrices.
        %
        % function Q = randessential(N)
        %
        % Q is a [3x6] matrix where each [3x3] block is a uniformly distributed
        % matrix.
        
        if nargin < 1
            N = 1;
        end
        
        Q = [randrot(3,N) randrot(3,N)];
    end
    
    M.randvec = @randomvec;
    function U = randomvec(X)
        U = tangentProjection(X, essential_sharp(randskew(3, 2*k)));
        U = U / sqrt(M.inner([],U,U));
    end
    
    M.lincomb = @matrixlincomb;
    
    M.zerovec = @(x) zeros(3, 6, k);
    
    M.transp = @transport;
    function S2 = transport(X1, X2, S1)
        % Transport a vector from the tangent space at X1 to the tangent
        % space at X2. This transport uses the left translation of the
        % ambient group and preserves the norm of S1. The left translation
        % aligns the vertical spaces at the two elements.
        
        % Group operation in the ambient group, X12=X2'*X1
        X12 = essential_sharp(multiprod(multitransp(essential_flat(X2)),essential_flat(X1)));
        X12Flat = essential_flat(X12);
        
        % Left translation, S2=X12*S*X12'
        S2 = essential_sharp(multiprod(X12Flat,multiprod(essential_flat(S1),multitransp(X12Flat))));
    end
    
    M.pairmean = @pairmean;
    function Y = pairmean(X1, X2)
        V = M.log(X1, X2);
        Y = M.exp(X1, .5*V);
    end
    
    M.dist = @(x, y) M.norm(x, M.log(x, y)); 
    
    M.vec = @(x, u_mat) u_mat(:);
    M.mat = @(x, u_vec) reshape(u_vec, [3, 6, k]);
    M.vecmatareisometries = @() true;
    
    M.tangent2ambient_is_identity = false;
    M.tangent2ambient = @tangent2ambient;
    
end


%% Some functions used by the essential factory

function v = vee3(V)
    v = squeeze([V(3,2,:)-V(2,3,:); V(1,3,:)-V(3,1,:); V(2,1,:)-V(1,2,:)])/2;
end


% Compute the exponential map in SO(3) using Rodrigues' formula
%  function R = rot3_exp(V)
% V must be a [3x3xN] array of [3x3] skew-symmetric matrices.
function R = rot3_exp(V)
    v = vee3(V);
    nv = cnorm(v);
    idxZero = nv < 1e-15;
    nvMod = nv;
    nvMod(idxZero) = 1;
    
    vNorm = v./([1;1;1]*nvMod);
    
    % Matrix exponential using Rodrigues' formula
    nv = shiftdim(nv,-1);
    c = cos(nv);
    s = sin(nv);
    [VNorm,vNormShift] = essential_hat3(vNorm);
    vNormvNormT = multiprod(vNormShift,multitransp(vNormShift));
    R=multiprod(eye(3),c)+multiprod(VNorm,s)+multiprod(vNormvNormT,1-c);
end



% Compute the logarithm map in SO(3)
%  function V = rot3_log(R)
% V is a [3x3xN] array of [3x3] skew-symmetric matrices
function V = rot3_log(R)
    skewR = multiskew(R);
    ctheta = (multitrace(R)'-1)/2;
    stheta = cnorm(vee3(skewR));
    theta = atan2(stheta,ctheta);
    
    V=skewR;
    for ik=1:size(R,3)
        V(:,:,ik)=V(:,:,ik)/sincN(theta(ik));
    end
end


function sx = sincN(x)
    sx = sin(x)./x;
    sx(x==0) = 1;
end

function nv = cnorm(v)
    nv = sqrt(sum(v.^2));
end

function M = vertproj(X, H)
    M = multiprod(X(3, 1:3, :), permute(vee3(H(:, 1:3, :)), [1 3 2])) + ...
        multiprod(X(3, 4:6, :), permute(vee3(H(:, 4:6, :)), [1 3 2]));
end

function M = p1(X)
    M = X(:, 1:3, :);
end

function M = p2(X)
    M = X(:, 4:6, :);
end

function U = tangent2ambient(X, H)
    U = essential_sharp(multiprod(essential_flat(X), essential_flat(H)));
end
