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