堆和栈

CvPoint3D32f ray_cross( CvMat* intr1, CvMat* rot1, CvMat* tran1, CvPoint2D32f pt1,

CvMat* intr2, CvMat* rot2, CvMat* tran2, CvPoint2D32f pt2, 

double* err, int* f1, int* f2)

{

// alloc data structure

CvMat* Q1 = cvCreateMat(3,3,CV_64F);

CvMat* Q2 = cvCreateMat(3,3,CV_64F);

CvMat* q1 = cvCreateMat(3,1,CV_64F);

CvMat* q2 = cvCreateMat(3,1,CV_64F);

CvMat* Q1inv = cvCreateMat(3,3,CV_64F);

CvMat* Q2inv = cvCreateMat(3,3,CV_64F);

CvMat* x1 = cvCreateMat(3,1,CV_64F); // image position

CvMat* x2 = cvCreateMat(3,1,CV_64F);

CvMat* v1 = cvCreateMat(3,1,CV_64F); // temporary data

CvMat* v2 = cvCreateMat(3,1,CV_64F);

CvMat* v3 = cvCreateMat(3,1,CV_64F);

CvMat* v4 = cvCreateMat(3,1,CV_64F);

CvMat* A = cvCreateMat(3,2,CV_64F); // for solving linear equation

CvMat* B = cvCreateMat(3,1,CV_64F);

CvMat* Lumda = cvCreateMat(2,1,CV_64F); // equation result

CvMat* XYZ1 = cvCreateMat(3,1,CV_64F); // 3D result

CvMat* XYZ2 = cvCreateMat(3,1,CV_64F);

CvMat* XYZ = cvCreateMat(3,1,CV_64F);

CvMat* ex1 = cvCreateMat(3,1,CV_64F);

CvMat* ex2 = cvCreateMat(3,1,CV_64F);

// prepare data

cvGEMM(intr1, tran1, 1, NULL, 0, q1);

cvGEMM(intr1, rot1, 1, NULL, 0, Q1);

cvGEMM(intr2, tran2, 1, NULL, 0, q2);

cvGEMM(intr2, rot2, 1, NULL, 0, Q2);

cvInvert(Q1, Q1inv);

cvInvert(Q2, Q2inv);

cvmSet(x1, 0,0, pt1.x); cvmSet(x1, 1,0, pt1.y); cvmSet(x1, 2,0, 1);

cvmSet(x2, 0,0, pt2.x); cvmSet(x2, 1,0, pt2.y); cvmSet(x2, 2,0, 1);

// process

cvGEMM(Q1inv, x1, 1, NULL, 0, v1);

cvGEMM(Q1inv, q1, 1, NULL, 0, v2);

cvGEMM(Q2inv, x2, 1, NULL, 0, v3);

cvGEMM(Q2inv, q2, 1, NULL, 0, v4);

A->data.db[0] = v1->data.db[0];

A->data.db[2] = v1->data.db[1];

A->data.db[4] = v1->data.db[2];

A->data.db[1] = -v3->data.db[0];

A->data.db[3] = -v3->data.db[1];

A->data.db[5] = -v3->data.db[2];

B->data.db[0] = v2->data.db[0]-v4->data.db[0];

B->data.db[1] = v2->data.db[1]-v4->data.db[1];

B->data.db[2] = v2->data.db[2]-v4->data.db[2];

cvSolve(A, B, Lumda, CV_SVD);

// return value

double s1, s2;

s1 = Lumda->data.db[0];

s2 = Lumda->data.db[1];

cvAddWeighted(v1, s1, v2, -1, 0, XYZ1);

cvAddWeighted(v3, s2, v4, -1, 0, XYZ2);

CvPoint3D32f pos3d;

XYZ->data.db[0] = pos3d.x = (XYZ1->data.db[0] + XYZ2->data.db[0]) / 2;

XYZ->data.db[1] = pos3d.y = (XYZ1->data.db[1] + XYZ2->data.db[1]) / 2;

XYZ->data.db[2] = pos3d.z = (XYZ1->data.db[2] + XYZ2->data.db[2]) / 2;

// error

cvGEMM(Q1, XYZ, 1, q1, 1, ex1);

cvGEMM(Q2, XYZ, 1, q2, 1, ex2);

CvPoint2D32f ept1, ept2;

ept1.x = ex1->data.db[0] / ex1->data.db[2];

ept1.y = ex1->data.db[1] / ex1->data.db[2];

ept2.x = ex2->data.db[0] / ex2->data.db[2];

ept2.y = ex2->data.db[1] / ex2->data.db[2];

*err = sqrt( (ept1.x-pt1.x)*(ept1.x-pt1.x) + (ept1.y-pt1.y)*(ept1.y-pt1.y) )

+ sqrt( (ept2.x-pt2.x)*(ept2.x-pt2.x) + (ept2.y-pt2.y)*(ept2.y-pt2.y) );

if (f1!=NULL && f2!=NULL)

{

*f1 = s1>0? 1:-1;

*f2 = s2>0? 1:-1;

}

// release matrix

cvReleaseMat(&Q1);

cvReleaseMat(&Q2);

cvReleaseMat(&q1);

cvReleaseMat(&q2);

cvReleaseMat(&Q1inv);

cvReleaseMat(&Q2inv);

cvReleaseMat(&x1);

cvReleaseMat(&x2);

cvReleaseMat(&v1);

cvReleaseMat(&v2);

cvReleaseMat(&v3);

cvReleaseMat(&v4);

cvReleaseMat(&A);

cvReleaseMat(&B);

cvReleaseMat(&Lumda);

cvReleaseMat(&XYZ1);

cvReleaseMat(&XYZ2);

cvReleaseMat(&XYZ);

cvReleaseMat(&ex1);

cvReleaseMat(&ex2);

return pos3d;

}

void draw_epiline(IplImage* left_image, IplImage* right_image, CvMat* F)

{

IplImage* img1 = cvCloneImage(left_image);

IplImage* img2 = cvCloneImage(right_image);

double epl[20][3];

CvMat linev = cvMat(20,3, CV_64F, epl);

double lpt[20][2];

CvMat lptv = cvMat(20,2, CV_64F, lpt);

int i;

int w = left_image->width;

int h = left_image->height;

for (i=0; i<20; ++i)

{

lpt[i][0] = w/2; 

lpt[i][1] = h*i/20;

}

cvComputeCorrespondEpilines(&lptv, 1, F, &linev);

for (i=0; i<20; ++i)

{

double y1 = -epl[i][2] / epl[i][1];

double y2 = -(epl[i][0]*w+epl[i][2]) / epl[i][1];

cvLine(img2, cvPoint(0, y1), cvPoint(w,y2), cvScalar(0,0,255), 3);

lpt[i][0] = 0;

lpt[i][1] = y1;

}

cvComputeCorrespondEpilines(&lptv, 2, F, &linev);

for (i=0; i<20; ++i)

{

double y1 = -epl[i][2] / epl[i][1];

double y2 = -(epl[i][0]*w+epl[i][2]) / epl[i][1];

cvLine(img1, cvPoint(0, y1), cvPoint(w,y2), cvScalar(0,0,255), 3);

}

cvSaveImage("left_epline.jpg", img1);

cvSaveImage("right_epline.jpg", img2);

cvReleaseImage(&img1);

cvReleaseImage(&img2);

}

void rotMatrix_to_quaternion(CvMat *src, CvMat *dst)

{

CvMat* rotvec = cvCreateMat(3,1, CV_64F);

cvRodrigues2(src, rotvec);

double th, u1, u2, u3;

u1 = cvmGet(rotvec,0,0);

u2 = cvmGet(rotvec,1,0);

u3 = cvmGet(rotvec,2,0);

th = sqrt(u1*u1+u2*u2+u3*u3);

// cvmSet(dst, 0, 0, cos(th));

// cvmSet(dst, 1, 0, sin(th)*u1);

// cvmSet(dst, 2, 0, sin(th)*u2);

// cvmSet(dst, 3, 0, sin(th)*u3);

if (th<1e-10)

{

cvSetZero(dst);

}

else

{

cvmSet(dst, 0, 0, sin(th/2)*u1/th);

cvmSet(dst, 1, 0, sin(th/2)*u2/th);

cvmSet(dst, 2, 0, sin(th/2)*u3/th);

}

cvReleaseMat(&rotvec);

}

void quaternion_to_rotMatrix(CvMat *src, CvMat *dst)

{

CvMat* rotvec = cvCreateMat(3,1, CV_64F);

double r1, r2, r3;

r1 = cvmGet(src, 0, 0);

r2 = cvmGet(src, 1, 0);

r3 = cvmGet(src, 2, 0);

double sin_th_2 = sqrt(r1*r1+r2*r2+r3*r3);

if (sin_th_2<1e-10)

{

cvSetZero(rotvec);

}

else

{

double u1 = r1 / sin_th_2;

double u2 = r2 / sin_th_2;

double u3 = r3 / sin_th_2;

double th = asin(sin_th_2) * 2;

cvmSet(rotvec, 0,0, th*u1);

cvmSet(rotvec, 1,0, th*u2);

cvmSet(rotvec, 2,0, th*u3);

}

cvRodrigues2(rotvec, dst);

cvReleaseMat(&rotvec);

}

void sba_optimize(double* _K, double* _Rot, double* _tran, double* points1, double* points2, double* p3d, int pnum)

{

int numpts3D = pnum;

int nframes = 2;

int nconstframes = 1;

char* vmask = new char[nframes*numpts3D];

memset(vmask, 1, nframes*numpts3D);

int cnp = 3+3; // camera number of parameters

int pnp = 3; // 3D point number of parameters

int mnp = 2; // image point number of parameters

double* motstruct = new double[nframes*cnp+numpts3D*pnp];

memset(motstruct, 0, nframes*cnp+numpts3D*pnp*sizeof(double));

int imagepointnum = nframes*numpts3D;

double* imgpts = new double[imagepointnum*mnp];

memset(imgpts, 0, sizeof(double)*(imagepointnum*mnp));

SBA_Globs globs;

double opts[SBA_OPTSSZ], info[SBA_INFOSZ];

int verbose = 1;

// initial data

int i;

for (i=0; i<pnum; ++i)

{

imgpts[i*4] = points1[i*2];

imgpts[i*4+1] = points1[i*2+1];

imgpts[i*4+2] = points2[i*2];

imgpts[i*4+3] = points2[i*2+1];

}

// camera paras

double* cam1 = motstruct;

memset(cam1, 0, sizeof(double)*cnp);

double* cam2 = cam1 + cnp;

CvMat R2 = cvMat(3,3, CV_64F, _Rot);

double _quaternion[3];

CvMat quat = cvMat(3,1, CV_64F, _quaternion);

rotMatrix_to_quaternion(&R2, &quat);

memcpy(cam2, _quaternion, sizeof(double)*3);

memcpy(cam2+3, _tran, sizeof(double)*3);

// 3D structure

memcpy(motstruct+cnp*nframes, p3d, sizeof(double)*pnp*numpts3D);

// etc.

/* call sparse LM routine */

opts[0]=SBA_INIT_MU; opts[1]=SBA_STOP_THRESH; opts[2]=SBA_STOP_THRESH;

opts[3]=SBA_STOP_THRESH;

// opts[3]=0.005*imagepointnum; // uncomment to force termination if the average reprojection error drops below 0.05

opts[4]=0.0;

// opts[4]=1E-05; // uncomment to force termination if the relative reduction in the RMS reprojection error drops below 1E-05

// init globs

globs.cnp = cnp; globs.mnp = mnp; globs.pnp = pnp;

double ical[5];

ical[0] = _K[0];

ical[1] = _K[2];

ical[2] = _K[5];

ical[3] = _K[4]/_K[0];

ical[4] = _K[1];

globs.intrcalib = ical;

globs.nccalib = 2;

globs.ptparams=NULL;

globs.camparams=NULL;

int n = sba_motstr_levmar(numpts3D, nframes, nconstframes, vmask, motstruct, cnp, pnp, imgpts, NULL, mnp,

img_projRTS, img_projRTS_jac, (void *)(&globs), 50, verbose, opts, info);

n = sba_motstr_levmar(numpts3D, nframes, nconstframes, vmask, motstruct, cnp, pnp, imgpts, NULL, mnp,

img_projRTS, img_projRTS_jac, (void *)(&globs), 50, verbose, opts, info);

// set result

memcpy(_quaternion, cam2, sizeof(double)*3);

quaternion_to_rotMatrix(&quat, &R2);

memcpy(_tran, cam2+3, sizeof(double)*3);

}