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);
}