double arsModifyMatrix( double rot[3][3], double trans[3], ARSParam *arsParam, double pos3dL[][3], double pos2dL[][2], int numL, double pos3dR[][3], double pos2dR[][2], int numR ) { double factor; double a, b, c; double a1, b1, c1; double a2, b2, c2; double ma = 0.0, mb = 0.0, mc = 0.0; double combo[3][4]; double hx, hy, h, x, y; double err, minerr; int t1, t2, t3; int s1 = 0, s2 = 0, s3 = 0; int i, j; arGetAngle( rot, &a, &b, &c ); a2 = a; b2 = b; c2 = c; factor = 10.0*MD_PI/180.0; for( j = 0; j < 10; j++ ) { minerr = 1000000000.0; for(t1=-1;t1<=1;t1++) { for(t2=-1;t2<=1;t2++) { for(t3=-1;t3<=1;t3++) { a1 = a2 + factor*t1; b1 = b2 + factor*t2; c1 = c2 + factor*t3; err = 0.0; arGetNewMatrix( a1, b1, c1, trans, NULL, arsParam->matL, combo ); for( i = 0; i < numL; i++ ) { hx = combo[0][0] * pos3dL[i][0] + combo[0][1] * pos3dL[i][1] + combo[0][2] * pos3dL[i][2] + combo[0][3]; hy = combo[1][0] * pos3dL[i][0] + combo[1][1] * pos3dL[i][1] + combo[1][2] * pos3dL[i][2] + combo[1][3]; h = combo[2][0] * pos3dL[i][0] + combo[2][1] * pos3dL[i][1] + combo[2][2] * pos3dL[i][2] + combo[2][3]; x = hx / h; y = hy / h; err += (pos2dL[i][0] - x) * (pos2dL[i][0] - x) + (pos2dL[i][1] - y) * (pos2dL[i][1] - y); } arGetNewMatrix( a1, b1, c1, trans, arsParam->matL2R, arsParam->matR, combo ); for( i = 0; i < numR; i++ ) { hx = combo[0][0] * pos3dR[i][0] + combo[0][1] * pos3dR[i][1] + combo[0][2] * pos3dR[i][2] + combo[0][3]; hy = combo[1][0] * pos3dR[i][0] + combo[1][1] * pos3dR[i][1] + combo[1][2] * pos3dR[i][2] + combo[1][3]; h = combo[2][0] * pos3dR[i][0] + combo[2][1] * pos3dR[i][1] + combo[2][2] * pos3dR[i][2] + combo[2][3]; x = hx / h; y = hy / h; err += (pos2dR[i][0] - x) * (pos2dR[i][0] - x) + (pos2dR[i][1] - y) * (pos2dR[i][1] - y); } if( err < minerr ) { minerr = err; ma = a1; mb = b1; mc = c1; s1 = t1; s2 = t2; s3 = t3; } } } } if( s1 == 0 && s2 == 0 && s3 == 0 ) factor *= 0.5; a2 = ma; b2 = mb; c2 = mc; } arGetRot( ma, mb, mc, rot ); return minerr / (numL+numR); }
ARFloat Tracker::arModifyMatrix2(ARFloat rot[3][3], ARFloat trans[3], ARFloat cpara[3][4], ARFloat vertex[][3], ARFloat pos2d[][2], int num) { ARFloat factor; ARFloat a, b, c; ARFloat a1, b1, c1; ARFloat a2, b2, c2; ARFloat ma, mb, mc; ARFloat combo[3][4]; ARFloat hx, hy, h, x, y; ARFloat err, minerr; int t1, t2, t3, tt1, tt2, tt3; ARFloat tfact[5] = { 0.96f, 0.98f, 1.0f, 1.02f, 1.04f }; ARFloat modtrans[3], mmodtrans[3]; int s1, s2, s3, ss1, ss2, ss3; int i, j; arGetAngle(rot, &a, &b, &c); a2 = a; b2 = b; c2 = c; factor = (ARFloat) (40.0 * M_PI / 180.0); for (j = 0; j < 15; j++) { minerr = 1000000000.0; for (t1 = -1; t1 <= 1; t1++) { for (t2 = -1; t2 <= 1; t2++) { for (t3 = -1; t3 <= 1; t3++) { for (tt1 = -2; tt1 <= 2; tt1++) { for (tt2 = -2; tt2 <= 2; tt2++) { for (tt3 = -2; tt3 <= 2; tt3++) { a1 = a2 + factor * t1; b1 = b2 + factor * t2; c1 = c2 + factor * t3; modtrans[0] = trans[0] * tfact[tt1 + 2]; modtrans[1] = trans[1] * tfact[tt2 + 2]; modtrans[2] = trans[2] * tfact[tt3 + 2]; arGetNewMatrix(a1, b1, c1, modtrans, NULL, cpara, combo); err = 0.0; for (i = 0; i < num; i++) { hx = combo[0][0] * vertex[i][0] + combo[0][1] * vertex[i][1] + combo[0][2] * vertex[i][2] + combo[0][3]; hy = combo[1][0] * vertex[i][0] + combo[1][1] * vertex[i][1] + combo[1][2] * vertex[i][2] + combo[1][3]; h = combo[2][0] * vertex[i][0] + combo[2][1] * vertex[i][1] + combo[2][2] * vertex[i][2] + combo[2][3]; x = hx / h; y = hy / h; err += (pos2d[i][0] - x) * (pos2d[i][0] - x) + (pos2d[i][1] - y) * (pos2d[i][1] - y); } if (err < minerr) { minerr = err; ma = a1; mb = b1; mc = c1; mmodtrans[0] = modtrans[0]; mmodtrans[1] = modtrans[1]; mmodtrans[2] = modtrans[2]; s1 = t1; s2 = t2; s3 = t3; ss1 = tt1; ss2 = tt2; ss3 = tt3; } } } } } } } if (s1 == 0 && s2 == 0 && s3 == 0 /*&& ss1 == 0 && ss2 == 0 && ss3 == 0*/) factor *= 0.5; a2 = ma; b2 = mb; c2 = mc; trans[0] = mmodtrans[0]; trans[1] = mmodtrans[1]; trans[2] = mmodtrans[2]; } arGetRot(ma, mb, mc, rot); return minerr / num; }
double arModifyMatrix( double rot[3][3], double trans[3], double cpara[3][4], double vertex[][3], double pos2d[][2], int num ) { double factor; double a, b, c; double a1, b1, c1; double a2, b2, c2; double ma = 0.0, mb = 0.0, mc = 0.0; double combo[3][4]; double hx, hy, h, x, y; double err, minerr; int t1, t2, t3; int s1 = 0, s2 = 0, s3 = 0; int i, j; arGetAngle( rot, &a, &b, &c ); a2 = a; b2 = b; c2 = c; factor = 10.0*MD_PI/180.0; for( j = 0; j < 10; j++ ) { minerr = 1000000000.0; for(t1=-1;t1<=1;t1++) { for(t2=-1;t2<=1;t2++) { for(t3=-1;t3<=1;t3++) { a1 = a2 + factor*t1; b1 = b2 + factor*t2; c1 = c2 + factor*t3; arGetNewMatrix( a1, b1, c1, trans, NULL, cpara, combo ); err = 0.0; for( i = 0; i < num; i++ ) { hx = combo[0][0] * vertex[i][0] + combo[0][1] * vertex[i][1] + combo[0][2] * vertex[i][2] + combo[0][3]; hy = combo[1][0] * vertex[i][0] + combo[1][1] * vertex[i][1] + combo[1][2] * vertex[i][2] + combo[1][3]; h = combo[2][0] * vertex[i][0] + combo[2][1] * vertex[i][1] + combo[2][2] * vertex[i][2] + combo[2][3]; x = hx / h; y = hy / h; err += (pos2d[i][0] - x) * (pos2d[i][0] - x) + (pos2d[i][1] - y) * (pos2d[i][1] - y); } if( err < minerr ) { minerr = err; ma = a1; mb = b1; mc = c1; s1 = t1; s2 = t2; s3 = t3; } } } } if( s1 == 0 && s2 == 0 && s3 == 0 ) factor *= 0.5; a2 = ma; b2 = mb; c2 = mc; } arGetRot( ma, mb, mc, rot ); /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */ return minerr/num; }