improvespatialcoordinate.m

三维重建

function [Yimp,C] = improveSpatialCoordinate(ste,X,Y)
%
%   [Yimp, C] = improveSpatilaCoordinate(ste,X,Y)
%
%   Improves the estimated spatial coordinate Y of X with a BLUE estimator.
%   Additionally the covariance of the spatial coordinates is computed.
%   Diagonalizing C gives you the confidence ellipsoid of the spatial
%   estimation.
%
%
%
%



    %%%%%%%%%%%%%%%%%%%%%%%%%
        h = 1e-5;
        epsl = 1e-10;
        epsY = 1e-5;
    %%%%%%%%%%%%%%%%%%%%%%%%%
    Lstar = X';
    %--------------------------------------------------------
    Cinv = diag([ste.leftCam.s.^(-2)*ste.leftCam.s0;ste.rightCam.s.^(-2)*ste.rightCam.s0]);
    %-------initialization of loop conditions----------------
     l = Lstar - [project(ste,Y)'];
     dY = 1;
    %-----------------------Regression-----------------------
    disp('  ')
    disp('Performing Improvement with BLUE ...');
    while norm(l) > epsl & norm(dY) > epsY
        J1 = getNumericalSpatialJacobiMatrix(ste.leftCam,ste.leftParam,Y,h);   %get Jacobimatrix for Camera #1
        J2 = getNumericalSpatialJacobiMatrix(ste.rightCam,ste.rightParam,Y,h); %get Jacobimatrix for Camera #2
        
        A = [J1;J2];
        L = project(ste,Y)';
        l = Lstar - L;  
        dY = pinv(A'*Cinv*A)*A'*Cinv*l;
        err = sqrt(mean(l.^2));
        disp(['Norm dY ' num2str(norm(dY)) '  Error:  ' num2str(err)])
        Y = Y + dY';
    end
    
    %---- compute confidence ellipsoid ----
    disp(' ')
    disp('Computing covariance matrix ...')
    
    J1 = getNumericalSpatialJacobiMatrix(ste.leftCam,ste.leftParam,Y,h);   % get Jacobimatrix for Camera #1
    J2 = getNumericalSpatialJacobiMatrix(ste.rightCam,ste.rightParam,Y,h); % get Jacobimatrix for Camera #2
    A = [J1;J2];
    L = project(ste,Y)';
    l = Lstar - L;  
    Ctheta = pinv(A'*Cinv*A);
    dY = pinv(A'*Cinv*A)*A'*Cinv*l;
    v = A*dY - l;
    theta_s0 = sqrt(v'*Cinv*v); % (2*m-nEst) = 4-3 = 1
    C = theta_s0^2*Ctheta;           % get covariance matrix by vanishing s0
 
    
    %%% return %%%
    Yimp = Y;