getobjectcoordinate.m

三维重建

function r =getObjectCoordinate(ste, x)
%
%
%   r = GETOBJECTCOORDINATE(ste,x)
%
%
%   Returns the 3D point corresponding to the four image coordinates (in
%   px) of x. Te format is
%
%   x = [x_left,y_left,x_right,y_right] with x* in [1,640] and y* in [1,480]
%



% % %---------remove distortion from points------


[xwdl ywdl] = removeDistortionForPoint(ste.leftCam,x(1:2),ste.leftParam);
[xwdr ywdr] = removeDistortionForPoint(ste.rightCam,x(3:4),ste.rightParam);

x = [xwdl,ywdl,xwdr,ywdr];

if length(ste.leftParam) < ste.leftCam.maxParam
    ste.leftParam = expandParameterArray(ste.leftCam,ste.leftParam);
end
if length(ste.rightParam) < ste.rightCam.maxParam
    ste.rightParam = expandParameterArray(ste.rightCam,ste.rightParam);
end
 
%--------------get parameters-------------------
fr = ste.rightParam(1); % focal length is c
fl = ste.leftParam(1);  % focal length is c
tx = 1/(ste.vx);        % scaling factor for tschebyscheff polynomials
ty = 1/(ste.vy);        % scaling factor for tschebyscheff polynomials

xhl = ste.leftParam(3); % x-main point for left camera 
yhl = ste.leftParam(4); % y-main point for left camera 
xhr = ste.rightParam(3);% x-main point for right camera
yhr = ste.rightParam(4);% y-main point for right camera

X0l = ste.leftParam(5); % X-value of projection center of left camera
Y0l = ste.leftParam(6); % Y-value of projection center of left camera
Z0l = ste.leftParam(7); % Z-value of projection center of left camera

[vxl vyl] = getPictureToSensorTranslation(ste.leftCam); % translation parameters to middle of the picture (left)
[vxr vyr] = getPictureToSensorTranslation(ste.rightCam);% translation parameters to middle of the picture (right)

%---------convert to picture coordinate system (pretty sure to be correct)----------
xpl = (x(1) - vxl - xhl)*(getPxSize(ste.leftCam));%*sl;
ypl = (x(2) - vyl - yhl)*(getPxSize(ste.leftCam));
xpr = (x(3) - vxr - xhr)*(getPxSize(ste.rightCam));%*sr;
ypr = (x(4) - vyr - yhr)*(getPxSize(ste.rightCam));

   
%-----------compute rotation matrix (relative orientation between cameras)-(debugged)--------

  % Tranlsation from left to right
xoff = (ste.leftParam(5) - ste.rightParam(5));
yoff = (ste.leftParam(6) - ste.rightParam(6));
zoff = (ste.leftParam(7) - ste.rightParam(7));

Rr = getRotationMatrix(ste.rightCam,ste.rightParam);
Rl = getRotationMatrix(ste.leftCam,ste.leftParam);
X0L = [ste.leftParam(5) ste.leftParam(6) ste.leftParam(7)]';
X0R = [ste.rightParam(5) ste.rightParam(6) ste.rightParam(7)]';
            

X0L1 = X0L(1); X0L2 = X0L(2); X0L3 = X0L(3);
X0R1 = X0R(1); X0R2 = X0R(2); X0R3 = X0R(3);

rl11 = Rl(1,1); rl12 = Rl(1,2); rl13 = Rl(1,3);
rl21 = Rl(2,1); rl22 = Rl(2,2); rl23 = Rl(2,3);
rl31 = Rl(3,1); rl32 = Rl(3,2); rl33 = Rl(3,3);

rr11 = Rr(1,1); rr12 = Rr(1,2); rr13 = Rr(1,3);
rr21 = Rr(2,1); rr22 = Rr(2,2); rr23 = Rr(2,3);
rr31 = Rr(3,1); rr32 = Rr(3,2); rr33 = Rr(3,3);

sl = ste.leftParam(2); 
sr = ste.rightParam(2);


r(1) = (fr*rr22*sr*rr31*xpl*rl33*X0L3+fr*rr22*sr*rr31*xpl*rl23*X0L2+fr*rr32*X0R3*sr*rr21*sl*fl*rl31+rr32*sl*fl*rl21*xpr*rr23*X0R2+ypr*rr33*xpl*rl23*sr*rr21*X0R2-rr32*xpr*rr23*sl*fl*rl11*X0L1+rr12*X0R1*xpr*rr23*sl*fl*rl31-rr12*X0R1*xpl*rl23*xpr*rr33+rr12*X0R1*xpr*rr23*xpl*rl33-rr12*X0R1*sl*fl*rl21*xpr*rr33-rr32*xpr*rr23*sl*fl*rl21*X0L2-rr32*xpr*rr23*xpl*rl33*X0L3-rr32*xpr*rr23*xpl*rl13*X0L1-rr32*xpr*rr23*xpl*rl23*X0L2+rr22*xpr*rr33*xpl*rl33*X0L3+rr32*xpl*rl23*xpr*rr23*X0R2+rr22*xpr*rr33*xpl*rl13*X0L1+rr22*xpr*rr33*sl*fl*rl21*X0L2+ypr*rr23*sr*rr31*xpl*rl33*X0L3+ypr*rr23*sr*rr31*sl*fl*rl21*X0L2+ypr*rr23*sr*rr31*sl*fl*rl31*X0L3-ypr*rr33*sr*rr21*xpl*rl13*X0L1-ypr*rr33*sr*rr21*xpl*rl23*X0L2-ypr*rr33*sr*rr21*sl*fl*rl11*X0L1-rr32*xpr*rr23*sl*fl*rl31*X0L3+ypr*rr23*sr*rr31*sl*fl*rl11*X0L1-ypr*rr23*sr*rr31*X0R3*sl*fl*rl31-ypr*rr33*sr*rr21*xpl*rl33*X0L3-ypr*rr33*sr*rr21*sl*fl*rl21*X0L2-ypr*rr33*sr*rr21*sl*fl*rl31*X0L3+rr22*xpr*rr33*xpl*rl23*X0L2-ypr*rr13*X0R1*sl*fl*rl21*sr*rr31+rr22*xpr*rr33*sl*fl*rl31*X0L3+X0R1*xpl*rl23*xpr*rr13*rr32-rr22*xpr*rr33*X0R3*sl*fl*rl31-rr22*X0R2*sl*fl*rl21*xpr*rr33+X0R1*sl*fl*rl21*sr*rr11*ypr*rr33+rr32*X0R3*xpr*rr23*sl*fl*rl31-rr22*xpr*rr33*X0R3*xpl*rl33+rr22*xpr*rr33*sl*fl*rl11*X0L1+rr32*X0R3*xpr*rr23*xpl*rl33-X0R1*xpr*rr13*rr22*xpl*rl33-X0R1*xpr*rr13*rr22*sl*fl*rl31+X0R1*sl*fl*rl21*xpr*rr13*rr32-X0R1*sr*rr11*ypr*rr23*xpl*rl33+X0R1*xpl*rl23*sr*rr11*ypr*rr33-X0R1*sr*rr11*ypr*rr23*sl*fl*rl31-ypr*rr13*X0R1*xpl*rl23*sr*rr31+ypr*rr13*X0R1*sr*rr21*sl*fl*rl31+ypr*rr33*X0R3*sr*rr21*xpl*rl33+ypr*rr33*X0R3*sr*rr21*sl*fl*rl31-rr22*X0R2*xpl*rl23*xpr*rr33-ypr*rr23*X0R2*xpl*rl23*sr*rr31-ypr*rr23*X0R2*sl*fl*rl21*sr*rr31+ypr*rr23*sr*rr31*xpl*rl23*X0L2-ypr*rr23*sr*rr31*X0R3*xpl*rl33+ypr*rr23*sr*rr31*xpl*rl13*X0L1+ypr*rr33*sl*fl*rl21*sr*rr21*X0R2+ypr*rr13*X0R1*sr*rr21*xpl*rl33+fr*rr22*sr*rr31*sl*fl*rl31*X0L3+fr*rr32*xpl*rl23*sr*rr21*X0R2+fr*rr32*sl*fl*rl21*sr*rr21*X0R2-fr*X0R1*sr*rr11*rr22*xpl*rl33+fr*rr22*sr*rr31*xpl*rl13*X0L1+fr*X0R1*xpl*rl23*sr*rr11*rr32-fr*rr22*X0R2*xpl*rl23*sr*rr31+fr*rr22*sr*rr31*sl*fl*rl21*X0L2-fr*rr22*sr*rr31*X0R3*xpl*rl33-fr*rr32*sr*rr21*xpl*rl23*X0L2-fr*rr32*sr*rr21*xpl*rl13*X0L1-fr*rr32*sr*rr21*xpl*rl33*X0L3+fr*rr32*X0R3*sr*rr21*xpl*rl33-fr*rr22*X0R2*sl*fl*rl21*sr*rr31+fr*rr22*sr*rr31*sl*fl*rl11*X0L1-fr*X0R1*sr*rr11*rr22*sl*fl*rl31+fr*rr12*X0R1*sr*rr21*sl*fl*rl31-fr*rr12*X0R1*sl*fl*rl21*sr*rr31+fr*rr12*X0R1*sr*rr21*xpl*rl33-fr*rr22*sr*rr31*X0R3*sl*fl*rl31-fr*rr32*sr*rr21*sl*fl*rl31*X0L3-fr*rr12*X0R1*xpl*rl23*sr*rr31-fr*rr32*sr*rr21*sl*fl*rl21*X0L2-fr*rr32*sr*rr21*sl*fl*rl11*X0L1+fr*X0R1*sl*fl*rl21*sr*rr11*rr32)/(-xpr*rr13*rr22*sl*fl*rl31+sr*rr21*ypr*rr13*sl*fl*rl31-sr*rr21*sl*fl*rl11*ypr*rr33+sl*fl*rl11*rr22*xpr*rr33+sl*fl*rl21*xpr*rr13*rr32-ypr*rr13*xpl*rl23*sr*rr31+fr*xpl*rl13*rr22*sr*rr31+fr*xpl*rl23*sr*rr11*rr32-fr*sr*rr21*sl*fl*rl11*rr32+fr*rr12*sr*rr21*sl*fl*rl31-fr*rr12*sl*fl*rl21*sr*rr31-fr*sr*rr21*xpl*rl13*rr32+xpl*rl23*sr*rr11*ypr*rr33-sr*rr11*ypr*rr23*xpl*rl33-fr*rr12*xpl*rl23*sr*rr31-rr12*sl*fl*rl21*xpr*rr33+sr*rr21*ypr*rr13*xpl*rl33+rr12*xpr*rr23*sl*fl*rl31+sl*fl*rl21*sr*rr11*ypr*rr33-sl*fl*rl21*ypr*rr13*sr*rr31+xpl*rl13*ypr*rr23*sr*rr31-sr*rr11*ypr*rr23*sl*fl*rl31-xpr*rr13*rr22*xpl*rl33+rr12*xpr*rr23*xpl*rl33-rr12*xpl*rl23*xpr*rr33+xpl*rl23*xpr*rr13*rr32-xpr*rr23*xpl*rl13*rr32+xpl*rl13*rr22*xpr*rr33+fr*sl*fl*rl11*rr22*sr*rr31+fr*rr12*sr*rr21*xpl*rl33-fr*sr*rr11*rr22*sl*fl*rl31-fr*sr*rr11*rr22*xpl*rl33+fr*sl*fl*rl21*sr*rr11*rr32+sl*fl*rl11*ypr*rr23*sr*rr31-sr*rr21*xpl*rl13*ypr*rr33-xpr*rr23*sl*fl*rl11*rr32);
r(2) = (-rr12*xpr*rr33*xpl*rl33*X0L3+rr12*xpr*rr33*X0R3*xpl*rl33+X0R2*sl*fl*rl11*rr22*xpr*rr33+rr12*X0R2*xpr*rr23*xpl*rl33-rr12*xpr*rr33*xpl*rl23*X0L2-rr12*xpr*rr33*xpl*rl13*X0L1-xpr*rr13*rr32*X0R3*xpl*rl33-X0R2*xpr*rr23*xpl*rl13*rr32-xpr*rr13*rr32*xpl*rl13*X0R1+xpr*rr13*rr32*xpl*rl23*X0L2-X0R2*xpr*rr13*rr22*xpl*rl33+xpr*rr13*rr32*xpl*rl33*X0L3+xpr*rr13*rr32*xpl*rl13*X0L1+X0R2*xpl*rl13*rr22*xpr*rr33+rr12*xpr*rr33*xpl*rl13*X0R1+sr*fr*rr11*rr32*sl*fl*rl11*X0L1+sr*rr11*ypr*rr33*sl*fl*rl31*X0L3+sr*rr21*X0R2*ypr*rr13*sl*fl*rl31-sr*rr21*X0R2*sl*fl*rl11*ypr*rr33-sr*X0R2*rr11*ypr*rr23*sl*fl*rl31+sr*rr11*ypr*rr33*sl*fl*rl21*X0L2-sr*rr11*ypr*rr33*X0R3*sl*fl*rl31+sr*fr*rr11*rr32*sl*fl*rl21*X0L2+sr*fr*rr11*rr32*sl*fl*rl31*X0L3+sr*fr*rr11*rr32*xpl*rl13*X0L1+sr*rr31*sl*fl*rl11*ypr*rr13*X0R1-sr*rr31*ypr*rr13*sl*fl*rl11*X0L1-sr*rr31*ypr*rr13*sl*fl*rl21*X0L2-sr*rr31*ypr*rr13*sl*fl*rl31*X0L3+sr*rr11*ypr*rr33*sl*fl*rl11*X0L1+sr*rr31*X0R3*ypr*rr13*sl*fl*rl31+xpr*rr13*rr32*sl*fl*rl11*X0L1-sr*rr11*ypr*rr33*sl*fl*rl11*X0R1-sr*fr*rr21*X0R2*sl*fl*rl11*rr32-sr*fr*rr11*rr32*X0R3*sl*fl*rl31-sr*fr*X0R2*rr11*rr22*sl*fl*rl31-sr*fr*rr12*rr31*sl*fl*rl31*X0L3+sr*fr*rr12*rr21*X0R2*sl*fl*rl31-sr*fr*rr12*rr31*sl*fl*rl11*X0L1+sr*fr*rr12*rr31*X0R3*sl*fl*rl31+sr*fr*rr12*rr31*sl*fl*rl11*X0R1+sr*fr*X0R2*sl*fl*rl11*rr22*rr31-sr*fr*rr12*rr31*sl*fl*rl21*X0L2-sr*fr*rr11*rr32*sl*fl*rl11*X0R1+sr*X0R2*sl*fl*rl11*ypr*rr23*rr31+xpr*rr13*rr32*sl*fl*rl21*X0L2+xpr*rr13*rr32*sl*fl*rl31*X0L3-X0R2*xpr*rr13*rr22*sl*fl*rl31-rr12*xpr*rr33*sl*fl*rl21*X0L2-xpr*rr13*rr32*sl*fl*rl11*X0R1-xpr*rr13*rr32*X0R3*sl*fl*rl31-X0R2*xpr*rr23*sl*fl*rl11*rr32-rr12*xpr*rr33*sl*fl*rl31*X0L3+rr12*X0R2*xpr*rr23*sl*fl*rl31+rr12*xpr*rr33*X0R3*sl*fl*rl31+rr12*xpr*rr33*sl*fl*rl11*X0R1-rr12*xpr*rr33*sl*fl*rl11*X0L1-sr*fr*X0R2*rr11*rr22*xpl*rl33+sr*fr*X0R2*xpl*rl13*rr22*rr31+sr*fr*rr12*rr31*xpl*rl13*X0R1-sr*fr*rr21*X0R2*xpl*rl13*rr32+sr*rr31*X0R3*ypr*rr13*xpl*rl33-sr*fr*rr12*rr31*xpl*rl13*X0L1+sr*fr*rr11*rr32*xpl*rl33*X0L3-sr*rr21*X0R2*xpl*rl13*ypr*rr33+sr*rr21*X0R2*ypr*rr13*xpl*rl33-sr*rr11*ypr*rr33*xpl*rl13*X0R1+sr*fr*rr11*rr32*xpl*rl23*X0L2-sr*fr*rr12*rr31*xpl*rl33*X0L3-sr*X0R2*rr11*ypr*rr23*xpl*rl33-sr*rr31*ypr*rr13*xpl*rl23*X0L2+sr*rr11*ypr*rr33*xpl*rl13*X0L1-sr*fr*rr11*rr32*xpl*rl13*X0R1-sr*rr31*ypr*rr13*xpl*rl13*X0L1+sr*rr11*ypr*rr33*xpl*rl33*X0L3+sr*fr*rr12*rr31*X0R3*xpl*rl33-sr*fr*rr12*rr31*xpl*rl23*X0L2-sr*rr11*ypr*rr33*X0R3*xpl*rl33+sr*rr31*xpl*rl13*ypr*rr13*X0R1-sr*fr*rr11*rr32*X0R3*xpl*rl33-sr*rr31*ypr*rr13*xpl*rl33*X0L3+sr*fr*rr12*rr21*X0R2*xpl*rl33+sr*rr11*ypr*rr33*xpl*rl23*X0L2+sr*X0R2*xpl*rl13*ypr*rr23*rr31)/(-xpr*rr13*rr22*sl*fl*rl31+sr*rr21*ypr*rr13*sl*fl*rl31-sr*rr21*sl*fl*rl11*ypr*rr33+sl*fl*rl11*rr22*xpr*rr33+sl*fl*rl21*xpr*rr13*rr32-ypr*rr13*xpl*rl23*sr*rr31+fr*xpl*rl13*rr22*sr*rr31+fr*xpl*rl23*sr*rr11*rr32-fr*sr*rr21*sl*fl*rl11*rr32+fr*rr12*sr*rr21*sl*fl*rl31-fr*rr12*sl*fl*rl21*sr*rr31-fr*sr*rr21*xpl*rl13*rr32+xpl*rl23*sr*rr11*ypr*rr33-sr*rr11*ypr*rr23*xpl*rl33-fr*rr12*xpl*rl23*sr*rr31-rr12*sl*fl*rl21*xpr*rr33+sr*rr21*ypr*rr13*xpl*rl33+rr12*xpr*rr23*sl*fl*rl31+sl*fl*rl21*sr*rr11*ypr*rr33-sl*fl*rl21*ypr*rr13*sr*rr31+xpl*rl13*ypr*rr23*sr*rr31-sr*rr11*ypr*rr23*sl*fl*rl31-xpr*rr13*rr22*xpl*rl33+rr12*xpr*rr23*xpl*rl33-rr12*xpl*rl23*xpr*rr33+xpl*rl23*xpr*rr13*rr32-xpr*rr23*xpl*rl13*rr32+xpl*rl13*rr22*xpr*rr33+fr*sl*fl*rl11*rr22*sr*rr31+fr*rr12*sr*rr21*xpl*rl33-fr*sr*rr11*rr22*sl*fl*rl31-fr*sr*rr11*rr22*xpl*rl33+fr*sl*fl*rl21*sr*rr11*rr32+sl*fl*rl11*ypr*rr23*sr*rr31-sr*rr21*xpl*rl13*ypr*rr33-xpr*rr23*sl*fl*rl11*rr32);
r(3) = -(sr*rr21*sl*fl*rl11*ypr*rr13*X0R1-sr*rr21*ypr*rr13*xpl*rl13*X0L1+sr*rr11*ypr*rr23*sl*fl*rl21*X0L2+sr*rr11*ypr*rr23*sl*fl*rl11*X0L1+xpr*rr13*rr22*xpl*rl33*X0L3-xpr*rr13*rr22*xpl*rl13*X0R1+ypr*rr13*xpl*rl23*sr*rr31*X0R3-sr*rr21*ypr*rr13*xpl*rl33*X0L3+xpr*rr13*rr22*sl*fl*rl31*X0L3+xpr*rr13*rr22*sl*fl*rl21*X0L2+xpr*rr13*rr22*sl*fl*rl11*X0L1+xpr*rr23*sl*fl*rl11*rr32*X0R3-sr*rr21*ypr*rr13*sl*fl*rl11*X0L1-sr*rr21*ypr*rr13*xpl*rl23*X0L2-sr*rr21*ypr*rr13*sl*fl*rl31*X0L3-sr*rr21*ypr*rr13*sl*fl*rl21*X0L2+sr*rr21*xpl*rl13*ypr*rr33*X0R3+sr*rr21*xpl*rl13*ypr*rr13*X0R1+xpr*rr23*xpl*rl13*rr32*X0R3+sr*rr21*sl*fl*rl11*ypr*rr33*X0R3+xpr*rr13*rr22*xpl*rl23*X0L2-sr*rr11*ypr*rr23*sl*fl*rl11*X0R1+sr*rr11*ypr*rr23*sl*fl*rl31*X0L3-sr*rr11*ypr*rr23*xpl*rl13*X0R1+rr12*sl*fl*rl21*xpr*rr23*X0R2+rr12*sl*fl*rl21*xpr*rr33*X0R3+rr12*xpl*rl23*xpr*rr23*X0R2+rr12*xpr*rr23*sl*fl*rl11*X0R1+rr12*xpr*rr23*xpl*rl13*X0R1-rr12*xpr*rr23*sl*fl*rl31*X0L3-rr12*xpr*rr23*sl*fl*rl21*X0L2-rr12*xpr*rr23*xpl*rl33*X0L3-rr12*xpr*rr23*xpl*rl13*X0L1+rr12*xpl*rl23*xpr*rr33*X0R3-rr12*xpr*rr23*xpl*rl23*X0L2-xpr*rr13*rr22*sl*fl*rl11*X0R1-sl*fl*rl21*sr*rr11*ypr*rr23*X0R2-xpl*rl23*xpr*rr13*rr22*X0R2+sl*fl*rl21*ypr*rr13*sr*rr31*X0R3-xpl*rl23*sr*rr11*ypr*rr23*X0R2-xpl*rl23*xpr*rr13*rr32*X0R3-xpl*rl23*sr*rr11*ypr*rr33*X0R3-sl*fl*rl11*ypr*rr23*sr*rr31*X0R3+ypr*rr13*xpl*rl23*sr*rr21*X0R2-xpl*rl13*rr22*xpr*rr33*X0R3-xpl*rl13*ypr*rr23*sr*rr31*X0R3+sr*rr11*ypr*rr23*xpl*rl13*X0L1+sr*rr11*ypr*rr23*xpl*rl33*X0L3+sr*rr11*ypr*rr23*xpl*rl23*X0L2+fr*rr12*sl*fl*rl21*sr*rr31*X0R3-fr*rr12*sr*rr21*sl*fl*rl11*X0L1-fr*rr12*sr*rr21*xpl*rl23*X0L2+fr*rr12*sr*rr21*sl*fl*rl11*X0R1+fr*rr12*sl*fl*rl21*sr*rr21*X0R2-fr*rr12*sr*rr21*xpl*rl13*X0L1-fr*sl*fl*rl11*rr22*sr*rr31*X0R3-fr*sl*fl*rl21*sr*rr11*rr22*X0R2-fr*xpl*rl23*sr*rr11*rr22*X0R2-fr*rr12*sr*rr21*xpl*rl33*X0L3-fr*rr12*sr*rr21*sl*fl*rl21*X0L2+fr*rr12*sr*rr21*xpl*rl13*X0R1+fr*sr*rr11*rr22*xpl*rl33*X0L3+fr*rr12*xpl*rl23*sr*rr31*X0R3-fr*rr12*sr*rr21*sl*fl*rl31*X0L3+fr*sr*rr11*rr22*xpl*rl23*X0L2-fr*sr*rr11*rr22*sl*fl*rl11*X0R1+fr*sr*rr11*rr22*sl*fl*rl21*X0L2+fr*sr*rr11*rr22*sl*fl*rl31*X0L3-fr*sl*fl*rl21*sr*rr11*rr32*X0R3+fr*sr*rr21*sl*fl*rl11*rr32*X0R3-fr*xpl*rl13*rr22*sr*rr31*X0R3+fr*sr*rr11*rr22*sl*fl*rl11*X0L1+fr*rr12*xpl*rl23*sr*rr21*X0R2-fr*xpl*rl23*sr*rr11*rr32*X0R3-sl*fl*rl11*rr22*xpr*rr33*X0R3-fr*sr*rr11*rr22*xpl*rl13*X0R1+fr*sr*rr21*xpl*rl13*rr32*X0R3+fr*sr*rr11*rr22*xpl*rl13*X0L1-sl*fl*rl21*xpr*rr13*rr32*X0R3+sl*fl*rl21*ypr*rr13*sr*rr21*X0R2-sl*fl*rl21*xpr*rr13*rr22*X0R2-sl*fl*rl21*sr*rr11*ypr*rr33*X0R3+xpr*rr13*rr22*xpl*rl13*X0L1-rr12*xpr*rr23*sl*fl*rl11*X0L1)/(-xpr*rr13*rr22*sl*fl*rl31+sr*rr21*ypr*rr13*sl*fl*rl31-sr*rr21*sl*fl*rl11*ypr*rr33+sl*fl*rl11*rr22*xpr*rr33+sl*fl*rl21*xpr*rr13*rr32-ypr*rr13*xpl*rl23*sr*rr31+fr*xpl*rl13*rr22*sr*rr31+fr*xpl*rl23*sr*rr11*rr32-fr*sr*rr21*sl*fl*rl11*rr32+fr*rr12*sr*rr21*sl*fl*rl31-fr*rr12*sl*fl*rl21*sr*rr31-fr*sr*rr21*xpl*rl13*rr32+xpl*rl23*sr*rr11*ypr*rr33-sr*rr11*ypr*rr23*xpl*rl33-fr*rr12*xpl*rl23*sr*rr31-rr12*sl*fl*rl21*xpr*rr33+sr*rr21*ypr*rr13*xpl*rl33+rr12*xpr*rr23*sl*fl*rl31+sl*fl*rl21*sr*rr11*ypr*rr33-sl*fl*rl21*ypr*rr13*sr*rr31+xpl*rl13*ypr*rr23*sr*rr31-sr*rr11*ypr*rr23*sl*fl*rl31-xpr*rr13*rr22*xpl*rl33+rr12*xpr*rr23*xpl*rl33-rr12*xpl*rl23*xpr*rr33+xpl*rl23*xpr*rr13*rr32-xpr*rr23*xpl*rl13*rr32+xpl*rl13*rr22*xpr*rr33+fr*sl*fl*rl11*rr22*sr*rr31+fr*rr12*sr*rr21*xpl*rl33-fr*sr*rr11*rr22*sl*fl*rl31-fr*sr*rr11*rr22*xpl*rl33+fr*sl*fl*rl21*sr*rr11*rr32+sl*fl*rl11*ypr*rr23*sr*rr31-sr*rr21*xpl*rl13*ypr*rr33-xpr*rr23*sl*fl*rl11*rr32);