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;
}
static double arGetTransMatSub( double rot[3][3], double ppos2d[][2],
double pos3d[][3], int num, double conv[3][4],
double *dist_factor, double cpara[3][4] )
{
ARMat *mat_a, *mat_b, *mat_c, *mat_d, *mat_e, *mat_f;
double trans[3];
double wx, wy, wz;
double ret;
int i, j;
mat_a = arMatrixAlloc( num*2, 3 );
mat_b = arMatrixAlloc( 3, num*2 );
mat_c = arMatrixAlloc( num*2, 1 );
mat_d = arMatrixAlloc( 3, 3 );
mat_e = arMatrixAlloc( 3, 1 );
mat_f = arMatrixAlloc( 3, 1 );
if( arFittingMode == AR_FITTING_TO_INPUT ) {
for( i = 0; i < num; i++ ) {
arParamIdeal2Observ(dist_factor, 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] = cpara[0][0];
mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1];
mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];
mat_c->m[j*2+0] = wz * pos2d[j][0]
- cpara[0][0]*wx - cpara[0][1]*wy - cpara[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] = cpara[1][1];
mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];
mat_c->m[j*2+1] = wz * pos2d[j][1]
- cpara[1][1]*wy - cpara[1][2]*wz;
}
arMatrixMul( mat_d, mat_b, mat_a );
arMatrixMul( mat_e, mat_b, mat_c );
arMatrixSelfInv( mat_d );
arMatrixMul( 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, cpara, pos3d, pos2d, num );
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] = cpara[0][0];
mat_a->m[j*6+1] = mat_b->m[num*2+j*2] = cpara[0][1];
mat_a->m[j*6+2] = mat_b->m[num*4+j*2] = cpara[0][2] - pos2d[j][0];
mat_c->m[j*2+0] = wz * pos2d[j][0]
- cpara[0][0]*wx - cpara[0][1]*wy - cpara[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] = cpara[1][1];
mat_a->m[j*6+5] = mat_b->m[num*4+j*2+1] = cpara[1][2] - pos2d[j][1];
mat_c->m[j*2+1] = wz * pos2d[j][1]
- cpara[1][1]*wy - cpara[1][2]*wz;
}
arMatrixMul( mat_d, mat_b, mat_a );
arMatrixMul( mat_e, mat_b, mat_c );
arMatrixSelfInv( mat_d );
arMatrixMul( 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, cpara, pos3d, pos2d, num );
arMatrixFree( mat_a );
arMatrixFree( mat_b );
arMatrixFree( mat_c );
arMatrixFree( mat_d );
arMatrixFree( mat_e );
arMatrixFree( 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;
}
