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