static int icp2GetTS(ARdouble TS[], ARdouble dU[], ARdouble J[], int dataNum, int paramNum)
{
ARMat matTS, matU, matJ;
ARMat *matJt, *matJtJ, *matJtU;
matTS.row = paramNum;
matTS.clm = 1;
matTS.m = TS;
matU.row = dataNum;
matU.clm = 1;
matU.m = dU;
matJ.row = dataNum;
matJ.clm = paramNum;
matJ.m = J;
matJt = arMatrixAllocTrans(&matJ);
if (matJt == NULL)
return -1;
matJtJ = arMatrixAllocMul(matJt, &matJ);
if (matJtJ == NULL)
{
arMatrixFree(matJt);
return -1;
}
matJtU = arMatrixAllocMul(matJt, &matU);
if (matJtJ == NULL)
{
arMatrixFree(matJt);
arMatrixFree(matJtJ);
return -1;
}
if (arMatrixSelfInv(matJtJ) < 0)
{
arMatrixFree(matJt);
arMatrixFree(matJtJ);
arMatrixFree(matJtU);
return -1;
}
arMatrixMul(&matTS, matJtJ, matJtU);
arMatrixFree(matJt);
arMatrixFree(matJtJ);
arMatrixFree(matJtU);
return 0;
}
/* find the position of the paddle card relative to the base and set the dropped blob position to this */
static void findPaddlePosition(float curPaddlePos[], double card_trans[3][4],double base_trans[3][4])
{
int i,j;
ARMat *mat_a, *mat_b, *mat_c;
double x, y, z;
//get card position relative to base pattern
mat_a = arMatrixAlloc( 4, 4 );
mat_b = arMatrixAlloc( 4, 4 );
mat_c = arMatrixAlloc( 4, 4 );
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) {
mat_b->m[j*4+i] = base_trans[j][i];
}
}
mat_b->m[3*4+0] = 0.0;
mat_b->m[3*4+1] = 0.0;
mat_b->m[3*4+2] = 0.0;
mat_b->m[3*4+3] = 1.0;
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) {
mat_a->m[j*4+i] = card_trans[j][i];
}
}
mat_a->m[3*4+0] = 0.0;
mat_a->m[3*4+1] = 0.0;
mat_a->m[3*4+2] = 0.0;
mat_a->m[3*4+3] = 1.0;
arMatrixSelfInv( mat_b );
arMatrixMul( mat_c, mat_b, mat_a );
//x,y,z is card position relative to base pattern
x = mat_c->m[0*4+3];
y = mat_c->m[1*4+3];
z = mat_c->m[2*4+3];
curPaddlePos[0] = x;
curPaddlePos[1] = y;
curPaddlePos[2] = z;
// printf("Position: %3.2f %3.2f %3.2f\n",x,y,z);
arMatrixFree( mat_a );
arMatrixFree( mat_b );
arMatrixFree( mat_c );
}
static void get_cpara( double world[4][2], double vertex[4][2],
double para[3][3] )
{
ARMat *a, *b, *c;
int i;
a = arMatrixAlloc( 8, 8 );
b = arMatrixAlloc( 8, 1 );
c = arMatrixAlloc( 8, 1 );
for( i = 0; i < 4; i++ ) {
a->m[i*16+0] = world[i][0];
a->m[i*16+1] = world[i][1];
a->m[i*16+2] = 1.0;
a->m[i*16+3] = 0.0;
a->m[i*16+4] = 0.0;
a->m[i*16+5] = 0.0;
a->m[i*16+6] = -world[i][0] * vertex[i][0];
a->m[i*16+7] = -world[i][1] * vertex[i][0];
a->m[i*16+8] = 0.0;
a->m[i*16+9] = 0.0;
a->m[i*16+10] = 0.0;
a->m[i*16+11] = world[i][0];
a->m[i*16+12] = world[i][1];
a->m[i*16+13] = 1.0;
a->m[i*16+14] = -world[i][0] * vertex[i][1];
a->m[i*16+15] = -world[i][1] * vertex[i][1];
b->m[i*2+0] = vertex[i][0];
b->m[i*2+1] = vertex[i][1];
}
arMatrixSelfInv( a );
arMatrixMul( c, a, b );
for( i = 0; i < 2; i++ ) {
para[i][0] = c->m[i*3+0];
para[i][1] = c->m[i*3+1];
para[i][2] = c->m[i*3+2];
}
para[2][0] = c->m[2*3+0];
para[2][1] = c->m[2*3+1];
para[2][2] = 1.0;
arMatrixFree( a );
arMatrixFree( b );
arMatrixFree( c );
}
int icpGetDeltaS( ARdouble S[6], ARdouble dU[], ARdouble J_U_S[][6], int n )
{
ARMat matS, matU, matJ;
ARMat *matJt, *matJtJ, *matJtU;
matS.row = 6;
matS.clm = 1;
matS.m = S;
matU.row = n;
matU.clm = 1;
matU.m = dU;
matJ.row = n;
matJ.clm = 6;
matJ.m = &J_U_S[0][0];
#if ICP_DEBUG
ARLOG("cc1 - matU\n");
arMatrixDisp( &matU );
ARLOG("cc1 - matJ\n");
arMatrixDisp( &matJ );
#endif
matJt = arMatrixAllocTrans( &matJ );
if( matJt == NULL ) return -1;
#if ICP_DEBUG
ARLOG("cc1 - matJt\n");
arMatrixDisp( matJt );
#endif
matJtJ = arMatrixAllocMul( matJt, &matJ );
if( matJtJ == NULL ) {
arMatrixFree( matJt );
return -1;
}
#if ICP_DEBUG
ARLOG("cc2 - matJtJ\n");
arMatrixDisp( matJtJ );
#endif
matJtU = arMatrixAllocMul( matJt, &matU );
if( matJtU == NULL ) {
arMatrixFree( matJt );
arMatrixFree( matJtJ );
return -1;
}
#if ICP_DEBUG
ARLOG("cc3 -- matJtU\n");
arMatrixDisp( matJtU );
#endif
if( arMatrixSelfInv(matJtJ) < 0 ) {
arMatrixFree( matJt );
arMatrixFree( matJtJ );
arMatrixFree( matJtU );
return -1;
}
#if ICP_DEBUG
ARLOG("cc4 -- matJtJ_Inv\n");
arMatrixDisp( matJtJ );
#endif
arMatrixMul( &matS, matJtJ, matJtU );
arMatrixFree( matJt );
arMatrixFree( matJtJ );
arMatrixFree( matJtU );
#if ICP_DEBUG
ARLOG("cc5 -- matS\n");
arMatrixDisp( &matS );
#endif
return 0;
}
ARdouble arGetStereoMatchingError( AR3DStereoHandle *handle, ARdouble pos2dL[2], ARdouble pos2dR[2] )
{
ARMat *cL, *cR, *cLi, *cRi;
ARMat *tL, *tR, *tLi, *tRi;
ARMat *w1, *w2, *w3;
ARdouble q1, q2, q3, t1, t2, t3;
ARdouble a1, b1, c1, a2, b2, c2;
ARdouble lL2, lR2;
int i, j;
cL = arMatrixAlloc( 4, 4 );
cR = arMatrixAlloc( 4, 4 );
tL = arMatrixAlloc( 4, 4 );
tR = arMatrixAlloc( 4, 4 );
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) {
cL->m[j*4+i] = handle->icpStereoHandle->matXcl2Ul[j][i];
}
}
cL->m[12] = cL->m[13] = cL->m[14] = 0.0; cL->m[15] = 1.0;
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) {
cR->m[j*4+i] = handle->icpStereoHandle->matXcr2Ur[j][i];
}
}
cR->m[12] = cR->m[13] = cR->m[14] = 0.0; cR->m[15] = 1.0;
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) {
tL->m[j*4+i] = handle->icpStereoHandle->matC2L[j][i];
}
}
tL->m[12] = tL->m[13] = tL->m[14] = 0.0; tL->m[15] = 1.0;
for( j = 0; j < 3; j++ ) {
for( i = 0; i < 4; i++ ) {
tR->m[j*4+i] = handle->icpStereoHandle->matC2R[j][i];
}
}
tR->m[12] = tR->m[13] = tR->m[14] = 0.0; tR->m[15] = 1.0;
cLi = arMatrixAllocInv( cL );
cRi = arMatrixAllocInv( cR );
tLi = arMatrixAllocInv( tL );
tRi = arMatrixAllocInv( tR );
w1 = arMatrixAllocMul( cR, tR );
w2 = arMatrixAllocMul( w1, tLi );
w3 = arMatrixAllocMul( w2, cLi );
q1 = w3->m[0*4+0]*pos2dL[0] + w3->m[0*4+1]*pos2dL[1] + w3->m[0*4+2];
q2 = w3->m[1*4+0]*pos2dL[0] + w3->m[1*4+1]*pos2dL[1] + w3->m[1*4+2];
q3 = w3->m[2*4+0]*pos2dL[0] + w3->m[2*4+1]*pos2dL[1] + w3->m[2*4+2];
t1 = w3->m[0*4+3];
t2 = w3->m[1*4+3];
t3 = w3->m[2*4+3];
a1 = q3*t2 - q2*t3;
b1 = q1*t3 - q3*t1;
c1 = q2*t1 - q1*t2;
arMatrixMul( w1, cL, tL );
arMatrixMul( w2, w1, tRi );
arMatrixMul( w3, w2, cRi );
q1 = w3->m[0*4+0]*pos2dR[0] + w3->m[0*4+1]*pos2dR[1] + w3->m[0*4+2];
q2 = w3->m[1*4+0]*pos2dR[0] + w3->m[1*4+1]*pos2dR[1] + w3->m[1*4+2];
q3 = w3->m[2*4+0]*pos2dR[0] + w3->m[2*4+1]*pos2dR[1] + w3->m[2*4+2];
t1 = w3->m[0*4+3];
t2 = w3->m[1*4+3];
t3 = w3->m[2*4+3];
a2 = q3*t2 - q2*t3;
b2 = q1*t3 - q3*t1;
c2 = q2*t1 - q1*t2;
arMatrixFree( w3 );
arMatrixFree( w2 );
arMatrixFree( w1 );
arMatrixFree( tL );
arMatrixFree( tR );
arMatrixFree( tLi );
arMatrixFree( tRi );
arMatrixFree( cR );
arMatrixFree( cL );
arMatrixFree( cRi );
arMatrixFree( cLi );
lL2 = (a1*pos2dR[0]+b1*pos2dR[1]+c1)*(a1*pos2dR[0]+b1*pos2dR[1]+c1) / (a1*a1+b1*b1);
lR2 = (a2*pos2dL[0]+b2*pos2dL[1]+c2)*(a2*pos2dL[0]+b2*pos2dL[1]+c2) / (a2*a2+b2*b2);
return (lL2 + lR2)/2.0;
}
int arGetStereoMatching(AR3DStereoHandle *handle, ARdouble pos2dL[2], ARdouble pos2dR[2], ARdouble pos3d[3])
{
ARMat *matP, *matQ;
ARMat *matPt, *matR, *matS, *matT;
ARdouble wL[3][4], wR[3][4];
arUtilMatMul( handle->icpStereoHandle->matXcl2Ul, handle->icpStereoHandle->matC2L, wL );
arUtilMatMul( handle->icpStereoHandle->matXcr2Ur, handle->icpStereoHandle->matC2R, wR );
matP = arMatrixAlloc(4,3);
matPt = arMatrixAlloc(3,4);
matQ = arMatrixAlloc(4,1);
matR = arMatrixAlloc(3,3);
matS = arMatrixAlloc(3,1);
matT = arMatrixAlloc(3,1);
matP->m[0*3+0] = matPt->m[0*4+0] = wL[0][0] - wL[2][0] * pos2dL[0];
matP->m[0*3+1] = matPt->m[1*4+0] = wL[0][1] - wL[2][1] * pos2dL[0];
matP->m[0*3+2] = matPt->m[2*4+0] = wL[0][2] - wL[2][2] * pos2dL[0];
matP->m[1*3+0] = matPt->m[0*4+1] = wL[1][0] - wL[2][0] * pos2dL[1];
matP->m[1*3+1] = matPt->m[1*4+1] = wL[1][1] - wL[2][1] * pos2dL[1];
matP->m[1*3+2] = matPt->m[2*4+1] = wL[1][2] - wL[2][2] * pos2dL[1];
matP->m[2*3+0] = matPt->m[0*4+2] = wR[0][0] - wR[2][0] * pos2dR[0];
matP->m[2*3+1] = matPt->m[1*4+2] = wR[0][1] - wR[2][1] * pos2dR[0];
matP->m[2*3+2] = matPt->m[2*4+2] = wR[0][2] - wR[2][2] * pos2dR[0];
matP->m[3*3+0] = matPt->m[0*4+3] = wR[1][0] - wR[2][0] * pos2dR[1];
matP->m[3*3+1] = matPt->m[1*4+3] = wR[1][1] - wR[2][1] * pos2dR[1];
matP->m[3*3+2] = matPt->m[2*4+3] = wR[1][2] - wR[2][2] * pos2dR[1];
matQ->m[0] = wL[2][3] * pos2dL[0] - wL[0][3];
matQ->m[1] = wL[2][3] * pos2dL[1] - wL[1][3];
matQ->m[2] = wR[2][3] * pos2dR[0] - wR[0][3];
matQ->m[3] = wR[2][3] * pos2dR[1] - wR[1][3];
arMatrixMul( matR, matPt, matP );
arMatrixMul( matS, matPt, matQ );
if( arMatrixSelfInv( matR ) < 0 ) {
arMatrixFree( matP );
arMatrixFree( matPt);
arMatrixFree( matQ );
arMatrixFree( matR );
arMatrixFree( matS );
arMatrixFree( matT );
return -1;
}
arMatrixMul( matT, matR, matS );
pos3d[0] = matT->m[0];
pos3d[1] = matT->m[1];
pos3d[2] = matT->m[2];
arMatrixFree( matP );
arMatrixFree( matPt);
arMatrixFree( matQ );
arMatrixFree( matR );
arMatrixFree( matS );
arMatrixFree( matT );
return 0;
}
int arParamGet( double global[][3], double screen[][2], int num,
double mat[3][4] )
{
ARMat *mat_a, *mat_at, *mat_r, mat_cpara;
ARMat *mat_wm1, *mat_wm2;
double *pa1, *pa2, *pr; /* working pointer */
int i; /* working variables */
if(num < AR_PARAM_NMIN) return( -1 );
if(num > AR_PARAM_NMAX) return( -1 );
mat_a = arMatrixAlloc( 2*num, AR_PARAM_CDMIN-1 );
if( mat_a == NULL ) {
return -1;
}
mat_at = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );
if( mat_at == NULL ) {
arMatrixFree( mat_a );
return -1;
}
mat_r = arMatrixAlloc( 2*num, 1 );
if( mat_r == NULL ) {
arMatrixFree( mat_a );
arMatrixFree( mat_at );
return -1;
}
mat_wm1 = arMatrixAlloc( AR_PARAM_CDMIN-1, AR_PARAM_CDMIN-1 );
if( mat_wm1 == NULL ) {
arMatrixFree( mat_a );
arMatrixFree( mat_at );
arMatrixFree( mat_r );
return -1;
}
mat_wm2 = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );
if( mat_wm2 == NULL ) {
arMatrixFree( mat_a );
arMatrixFree( mat_at );
arMatrixFree( mat_r );
arMatrixFree( mat_wm1 );
return -1;
}
/* Initializing array */
pa1 = mat_a->m;
for(i = 0; i < 2 * num * (AR_PARAM_CDMIN-1); i++) *pa1++ = 0.0;
/* Calculate A,R matrix */
for(i = 0, pr = mat_r->m; i < num; i++) {
pa1 = &(mat_a->m[ (2*i) * (AR_PARAM_CDMIN-1) ]);
pa2 = &(mat_a->m[ (2*i+1) * (AR_PARAM_CDMIN-1) + 4]);
*pa1++ = global[i][0]; *pa1++ = global[i][1];
*pa1++ = global[i][2]; *pa1++ = 1.0;
*pa2++ = global[i][0]; *pa2++ = global[i][1];
*pa2++ = global[i][2]; *pa2++ = 1.0;
pa1 += 4;
*pa1++ = -global[i][0] * screen[i][0];
*pa1++ = -global[i][1] * screen[i][0];
*pa1 = -global[i][2] * screen[i][0];
*pa2++ = -global[i][0] * screen[i][1];
*pa2++ = -global[i][1] * screen[i][1];
*pa2 = -global[i][2] * screen[i][1];
*pr++ = screen[i][0] * AR_PARAM_C34;
*pr++ = screen[i][1] * AR_PARAM_C34;
}
if( arMatrixTrans( mat_at, mat_a ) < 0 ) {
arMatrixFree( mat_a );
arMatrixFree( mat_at );
arMatrixFree( mat_r );
arMatrixFree( mat_wm1 );
arMatrixFree( mat_wm2 );
return -1;
}
if( arMatrixMul( mat_wm1, mat_at, mat_a ) < 0 ) {
arMatrixFree( mat_a );
arMatrixFree( mat_at );
arMatrixFree( mat_r );
arMatrixFree( mat_wm1 );
arMatrixFree( mat_wm2 );
return -1;
}
if( arMatrixSelfInv( mat_wm1 ) < 0 ) {
arMatrixFree( mat_a );
arMatrixFree( mat_at );
arMatrixFree( mat_r );
arMatrixFree( mat_wm1 );
arMatrixFree( mat_wm2 );
return -1;
}
if( arMatrixMul( mat_wm2, mat_wm1, mat_at ) < 0 ) {
arMatrixFree( mat_a );
arMatrixFree( mat_at );
arMatrixFree( mat_r );
arMatrixFree( mat_wm1 );
arMatrixFree( mat_wm2 );
return -1;
}
mat_cpara.row = AR_PARAM_CDMIN-1;
mat_cpara.clm = 1;
mat_cpara.m = &(mat[0][0]);
if( arMatrixMul( &mat_cpara, mat_wm2, mat_r ) < 0 ) {
arMatrixFree( mat_a );
arMatrixFree( mat_at );
arMatrixFree( mat_r );
arMatrixFree( mat_wm1 );
arMatrixFree( mat_wm2 );
return -1;
}
mat[2][3] = AR_PARAM_C34;
arMatrixFree( mat_a );
arMatrixFree( mat_at );
arMatrixFree( mat_r );
arMatrixFree( mat_wm1 );
arMatrixFree( mat_wm2 );
return 0;
}
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;
}