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