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;
}
ARFloat
Tracker::arModifyMatrix(ARFloat rot[3][3], ARFloat trans[3], ARFloat cpara[3][4],
ARFloat vertex[][3], ARFloat pos2d[][2], int num)
{
ARFloat a, b, c;
ARFloat a2, b2, c2;
ARFloat ma, mb, mc;
int t1, t2, t3;
int s1, s2, s3;
int i, j, k;
ARFloat minerr;
I32 _hx, _hy, _h;
I32 _err, _minerr;
U32 _ures;
I32 _a1,_b1,_c1;
I32 _a2,_b2,_c2;
I32 _ma, _mb, _mc;
// PROFILE_BEGINSEC(profiler, MODIFYMATRIX)
FIXED_VEC3D *_vertex = (FIXED_VEC3D*)malloc(num*sizeof(FIXED_VEC3D)),
*_pos2d = (FIXED_VEC3D*)malloc(num*sizeof(FIXED_VEC3D)),
_combo[3], _vec1, _vec2, _trans;
I32 _combo3[3];
FIXED_VEC3D _cpara[3];
I32 _cpara3[3];
_vec1.z = 0;
_trans.x = FIXED_Float_To_Fixed_n(trans[0], 12);
_trans.y = FIXED_Float_To_Fixed_n(trans[1], 12);
_trans.z = FIXED_Float_To_Fixed_n(trans[2], 12);
for(j=0; j<3; j++)
{
_cpara[j].x = FIXED_Float_To_Fixed_n(cpara[j][0], 12);
_cpara[j].y = FIXED_Float_To_Fixed_n(cpara[j][1], 12);
_cpara[j].z = FIXED_Float_To_Fixed_n(cpara[j][2], 12);
_cpara3[j] = FIXED_Float_To_Fixed_n(cpara[j][3], 12);
}
for(j=0; j<num; j++)
{
_vertex[j].x = FIXED_Float_To_Fixed_n(vertex[j][0], BITS);
_vertex[j].y = FIXED_Float_To_Fixed_n(vertex[j][1], BITS);
_vertex[j].z = FIXED_Float_To_Fixed_n(vertex[j][2], BITS);
_pos2d[j].x = FIXED_Float_To_Fixed_n(pos2d[j][0], BITS);
_pos2d[j].y = FIXED_Float_To_Fixed_n(pos2d[j][1], BITS);
_pos2d[j].z = 0;
}
arGetAngle( rot, &a, &b, &c );
// _cpara and _trans are constant, which allows us to calcualte _combo3 beforehand...
//
for(j=0; j<3; j++)
{
FIXED_VEC3_DOT(_cpara+j, &_trans, _combo3+j, 12);
_combo3[j] += _cpara3[j];
_combo3[j] >>= 4;
}
// PROFILE_BEGINSEC(profiler, MODIFYMATRIX_LOOP)
a2 = a;
b2 = b;
c2 = c;
//factor = (ARFloat)(10.0*M_PI/180.0);
//I32 fix_a2, fix_b2, fix_c2;
I32 fix_factor = FIXED_Float_To_Fixed_n((10.0*M_PI/180.0), 12);
I32 fix_a[3], fix_b[3], fix_c[3];
_a2 = FIXED_Float_To_Fixed_n(a2, 12);
_b2 = FIXED_Float_To_Fixed_n(b2, 12);
_c2 = FIXED_Float_To_Fixed_n(c2, 12);
for( j = 0; j < 10; j++ ) {
_minerr = 0x40000000; // value
fix_a[0] = _a2 - fix_factor; fix_a[1] = _a2; fix_a[2] = _a2 + fix_factor;
fix_b[0] = _b2 - fix_factor; fix_b[1] = _b2; fix_b[2] = _b2 + fix_factor;
fix_c[0] = _c2 - fix_factor; fix_c[1] = _c2; fix_c[2] = _c2 + fix_factor;
for(t1=-1;t1<=1;t1++) {
for(t2=-1;t2<=1;t2++) {
for(t3=-1;t3<=1;t3++) {
_a1 = fix_a[t1+1];
_b1 = fix_b[t2+1];
_c1 = fix_c[t3+1];
//PROFILE_BEGINSEC(profiler, GETNEWMATRIX)
arGetNewMatrix12(_a1, _b1, _c1, _trans, NULL, _cpara, _cpara3, _combo, _combo3, profiler);
//PROFILE_ENDSEC(profiler, GETNEWMATRIX)
for(k=0; k<3; k++)
{
_combo[k].x >>= 4;
_combo[k].y >>= 4;
_combo[k].z >>= 4;
//_combo3[k] >>= 4;
}
_err = 0;
for( i = 0; i < num; i++ ) {
FIXED_VEC3_DOT(_combo+0, _vertex+i, &_hx, BITS);
_hx += _combo3[0];
FIXED_VEC3_DOT(_combo+1, _vertex+i, &_hy, BITS);
_hy += _combo3[1];
FIXED_VEC3_DOT(_combo+2, _vertex+i, &_h, BITS);
_h += _combo3[2];
// old method of doing two divides
// there is no reason at all to use this anymore
//
//FIXED_DIV2(_hx, _h, _vec1.x, BITS);
//FIXED_DIV2(_hy, _h, _vec1.y, BITS);
#ifndef _USE_DIV_TABLE_
// new method of doing one correct division
// and two multiplications. as accurate as two divisions but faster
//
I64 _rev = ((I64)1<<(BITS+32))/_h;
_vec1.x = (I32)((_hx*_rev)>>32);
_vec1.y = (I32)((_hy*_rev)>>32);
#endif
#ifdef _USE_DIV_TABLE_
// latest method of using a lookup table for division
// extremely fast (+30%) but less accurate
if((_h>>8)>=DIV_TABLE_MIN && (_h>>8)<DIV_TABLE_MAX)
{
I32 _revT4 = DIV_TABLE_FIXEDx4_FROM_FIXED(_h<<8);
I32 __x = imul(_hx, _revT4, 18);
I32 __y = imul(_hy, _revT4, 18);
#ifdef DEBUG_DIV_RANGE
int dx = __x-_vec1.x>0 ? __x-_vec1.x : _vec1.x-__x;
int dy = __y-_vec1.y>0 ? __y-_vec1.y : _vec1.y-__y;
if(dx>dbgInfo.dxMax) dbgInfo.dxMax = dx;
if(dy>dbgInfo.dyMax) dbgInfo.dyMax = dy;
if(dx/256.0f > 1.0f)
dx = dx;
if(dy/256.0f > 1.0f)
dy = dy;
#endif
_vec1.x = __x;
_vec1.y = __y;
}
else
{
I64 _rev = ((I64)1<<(BITS+32))/_h;
_vec1.x = (I32)((_hx*_rev)>>32);
_vec1.y = (I32)((_hy*_rev)>>32);
}
#endif //_USE_DIV_TABLE_
#ifdef DEBUG_DIV_RANGE
if(_h<dbgInfo.hMin) dbgInfo.hMin = _h;
if(_h>dbgInfo.hMax) dbgInfo.hMax = _h;
if(_hx<dbgInfo.hxMin) dbgInfo.hxMin = _hx;
if(_hx>dbgInfo.hxMax) dbgInfo.hxMax = _hx;
if(_hy<dbgInfo.hyMin) dbgInfo.hyMin = _hy;
if(_hy>dbgInfo.hyMax) dbgInfo.hyMax = _hy;
if(_hy>dbgInfo.hyMax) dbgInfo.hyMax = _hy;
#endif
FIXED_VEC2_SUB(_pos2d+i, &_vec1, &_vec2);
FIXED_VEC2_LENGTH_SQ(&_vec2, &_ures, BITS);
_err += _ures;
}
if( _err < _minerr ) {
_minerr = _err;
_ma = _a1;
_mb = _b1;
_mc = _c1;
s1 = t1; s2 = t2; s3 = t3;
}
}
}
}
if(s1 == 0 && s2 == 0 && s3 == 0)
fix_factor >>= 1;
_a2 = _ma;
_b2 = _mb;
_c2 = _mc;
}
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::arGetTransMatSub(ARFloat rot[3][3], ARFloat ppos2d[][2], ARFloat pos3d[][3], int num,
ARFloat conv[3][4], Camera *pCam)
//ARFloat *dist_factor, ARFloat cpara[3][4] )
{
ARMat *mat_a, *mat_b, *mat_c, *mat_d, *mat_e, *mat_f;
ARFloat trans[3];
ARFloat wx, wy, wz;
ARFloat ret;
int i, j;
mat_a = Matrix::alloc(num * 2, 3);
mat_b = Matrix::alloc(3, num * 2);
mat_c = Matrix::alloc(num * 2, 1);
mat_d = Matrix::alloc(3, 3);
mat_e = Matrix::alloc(3, 1);
mat_f = Matrix::alloc(3, 1);
if (arFittingMode == AR_FITTING_TO_INPUT) {
for (i = 0; i < num; i++) {
arCameraIdeal2Observ_std(pCam, ppos2d[i][0], ppos2d[i][1], &pos2d[i][0], &pos2d[i][1]);
}
} else {
for (i = 0; i < num; i++) {
pos2d[i][0] = ppos2d[i][0];
pos2d[i][1] = ppos2d[i][1];
}
}
for (j = 0; j < num; j++) {
wx = rot[0][0] * pos3d[j][0] + rot[0][1] * pos3d[j][1] + rot[0][2] * pos3d[j][2];
wy = rot[1][0] * pos3d[j][0] + rot[1][1] * pos3d[j][1] + rot[1][2] * pos3d[j][2];
wz = rot[2][0] * pos3d[j][0] + rot[2][1] * pos3d[j][1] + rot[2][2] * pos3d[j][2];
mat_a->m[j * 6 + 0] = mat_b->m[num * 0 + j * 2] = pCam->mat[0][0];
mat_a->m[j * 6 + 1] = mat_b->m[num * 2 + j * 2] = pCam->mat[0][1];
mat_a->m[j * 6 + 2] = mat_b->m[num * 4 + j * 2] = pCam->mat[0][2] - pos2d[j][0];
mat_c->m[j * 2 + 0] = wz * pos2d[j][0] - pCam->mat[0][0] * wx - pCam->mat[0][1] * wy - pCam->mat[0][2] * wz;
mat_a->m[j * 6 + 3] = mat_b->m[num * 0 + j * 2 + 1] = 0.0;
mat_a->m[j * 6 + 4] = mat_b->m[num * 2 + j * 2 + 1] = pCam->mat[1][1];
mat_a->m[j * 6 + 5] = mat_b->m[num * 4 + j * 2 + 1] = pCam->mat[1][2] - pos2d[j][1];
mat_c->m[j * 2 + 1] = wz * pos2d[j][1] - pCam->mat[1][1] * wy - pCam->mat[1][2] * wz;
}
Matrix::mul(mat_d, mat_b, mat_a);
Matrix::mul(mat_e, mat_b, mat_c);
Matrix::selfInv(mat_d);
Matrix::mul(mat_f, mat_d, mat_e);
trans[0] = mat_f->m[0];
trans[1] = mat_f->m[1];
trans[2] = mat_f->m[2];
/*trans[0] = 3.96559f;
trans[1] = 27.0546f;
trans[2] = 274.627f;*/
{
ARFloat a, b, c;
arGetAngle(rot, &a, &b, &c);
//trans[0] = -13.5f;
//trans[1] = 45.7f;
//trans[2] = 303.0f;
//arGetRot( -90.5f*3.1415f/180.0f, 120.3f*3.1415f/180.0f, 31.2f*3.1415f/180.0f, rot );
ret = arModifyMatrix(rot, trans, pCam->mat, pos3d, pos2d, num);
arGetAngle(rot, &a, &b, &c);
a = a;
}
// double begin
//
/*for( j = 0; j < num; j++ ) {
wx = rot[0][0] * pos3d[j][0]
+ rot[0][1] * pos3d[j][1]
+ rot[0][2] * pos3d[j][2];
wy = rot[1][0] * pos3d[j][0]
+ rot[1][1] * pos3d[j][1]
+ rot[1][2] * pos3d[j][2];
wz = rot[2][0] * pos3d[j][0]
+ rot[2][1] * pos3d[j][1]
+ rot[2][2] * pos3d[j][2];
mat_a->m[j*6+0] = mat_b->m[num*0+j*2] = pCam->mat[0][0];
mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = pCam->mat[0][1];
mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = pCam->mat[0][2] - pos2d[j][0];
mat_c->m[j*2+0] = wz * pos2d[j][0]
- pCam->mat[0][0]*wx - pCam->mat[0][1]*wy - pCam->mat[0][2]*wz;
mat_a->m[j*6+3] = mat_b->m[num*0+j*2+1] = 0.0;
mat_a->m[j*6+4] = mat_b->m[num*2+j*2+1] = pCam->mat[1][1];
mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = pCam->mat[1][2] - pos2d[j][1];
mat_c->m[j*2+1] = wz * pos2d[j][1]
- pCam->mat[1][1]*wy - pCam->mat[1][2]*wz;
}
Matrix::mul( mat_d, mat_b, mat_a );
Matrix::mul( mat_e, mat_b, mat_c );
Matrix::selfInv( mat_d );
Matrix::mul( mat_f, mat_d, mat_e );
trans[0] = mat_f->m[0];
trans[1] = mat_f->m[1];
trans[2] = mat_f->m[2];
ret = arModifyMatrix( rot, trans, pCam->mat, pos3d, pos2d, num );*/
//
// double end
Matrix::free(mat_a);
Matrix::free(mat_b);
Matrix::free(mat_c);
Matrix::free(mat_d);
Matrix::free(mat_e);
Matrix::free(mat_f);
for (j = 0; j < 3; j++) {
for (i = 0; i < 3; i++)
conv[j][i] = rot[j][i];
conv[j][3] = trans[j];
}
return ret;
}
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;
}
