视觉（3）blepo.
把matlab转成c程序有好办法了，从网上下载了一个函数库blepo,转换为c几乎是一行对一行，openCv经常涉及到的内存申请和释放这里都不用管。高兴！
看看这段程序比较一下差别
matlab的
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 lenghtif nargin == 1 fsign = 1;ends = 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.'); endR = 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'); endif 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 factorW = A*[R,t];if (rank([P(:), W(:)]) ~= 1 ) error('Something wrong with the ART factorization.'); end
c++的：
//P 3*4//A,R 3*3,T 3*1void 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");}