stereocameramodel.m

三维重建

function ste = StereoCameraModel(pt,lparam,rparam,flags,restrictions,X,Y,spatial_s)
%
%
%   STEREOCAMERAMODEL   ste = StereoCameraModel(X,Y,angles, lProjCen, rProjCen, flags, restrictions, X,Y[, spatial_std]) 
%
%   Models a stereo camera consiting of two single camera models (so
%   @CameraModel is required). 
%
%
%   Parameters: 
%       Parameters:     
%           ANGLES contains the preestimates of the cameras' orientation in
%           space.
%                       angles(1) is the yaw angle
%                       angles(2) is the pitch angle
%
%           LPROJCEN and RPROJCEN denote the prestimates of the left
%           camera's resp. the right camera's projection center.
%
%           FLAGS and RESTRICTIONS contain the falgs and restrictions for
%           the single camera models (see CAMERAMODEL for further information on FLAGS and RESTRICTIONS)
%
%           X,Y are the know points for calibration, where X contains the image coordinate pairs as row vectors and
%           Y contains the respective spatial coordinates as row vectors.
%
%           If you specify SPATIAL_STD the spatial coordinates will be
%           considered as estimations of the real position and improved
%           when fitting the StereoCameraModel to the data. SPATIAL_STD
%           then denotes the standard deviation of spatial coordinates. The
%           imroved Ys will be stored in ste.improved_Y_left and
%           ste.improved_Y_right
%       
%

if nargin < 8
    isc = 0;
    spatial_s = 0;        
else
    isc = 1;    
end

    ste.nEst = 36;
    ste.d = 3;
    ste.pxSize = 0.0083;
    ste.vx = 320*ste.pxSize; 
    ste.vy = 240*ste.pxSize;
    ste.leftCam = CameraModel([pt lparam]);
    ste.rightCam = CameraModel([pt rparam]);
     ste.rightCam = setFlag(ste.rightCam, [1:42],flags);
    ste.leftCam = setFlag(ste.leftCam, [1:42],flags);
     ste.rightCam = setRestrictions(ste.rightCam,restrictions);
    ste.leftCam = setRestrictions(ste.leftCam,restrictions);
    
    spatial_s = ones(3,1)*spatial_s;
    ste.leftCam = setAprioriSpatialStd(ste.leftCam,spatial_s);
    ste.rightCam = setAprioriSpatialStd(ste.rightCam,spatial_s);
    
    
    [Pl Yimp_left] = fit(ste.leftCam,X(:,[1:2]),Y,isc);
    ste.leftParam = Pl;
    ste.improved_Y_left = Yimp_left;
    
    [Pr Yimp_right] = fit(ste.rightCam,X(:,[3:4]),Y,isc);
    ste.rightParam = Pr;
    ste.improved_Y_right = Yimp_right;
    ste = class(ste,'StereoCameraModel');