Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 204 | chris | 1 | /******************************************************* |
| 2 | * |
||
| 3 | * Author: Takeshi Mita, Shinsaku Hiura, Hirokazu Kato |
||
| 4 | * |
||
| 5 | * tmita@inolab.sys.es.osaka-u.ac.jp |
||
| 6 | * shinsaku@sys.es.osaka-u.ac.jp |
||
| 7 | * kato@sys.im.hiroshima-cu.ac.jp |
||
| 8 | * |
||
| 9 | * Revision: 3.1 |
||
| 10 | * Date: 99/07/16 |
||
| 11 | * |
||
| 12 | *******************************************************/ |
||
| 13 | |||
| 14 | #include <stdio.h> |
||
| 15 | #include <math.h> |
||
| 16 | #include <AR/matrix.h> |
||
| 17 | #include <AR/param.h> |
||
| 18 | |||
| 19 | #define AR_PARAM_CDMIN 12 |
||
| 20 | |||
| 21 | int arParamGet( double global[][3], double screen[][2], int num, |
||
| 22 | double mat[3][4] ) |
||
| 23 | { |
||
| 24 | ARMat *mat_a, *mat_at, *mat_r, mat_cpara; |
||
| 25 | ARMat *mat_wm1, *mat_wm2; |
||
| 26 | double *pa1, *pa2, *pr; /* working pointer */ |
||
| 27 | int i; /* working variables */ |
||
| 28 | |||
| 29 | if(num < AR_PARAM_NMIN) return( -1 ); |
||
| 30 | if(num > AR_PARAM_NMAX) return( -1 ); |
||
| 31 | |||
| 32 | mat_a = arMatrixAlloc( 2*num, AR_PARAM_CDMIN-1 ); |
||
| 33 | if( mat_a == NULL ) { |
||
| 34 | return -1; |
||
| 35 | } |
||
| 36 | mat_at = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num ); |
||
| 37 | if( mat_at == NULL ) { |
||
| 38 | arMatrixFree( mat_a ); |
||
| 39 | return -1; |
||
| 40 | } |
||
| 41 | mat_r = arMatrixAlloc( 2*num, 1 ); |
||
| 42 | if( mat_r == NULL ) { |
||
| 43 | arMatrixFree( mat_a ); |
||
| 44 | arMatrixFree( mat_at ); |
||
| 45 | return -1; |
||
| 46 | } |
||
| 47 | mat_wm1 = arMatrixAlloc( AR_PARAM_CDMIN-1, AR_PARAM_CDMIN-1 ); |
||
| 48 | if( mat_wm1 == NULL ) { |
||
| 49 | arMatrixFree( mat_a ); |
||
| 50 | arMatrixFree( mat_at ); |
||
| 51 | arMatrixFree( mat_r ); |
||
| 52 | return -1; |
||
| 53 | } |
||
| 54 | mat_wm2 = arMatrixAlloc( AR_PARAM_CDMIN-1, 2*num ); |
||
| 55 | if( mat_wm2 == NULL ) { |
||
| 56 | arMatrixFree( mat_a ); |
||
| 57 | arMatrixFree( mat_at ); |
||
| 58 | arMatrixFree( mat_r ); |
||
| 59 | arMatrixFree( mat_wm1 ); |
||
| 60 | return -1; |
||
| 61 | } |
||
| 62 | |||
| 63 | /* Initializing array */ |
||
| 64 | pa1 = mat_a->m; |
||
| 65 | for(i = 0; i < 2 * num * (AR_PARAM_CDMIN-1); i++) *pa1++ = 0.0; |
||
| 66 | |||
| 67 | /* Calculate A,R matrix */ |
||
| 68 | for(i = 0, pr = mat_r->m; i < num; i++) { |
||
| 69 | pa1 = &(mat_a->m[ (2*i) * (AR_PARAM_CDMIN-1) ]); |
||
| 70 | pa2 = &(mat_a->m[ (2*i+1) * (AR_PARAM_CDMIN-1) + 4]); |
||
| 71 | *pa1++ = global[i][0]; *pa1++ = global[i][1]; |
||
| 72 | *pa1++ = global[i][2]; *pa1++ = 1.0; |
||
| 73 | *pa2++ = global[i][0]; *pa2++ = global[i][1]; |
||
| 74 | *pa2++ = global[i][2]; *pa2++ = 1.0; |
||
| 75 | pa1 += 4; |
||
| 76 | *pa1++ = -global[i][0] * screen[i][0]; |
||
| 77 | *pa1++ = -global[i][1] * screen[i][0]; |
||
| 78 | *pa1 = -global[i][2] * screen[i][0]; |
||
| 79 | *pa2++ = -global[i][0] * screen[i][1]; |
||
| 80 | *pa2++ = -global[i][1] * screen[i][1]; |
||
| 81 | *pa2 = -global[i][2] * screen[i][1]; |
||
| 82 | |||
| 83 | *pr++ = screen[i][0] * AR_PARAM_C34; |
||
| 84 | *pr++ = screen[i][1] * AR_PARAM_C34; |
||
| 85 | } |
||
| 86 | |||
| 87 | if( arMatrixTrans( mat_at, mat_a ) < 0 ) { |
||
| 88 | arMatrixFree( mat_a ); |
||
| 89 | arMatrixFree( mat_at ); |
||
| 90 | arMatrixFree( mat_r ); |
||
| 91 | arMatrixFree( mat_wm1 ); |
||
| 92 | arMatrixFree( mat_wm2 ); |
||
| 93 | return -1; |
||
| 94 | } |
||
| 95 | if( arMatrixMul( mat_wm1, mat_at, mat_a ) < 0 ) { |
||
| 96 | arMatrixFree( mat_a ); |
||
| 97 | arMatrixFree( mat_at ); |
||
| 98 | arMatrixFree( mat_r ); |
||
| 99 | arMatrixFree( mat_wm1 ); |
||
| 100 | arMatrixFree( mat_wm2 ); |
||
| 101 | return -1; |
||
| 102 | } |
||
| 103 | if( arMatrixSelfInv( mat_wm1 ) < 0 ) { |
||
| 104 | arMatrixFree( mat_a ); |
||
| 105 | arMatrixFree( mat_at ); |
||
| 106 | arMatrixFree( mat_r ); |
||
| 107 | arMatrixFree( mat_wm1 ); |
||
| 108 | arMatrixFree( mat_wm2 ); |
||
| 109 | return -1; |
||
| 110 | } |
||
| 111 | if( arMatrixMul( mat_wm2, mat_wm1, mat_at ) < 0 ) { |
||
| 112 | arMatrixFree( mat_a ); |
||
| 113 | arMatrixFree( mat_at ); |
||
| 114 | arMatrixFree( mat_r ); |
||
| 115 | arMatrixFree( mat_wm1 ); |
||
| 116 | arMatrixFree( mat_wm2 ); |
||
| 117 | return -1; |
||
| 118 | } |
||
| 119 | mat_cpara.row = AR_PARAM_CDMIN-1; |
||
| 120 | mat_cpara.clm = 1; |
||
| 121 | mat_cpara.m = &(mat[0][0]); |
||
| 122 | if( arMatrixMul( &mat_cpara, mat_wm2, mat_r ) < 0 ) { |
||
| 123 | arMatrixFree( mat_a ); |
||
| 124 | arMatrixFree( mat_at ); |
||
| 125 | arMatrixFree( mat_r ); |
||
| 126 | arMatrixFree( mat_wm1 ); |
||
| 127 | arMatrixFree( mat_wm2 ); |
||
| 128 | return -1; |
||
| 129 | } |
||
| 130 | mat[2][3] = AR_PARAM_C34; |
||
| 131 | |||
| 132 | arMatrixFree( mat_a ); |
||
| 133 | arMatrixFree( mat_at ); |
||
| 134 | arMatrixFree( mat_r ); |
||
| 135 | arMatrixFree( mat_wm1 ); |
||
| 136 | arMatrixFree( mat_wm2 ); |
||
| 137 | return 0; |
||
| 138 | } |