00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include <stdio.h>
00015 #include <math.h>
00016 #include <AR/matrix.h>
00017 #include <AR/param.h>
00018
00019 #define AR_PARAM_CDMIN 12
00020
00021 int arParamGet( double global[][3], double screen[][2], int num,
00022 double mat[3][4] )
00023 {
00024 ARMat *mat_a, *mat_at, *mat_r, mat_cpara;
00025 ARMat *mat_wm1, *mat_wm2;
00026 double *pa1, *pa2, *pr;
00027 int i;
00028
00029 if(num < AR_PARAM_NMIN) return( -1 );
00030 if(num > AR_PARAM_NMAX) return( -1 );
00031
00032 mat_a = arMatrixAlloc( 2*num, AR_PARAM_CDMIN-1 );
00033 if( mat_a == NULL ) {
00034 return -1;
00035 }
00036 mat_at = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );
00037 if( mat_at == NULL ) {
00038 arMatrixFree( mat_a );
00039 return -1;
00040 }
00041 mat_r = arMatrixAlloc( 2*num, 1 );
00042 if( mat_r == NULL ) {
00043 arMatrixFree( mat_a );
00044 arMatrixFree( mat_at );
00045 return -1;
00046 }
00047 mat_wm1 = arMatrixAlloc( AR_PARAM_CDMIN-1, AR_PARAM_CDMIN-1 );
00048 if( mat_wm1 == NULL ) {
00049 arMatrixFree( mat_a );
00050 arMatrixFree( mat_at );
00051 arMatrixFree( mat_r );
00052 return -1;
00053 }
00054 mat_wm2 = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num );
00055 if( mat_wm2 == NULL ) {
00056 arMatrixFree( mat_a );
00057 arMatrixFree( mat_at );
00058 arMatrixFree( mat_r );
00059 arMatrixFree( mat_wm1 );
00060 return -1;
00061 }
00062
00063
00064 pa1 = mat_a->m;
00065 for(i = 0; i < 2 * num * (AR_PARAM_CDMIN-1); i++) *pa1++ = 0.0;
00066
00067
00068 for(i = 0, pr = mat_r->m; i < num; i++) {
00069 pa1 = &(mat_a->m[ (2*i) * (AR_PARAM_CDMIN-1) ]);
00070 pa2 = &(mat_a->m[ (2*i+1) * (AR_PARAM_CDMIN-1) + 4]);
00071 *pa1++ = global[i][0]; *pa1++ = global[i][1];
00072 *pa1++ = global[i][2]; *pa1++ = 1.0;
00073 *pa2++ = global[i][0]; *pa2++ = global[i][1];
00074 *pa2++ = global[i][2]; *pa2++ = 1.0;
00075 pa1 += 4;
00076 *pa1++ = -global[i][0] * screen[i][0];
00077 *pa1++ = -global[i][1] * screen[i][0];
00078 *pa1 = -global[i][2] * screen[i][0];
00079 *pa2++ = -global[i][0] * screen[i][1];
00080 *pa2++ = -global[i][1] * screen[i][1];
00081 *pa2 = -global[i][2] * screen[i][1];
00082
00083 *pr++ = screen[i][0] * AR_PARAM_C34;
00084 *pr++ = screen[i][1] * AR_PARAM_C34;
00085 }
00086
00087 if( arMatrixTrans( mat_at, mat_a ) < 0 ) {
00088 arMatrixFree( mat_a );
00089 arMatrixFree( mat_at );
00090 arMatrixFree( mat_r );
00091 arMatrixFree( mat_wm1 );
00092 arMatrixFree( mat_wm2 );
00093 return -1;
00094 }
00095 if( arMatrixMul( mat_wm1, mat_at, mat_a ) < 0 ) {
00096 arMatrixFree( mat_a );
00097 arMatrixFree( mat_at );
00098 arMatrixFree( mat_r );
00099 arMatrixFree( mat_wm1 );
00100 arMatrixFree( mat_wm2 );
00101 return -1;
00102 }
00103 if( arMatrixSelfInv( mat_wm1 ) < 0 ) {
00104 arMatrixFree( mat_a );
00105 arMatrixFree( mat_at );
00106 arMatrixFree( mat_r );
00107 arMatrixFree( mat_wm1 );
00108 arMatrixFree( mat_wm2 );
00109 return -1;
00110 }
00111 if( arMatrixMul( mat_wm2, mat_wm1, mat_at ) < 0 ) {
00112 arMatrixFree( mat_a );
00113 arMatrixFree( mat_at );
00114 arMatrixFree( mat_r );
00115 arMatrixFree( mat_wm1 );
00116 arMatrixFree( mat_wm2 );
00117 return -1;
00118 }
00119 mat_cpara.row = AR_PARAM_CDMIN-1;
00120 mat_cpara.clm = 1;
00121 mat_cpara.m = &(mat[0][0]);
00122 if( arMatrixMul( &mat_cpara, mat_wm2, mat_r ) < 0 ) {
00123 arMatrixFree( mat_a );
00124 arMatrixFree( mat_at );
00125 arMatrixFree( mat_r );
00126 arMatrixFree( mat_wm1 );
00127 arMatrixFree( mat_wm2 );
00128 return -1;
00129 }
00130 mat[2][3] = AR_PARAM_C34;
00131
00132 arMatrixFree( mat_a );
00133 arMatrixFree( mat_at );
00134 arMatrixFree( mat_r );
00135 arMatrixFree( mat_wm1 );
00136 arMatrixFree( mat_wm2 );
00137 return 0;
00138 }
