Vision (3) Blepo

Vision (3) Blepo

Translate MATLAB to C program has a good way, downloaded from the Internet a function library Blepo, converted to C is almost a row to a row, OpenCV often involved in the memory application and release here is no tube. Happy!
Let's see how this program compares the differences.
of 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-a scale factor, however.
sig = Sign (B (3,3));
B=b*sig;
S=s*sig;

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

If Fsign*b (< 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
% 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 then 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

For 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,&AMP;U,&AMP;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,&AMP;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 (<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 &AMP;PN2
/*double d1=0,double d2=0*/)
{
Matdbl a1,r1,t1;
Matdbl a2,r2,t2;
Art (PO1,&AMP;A1,&AMP;R1,&AMP;T1);
Art (PO2,&AMP;A2,&AMP;R2,&AMP;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");
}

Vision (3) Blepo