project.m

三维重建

function X = project(ste,Y,distort)
%
%
%   X = PROJECT(ste,Y,distort)
%
%   Project the 3D points stored as rows in Y woth the stereocamera's two
%   camera models. If distort equals 0, no modeled noise is added.
%
%
%
%
    if nargin < 3
        distort = 1;    
    end
    
    if (distort == 0)
        disp('Switching off distortion...')
        oldlP = ste.leftParam; 
        oldrP =  ste.rightParam;
        ste.leftParam = expandParameterArray(ste.leftCam,ste.leftParam);
        ste.leftParam(11:42) = ste.leftParam(11:42)*0;
        ste.rightParam = expandParameterArray(ste.rightCam,ste.rightParam);
        ste.rightParam(11:42) = ste.rightParam(11:42)*0;
        ste.leftParam
        ste.rightParam
    end
    
    X = [];
    for i = 1:size(Y,1)
        [xl,yl] = projectPoint(ste.leftCam,Y(i,:),ste.leftParam,0);
        [xr,yr] = projectPoint(ste.rightCam,Y(i,:),ste.rightParam,0);
        X = [X; xl,yl,xr,yr];
%         pause;    
    end
  
     if (distort == 0)
        disp('Restoring old parameters...') 
        ste.leftParam = oldlP;
        ste.rightParam = oldrP;

     end