视觉(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 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
c++的:
//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");
}