视觉（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.' ") ;

*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");

}