singlepointreconstruction.m

三维重建

function [X , pY, v] = singlePointReconstruction(ste,x,camnum,dsp)
%
%   [Y, pY, v] = SINGELPOINTRECONSTRUCTION(ste,x[,camnum,dsp])
%
%   Estimates the depth of the image coordinate pair store in the row vector
%   X. pY hold the difference between the estimations of the y coordinate
%   from the two cameras. v hold the length scales of the confidence
%   ellipsoid.
%
%   You can specify camnum (camnum = {1|2}) to specify which camera shall
%   be taken to estimate the depth. If dsp is set to 1 the estimation and pY will
%   be displayed.
%
    if nargin < 3
        camnum = 1;    
    end
    if nargin < 4
        dsp = 0;    
    end
    
    
    %------- get parameters for reconstruction-------
    org_x = x; % store untransformed picture coordinates
    R1 = getRotationMatrix(ste.leftCam,ste.leftParam); %rotation matrix for left camera
    R2 = getRotationMatrix(ste.rightCam,ste.rightParam); %rotation matrix for right camera
    X01 = getProjectioncenter(ste.leftCam,ste.leftParam)';% projection center for left camera
    X02 = getProjectioncenter(ste.rightCam,ste.rightParam)';% projection center for right camera
    b = X02-X01; % baseline
    vx1 = ste.leftCam.vx; vy1 = ste.leftCam.vy; % translation coefficients
    vx2 = ste.rightCam.vx; vy2 = ste.rightCam.vy; % (sensor --> picture)
    px1 = ste.leftCam.pxSize; px2 = ste.rightCam.pxSize;
    c1 = getCameraConstant(ste.leftCam,ste.leftParam);% camera constant for left camera
    c2 = getCameraConstant(ste.rightCam,ste.rightParam);% camera constant for right camera
    
    
    %---- transform sensor to picture coordinates ------
    x = x - [vx1,vy1,vx2,vy2]; % translate
    x = x.*[px1,px1,px2,px2]; % scale
    x1 = [x(1:2),-c1]'; x2 = [x(3:4),-c2]'; % split into left and right camerasystem coordinates
   
    %---- transform picture coordinates to world coordinate system
    x1 = X01 + R1*x1; x2 = X02 + R2*x2;
    
    %--- compute scaling coefficient ---
    tmp =  (x1(1)-X01(1))*(x2(3)-X02(3)) - (x2(1)-X02(1))*(x1(3) - X01(3)) ;
    lambda = ( b(1)*(x2(3)-X02(3)) - b(3)*(x2(1)-X02(1))) /tmp;
    mu = ( b(1)*(x1(3)-X01(3)) - b(3)*(x1(1)-X01(1))) /tmp;
        
    
    %--- computing object coordinates -----
    if camnum == 1
        X(1) = X01(1)+lambda*(x1(1)-X01(1));
        X(3) = X01(3)+lambda*(x1(3)-X01(3));
        Y1 = X01(2)+lambda*(x1(2)-X01(2)); Y2 = X02(2)+mu*(x2(2)-X02(2));
        X(2) = (Y1+Y2)/2; 
        pY = Y2-Y1;
    elseif camnum == 2
        X(1) = X02(1)+mu*(x2(1)-X02(1));
        X(3) = X02(3)+mu*(x2(3)-X02(3));
        Y1 = X01(2)+lambda*(x1(2)-X01(2)); Y2 = X02(2)+mu*(x2(2)-X02(2));
        X(2) = (Y1+Y2)/2; 
        pY = Y2-Y1;
    else
        disp(['Do not know camera #' num2str(camnum)])
    end
    
    
    
    [X C]= improveSpatialCoordinate(ste,org_x,X);
    [u,v] = eig(C); v = diag(v);  % <-- det(u) = -1 shouldn't it be a rotation matrix
    
    if (dsp == 1)
        plot3(X(1),X(2),X(3),'.g');
        hold on,grid on
        line([X(1),X(1)],[Y1,Y2],[X(3),X(3)]);
        pause(0.05);    
    end