视觉（3）blepo

function [A,R,t]=art(P,fsign)
%ART  Factorize camera matrix into intrinsic and extrinsic matrices
%
%   [A,R,t] = art(P,fsign)  factorize the projection matrix P 
%   as P=A*[R;t] and enforce the sign of the focal lenght to be fsign.
%   By defaukt fsign=1.

% Author: A. Fusiello, 1999
%
% fsign tells the position of the image plane wrt the focal plane. If it is
% negative the image plane is behind the focal plane.



% by default assume POSITIVE focal lenght
if nargin == 1
    fsign = 1;
end

s = P(1:3,4);
Q = inv(P(1:3, 1:3));
[U,B] = qr(Q);

% fix the sign of B(3,3). This can possibly change the sign of the resulting matrix,
% which is defined up to a scale factor, however.
sig = sign(B(3,3));
B=B*sig;
s=s*sig;

% if the sign of the focal lenght is not the required one, 
% change it, and change the rotation accordingly.

if fsign*B(1,1) < 0
     E= [-1     0     0
         0    1     0
         0     0     1];
     B = E*B;
     U = U*E;
 end
 
 if fsign*B(2,2) < 0
     E= [1     0     0
         0    -1     0
         0     0     1];
     B = E*B;
     U = U*E;
 end
 
% if U is not a rotation, fix the sign. This can possibly change the sign
% of the resulting matrix, which is defined up to a scale factor, however.
if det(U)< 0 
    U = -U;
    s= - s;
end

  
% sanity check 
if (norm(Q-U*B)>1e-10) & (norm(Q+U*B)>1e-10) 
    error('Something wrong with the QR factorization.'); end

R = U';
t = B*s;
A = inv(B);
A = A ./A(3,3);


% sanity check 
if det(R) < 0 error('R is not a rotation matrix'); end
if A(3,3) < 0 error('Wrong sign of A(3,3)'); end
% this guarantee that the result *is* a factorization of the given P, up to a scale factor
W = A*[R,t];
if (rank([P(:), W(:)]) ~= 1 )
    error('Something wrong with the ART factorization.'); end

//P 3*4
//A,R 3*3,T 3*1
void art(MatDbl P,
         OUT MatDbl *A,OUT MatDbl *R,OUT MatDbl *T,
         int fsign=1)
{
    MatDbl s;
    MatDbl Q;

    s=P.GetSubMat(0,3,3,1);
    Q=Inverse(P.GetSubMat(0,0,3,3));
//      Display(s,"s");
//      Display(Q,"Q");

    MatDbl U,B;
    Qr(Q,&U,&B);
//     PrintF(U,"U");
//     PrintF(B,"B");
//     PrintF(U*B-Q,"U*B-Q");
//     PrintF(U*Transpose(U),"U*U'");
    
    if(B(2,2)<0)
    {
        Negate(B,&B);
        Negate(s,&s);
    }

    if(fsign*B(0,0)<0)
    {
        double E[9]={-1 ,0 ,0 ,0 ,1 ,0 ,0 ,0 ,1};
        MatDbl _E;
        _E.FromArray(E,3,3);
        B=_E*B;
        U=U*_E;
    }

    if(fsign*B(1,1)<0)
    {
        double E[9]={1 ,0 ,0 ,0 ,-1 ,0 ,0 ,0 ,1};
        MatDbl _E;
        _E.FromArray(E,3,3);
        B=_E*B;
        U=U*_E;
    }

    if(Determinant(U)<0)
    {
        Negate(U,&U);
        Negate(s,&s);
    }

//     if(Norm((Q-U*B))>1e-10 && Norm((Q+U*B).ToVector)>1e-10)
//         printf("'Something wrong with the QR factorization.'\n")    ;

    *R=Transpose(U);
    *T=B*s;
    *A=Inverse(B);
    *A= *A * (1.0 / (*A)(2,2));

//     PrintF(*A,"A");
//     PrintF(*R,"R");
//     PrintF(*T,"T");
//     PrintF((*A) * (*R),"A*R");
//     PrintF((*A) * (*T),"A*T");
}

//[T1,T2,Pn1,Pn2] = rectify(Po1,Po2,d1,d2)
void Rectify(MatDbl Po1,MatDbl Po2,
             OUT MatDbl &T1,OUT MatDbl &T2,OUT MatDbl &Pn1,OUT MatDbl &Pn2
             /*double d1=0,double d2=0*/)
{
    MatDbl A1,R1,t1;
    MatDbl A2,R2,t2;
    art(Po1,&A1,&R1,&t1);
    art(Po2,&A2,&R2,&t2);

    MatDbl c1,c2;
    c1=-Transpose(R1)*Inverse(A1)*Po1.GetSubMat(0,3,3,1);
    c2=-Transpose(R2)*Inverse(A2)*Po2.GetSubMat(0,3,3,1);
//     PrintF(c1,"c1");
//     PrintF(c2,"c2");

    MatDbl v1,v2,v3;
    v1=c2-c1;
    v2=CrossProduct(Transpose(R1.GetSubMat(2,0,1,3)),v1);
    v3=CrossProduct(v1,v2);
//      Display(v1,"v1");
//      Display(v2,"v2");
//     Display(v3,"v3");
    
    v1=(v1 * (1.0/Norm(v1)));
    v2=(v2 * (1.0/Norm(v2)));
    v3=(v3 * (1.0/Norm(v3)));
    double r[9]={v1(0),v1(1),v1(2),
        v2(0),v2(1),v2(2),
        v3(0),v3(1),v3(2)};
    MatDbl R;
    R.FromArray(r,3,3);
    //Display(R,"R");

    MatDbl An1=A2;
    An1(1,0)=0;
    MatDbl An2=An1;
    //PrintF(An1,"An1");

//    Display(An1 *(-1.0) * R * c1,"An1 *(-1.0) * R * c1");
    Pn1=An1 * PackX(R, (-1.0) * R * c1);
    Pn2=An2 * PackX(R, (-1.0) * R * c2);
    PrintF(Pn1,"Pn1");
    PrintF(Pn2,"Pn2");

    T1=Pn1.GetSubMat(0,0,3,3) * Inverse(Po1.GetSubMat(0,0,3,3));
    T2=Pn2.GetSubMat(0,0,3,3) * Inverse(Po2.GetSubMat(0,0,3,3));
    PrintF(T1,"T1");
    PrintF(T2,"T2");
}