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