黎曼几何与切空间之间的投影

公式：

从黎曼空间投影到切空间，其中P为黎曼均值，也是切空间的参考中心点，Pi是要投影到切空间的点。

从切空间投影回来，其中Si为切空间中的向量。

function Tcov = CovToTan(cov,Mcov)
    Cm12 = Mcov^(-1/2);
    X_new = logm(Cm12 * cov * Cm12);
    C12 = Mcov^(1/2);
    Tcov = Mupper(C12 * X_new * C12);
end

function Cov = TanToCov(vec,Mcov)
    X = Munupper(vec);
    Cm12 = Mcov^(-1/2);
    X = Cm12 * X * Cm12;
    C12 = Mcov^(1/2);
    Cov = C12 * expm(X) * C12;
end

function T = Mupper(X)
    % Upper triangular part vectorization with diagonal preservation.
    % This function keeps the upper triangular part of the matrix and
    % vectorizes it while multiplying non-diagonal elements by sqrt(2).

    % Get the size of X
    [M, N] = size(X);
    
    % Check if matrices are square
    if M ~= N
        error('Matrices must be square');
    end
    
    % Initialize T with zeros
    T = zeros(M, M, 'like', X);
    
    % Calculate the multiplier for non-diagonal elements
    multiplier = sqrt(2);
    
    % Fill T with the upper triangular part, preserving the diagonal
    for i = 1:M
        for j = i:M
            if i == j
                T(i, j) = X(i, j);  % Diagonal element remains the same
            else
                T(i, j) = X(i, j) * multiplier;  % Non-diagonal elements multiplied by sqrt(2)
            end
        end
    end
    
    % Flatten the upper triangular part of T to a vector
    T = T(triu(true(size(T))) == 1);
    T = T';
end

function X = Munupper(T, n)
    % Reverse the operation to reconstruct the matrix from its upper triangular part.
    
    % Calculate the size of the square matrix based on the length of the input vector T
    n = round((sqrt(1 + 8 * length(T)) - 1) / 2);
    
    % Check if T is a valid upper triangular vector
    m = n * (n + 1) / 2;
    if numel(T) ~= m
        error('Invalid input. Input vector size does not match the expected size for upper triangular vectors.');
    end
    
    % Initialize the symmetric matrix X with zeros
    X = zeros(n, n, 'like', T);
    
    % Calculate the indices for the upper triangular part
    [I, J] = find(triu(ones(n)));
    
    % Reverse the vectorization and apply the appropriate scaling to non-diagonal elements
    for k = 1:numel(I)
        i = I(k);
        j = J(k);
        if i == j
            X(i, j) = T(k);  % Diagonal elements remain the same
        else
            X(i, j) = T(k) / sqrt(2);  % Reverse scaling for non-diagonal elements
            X(j, i) = X(i, j);  % Symmetric matrix
        end
    end
end