decompose

void decompose_fundamental_matrix(double* mK, double* mF, double* mR, double* vt, CvMat* left_points, CvMat* right_points)

{

CvMat K_matrix = cvMat(3,3, CV_64F, mK);

CvMat rotation_matrix = cvMat(3,3, CV_64F, mR);

CvMat translate_vector = cvMat(3,1, CV_64F, vt);

int i;

int pcount = left_points->rows;

double NORMALIZE = 1;

CvMat* points1 = cvCreateMat(pcount, 3, CV_64F);

CvMat* points2 = cvCreateMat(pcount, 3, CV_64F);

CvMat* status = cvCreateMat(1, pcount, CV_8U);

CvMat F = cvMat(3,3, CV_64F, mF);

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

{

cvmSet(points1, i, 0, cvmGet(left_points, i, 0)/NORMALIZE);

cvmSet(points1, i, 1, cvmGet(left_points, i, 1)/NORMALIZE);

cvmSet(points1, i, 2, 1);

cvmSet(points2, i, 0, cvmGet(right_points, i, 0)/NORMALIZE);

cvmSet(points2, i, 1, cvmGet(right_points, i, 1)/NORMALIZE);

cvmSet(points2, i, 2, 1);

}

cvFindFundamentalMat(points1, points2, &F, CV_FM_RANSAC, 1.0/NORMALIZE, 0.95, status);

// 2.2 show matching results

//for (i=0; i<pcount; ++i)

//{

// if (status->data.ptr[i]!=1) continue;

// CvPoint2D32f pt1 = cvPoint2D32f(cvmGet(left_points, i, 0), cvmGet(left_points, i, 1));

// CvPoint2D32f pt2 = cvPoint2D32f(cvmGet(right_points, i, 0), cvmGet(right_points, i, 1));

// cvCircle(image_left, cvPointFrom32f(pt1), 3, cvScalar(255,0,0));

// cvLine(image_left, cvPointFrom32f(pt1), cvPointFrom32f(pt2), cvScalar(0,0,255), 2);

//}

//cvSaveImage("match.jpg", image_left);

draw_epiline(image_left, image_right, &F);

// debug

/*cvSave("F.txt", &F);

cvSave("points1.txt", points1);

cvSave("points2.txt", points2);*/

// get good points

int goodcnt = 0;

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

if (status->data.ptr[i]==1) ++ goodcnt;

double* good_points1 = new double[goodcnt*2];

double* good_points2 = new double[goodcnt*2];

double* p3ds = new double[goodcnt*3];

int ni = 0;

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

{

if (status->data.ptr[i]!=1) continue;

good_points1[ni*2] = cvmGet(points1, i, 0);

good_points1[ni*2+1] = cvmGet(points1, i, 1);

good_points2[ni*2] = cvmGet(points2, i, 0);

good_points2[ni*2+1] = cvmGet(points2, i, 1);

++ ni;

}

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

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

cvCopy(&K_matrix, K);

// normalize K

K->data.db[0] /= NORMALIZE;

K->data.db[2] /= NORMALIZE;

K->data.db[4] /= NORMALIZE;

K->data.db[5] /= NORMALIZE;

cvGEMM(&F, K, 1, NULL, 0, E);

cvGEMM(K, E, 1, NULL, 0, E, CV_GEMM_A_T);

// cvCopy(F, E); // debug

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

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

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

cvSVD(E, TH, U, V);

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

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

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

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

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

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

cvSetIdentity(I33);

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

cvZero(Z3);

int wt; // try 4 possibilities

int maxmc=-1, mc, maxwt;

double err;

int f1, f2;

CvPoint2D32f left_pt, right_pt;

for (wt=0; wt<4; wt++)

{

cvZero(W);

W->data.db[1] = 1;

W->data.db[3] = -1;

W->data.db[8] = 1;

if (wt==2 || wt==3) cvTranspose(W,W);

cvZero(Z);

Z->data.db[1] = -1;

Z->data.db[3] = 1;

if (wt==1 || wt==3) cvScale(Z,Z,-1);

// T = V * W * TH * V'

cvMatMul(V, Z, T);

cvGEMM(T, V, 1, NULL, 0, T, CV_GEMM_B_T);

t->data.db[0] = T->data.db[7];

t->data.db[1] = T->data.db[2];

t->data.db[2] = T->data.db[3];

// R = U * W' * V'

cvGEMM(U, W, 1, NULL, 0, R, CV_GEMM_B_T);

cvGEMM(R, V, 1, NULL, 0, R, CV_GEMM_B_T);

double detR = cvDet(R);

if (detR<0) cvScale(R,R,-1);

mc = 0;

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

{

left_pt.x = good_points1[i*2];

left_pt.y = good_points1[i*2+1];

right_pt.x = good_points2[i*2];

right_pt.y = good_points2[i*2+1];

ray_cross(K, I33, Z3, left_pt,

K, R, t, right_pt, &err, &f1, &f2);

if (f1>0 && f2>0) 

mc ++;

}

if (mc>maxmc)

{

maxmc = mc; maxwt = wt;

}

}

// recalculate

cvZero(W);

W->data.db[1] = 1;

W->data.db[3] = -1;

W->data.db[8] = 1;

if (maxwt==2 || maxwt==3) cvTranspose(W,W);

cvZero(Z);

Z->data.db[1] = -1;

Z->data.db[3] = 1;

if (maxwt==1 || maxwt==3) cvScale(Z,Z,-1);

// T = V * W * TH * V'

cvMatMul(V, Z, T);

cvGEMM(T, V, 1, NULL, 0, T, CV_GEMM_B_T);

t->data.db[0] = T->data.db[7];

t->data.db[1] = T->data.db[2];

t->data.db[2] = T->data.db[3];

// R = U * W' * V'

cvGEMM(U, W, 1, NULL, 0, R, CV_GEMM_B_T);

cvGEMM(R, V, 1, NULL, 0, R, CV_GEMM_B_T);

double detR = cvDet(R);

if (detR<0) cvScale(R,R,-1);

// set

cvCopy(R, &rotation_matrix);

cvCopy(t, &translate_vector);

// debug

//cvSave("D:\\R.txt", R);

//cvSave("D:\\t.txt", t);

// reconstruction

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

{

left_pt.x = good_points1[i*2];

left_pt.y = good_points1[i*2+1];

right_pt.x = good_points2[i*2];

right_pt.y = good_points2[i*2+1];

CvPoint3D32f p3d = ray_cross(K, I33, Z3, left_pt,

K, R, t, right_pt, &err, &f1, &f2);

p3ds[i*3] = p3d.x;

p3ds[i*3+1] = p3d.y;

p3ds[i*3+2] = p3d.z;

}

// sba optimize

sba_optimize(mK, mR, vt, good_points1, good_points2, p3ds, goodcnt);

// release

cvReleaseMat(&points1);

cvReleaseMat(&points2);

cvReleaseMat(&status);

cvReleaseMat(&K);

cvReleaseMat(&E);

cvReleaseMat(&U);

cvReleaseMat(&TH);

cvReleaseMat(&V);

cvReleaseMat(&W);

cvReleaseMat(&Z);

cvReleaseMat(&T);

cvReleaseMat(&R);

cvReleaseMat(&t);

cvReleaseMat(&I33);

cvReleaseMat(&Z3);

}