00001 #include "anigauss.h"
00002
00003
00004 void YvVfilterCoef(double sigma, double *filter);
00005 void TriggsM(double *filter, double *M);
00006
00007 static void f_iir_xline_filter(SRCTYPE *src, DSTTYPE *dest, int sx, int sy,
00008 double *filter);
00009 static void f_iir_yline_filter(DSTTYPE *src, DSTTYPE *dest, int sx, int sy,
00010 double *filter);
00011 static void f_iir_tline_filter(DSTTYPE *src, DSTTYPE *dest, int sx, int sy,
00012 double *filter, double tanp);
00013 static void f_iir_derivative_filter(DSTTYPE *src, DSTTYPE *dest, int sx, int sy,
00014 double phi, int order);
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 void anigauss(SRCTYPE *input, DSTTYPE *output, int sizex, int sizey,
00042 double sigmav, double sigmau, double phi, int orderv, int orderu)
00043 {
00044 double filter[7];
00045 double sigmax, sigmay, tanp;
00046 double su2, sv2;
00047 double phirad;
00048 double a11, a21, a22;
00049 int i;
00050
00051 su2 = sigmau*sigmau;
00052 sv2 = sigmav*sigmav;
00053 phirad = phi*PI/180.;
00054
00055 a11 = cos(phirad)*cos(phirad)*su2 + sin(phirad)*sin(phirad)*sv2;
00056 a21 = cos(phirad)*sin(phirad)*(su2-sv2);
00057 a22 = cos(phirad)*cos(phirad)*sv2 + sin(phirad)*sin(phirad)*su2;
00058
00059 sigmax = sqrt(a11-a21*a21/a22);
00060 tanp = a21/a22;
00061 sigmay = sqrt(a22);
00062
00063
00064 YvVfilterCoef(sigmax, filter);
00065
00066
00067 f_iir_xline_filter(input,output,sizex,sizey,filter);
00068
00069
00070 YvVfilterCoef(sigmay, filter);
00071
00072 if (tanp != 0.0) {
00073
00074 f_iir_tline_filter(output,output,sizex,sizey,filter, tanp);
00075 }
00076 else {
00077
00078 f_iir_yline_filter(output,output,sizex,sizey,filter);
00079 }
00080
00081
00082 for(i=0; i<orderv; i++)
00083 f_iir_derivative_filter(output, output, sizex, sizey, phirad-PI/2., 1);
00084 for(i=0; i<orderu; i++)
00085 f_iir_derivative_filter(output, output, sizex, sizey, phirad, 1);
00086 }
00087
00088
00089 void YvVfilterCoef(double sigma, double *filter)
00090 {
00091
00092
00093
00094
00095
00096
00097
00098 double q, qsq;
00099 double scale;
00100 double B, b1, b2, b3;
00101
00102
00103 double m0 = 1.16680, m1 = 1.10783, m2 = 1.40586;
00104 double m1sq = m1*m1, m2sq = m2*m2;
00105
00106
00107 if(sigma < 3.556)
00108 q = -0.2568 + 0.5784 * sigma + 0.0561 * sigma * sigma;
00109 else
00110 q = 2.5091 + 0.9804 * (sigma - 3.556);
00111
00112 qsq = q*q;
00113
00114
00115 scale = (m0 + q) * (m1sq + m2sq + 2*m1*q + qsq);
00116 b1 = -q * (2*m0*m1 + m1sq + m2sq + (2*m0 + 4*m1) * q + 3*qsq) / scale;
00117 b2 = qsq * (m0 + 2*m1 + 3*q) / scale;
00118 b3 = - qsq * q / scale;
00119
00120
00121 B = (m0 * (m1sq + m2sq))/scale;
00122
00123
00124 filter[0] = -b3;
00125 filter[1] = -b2;
00126 filter[2] = -b1;
00127 filter[3] = B;
00128 filter[4] = -b1;
00129 filter[5] = -b2;
00130 filter[6] = -b3;
00131 }
00132
00133 void TriggsM(double *filter, double *M)
00134 {
00135 double scale;
00136 double a1, a2, a3;
00137
00138 a3 = filter[0];
00139 a2 = filter[1];
00140 a1 = filter[2];
00141
00142 scale = 1.0/((1.0+a1-a2+a3)*(1.0-a1-a2-a3)*(1.0+a2+(a1-a3)*a3));
00143 M[0] = scale*(-a3*a1+1.0-a3*a3-a2);
00144 M[1] = scale*(a3+a1)*(a2+a3*a1);
00145 M[2] = scale*a3*(a1+a3*a2);
00146 M[3] = scale*(a1+a3*a2);
00147 M[4] = -scale*(a2-1.0)*(a2+a3*a1);
00148 M[5] = -scale*a3*(a3*a1+a3*a3+a2-1.0);
00149 M[6] = scale*(a3*a1+a2+a1*a1-a2*a2);
00150 M[7] = scale*(a1*a2+a3*a2*a2-a1*a3*a3-a3*a3*a3-a3*a2+a3);
00151 M[8] = scale*a3*(a1+a3*a2);
00152 }
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166 static void
00167 f_iir_xline_filter(SRCTYPE *src, DSTTYPE *dest, int sx, int sy, double *filter)
00168 {
00169 int i, j;
00170 double b1, b2, b3;
00171 double pix, p1, p2, p3;
00172 double sum, sumsq;
00173 double iplus, uplus, vplus;
00174 double unp, unp1, unp2;
00175 double M[9];
00176
00177 sumsq = filter[3];
00178 sum = sumsq*sumsq;
00179
00180 for (i = 0; i < sy; i++) {
00181
00182 b1 = filter[2]; b2 = filter[1]; b3 = filter[0];
00183 p1 = *src/sumsq; p2 = p1; p3 = p1;
00184
00185 iplus = src[sx-1];
00186 for (j = 0; j < sx; j++) {
00187 pix = *src++ + b1*p1 + b2*p2 + b3*p3;
00188 *dest++ = pix;
00189 p3 = p2; p2 = p1; p1 = pix;
00190 }
00191
00192
00193
00194
00195 uplus = iplus/(1.0-b1-b2-b3);
00196 b1 = filter[4]; b2 = filter[5]; b3 = filter[6];
00197 vplus = uplus/(1.0-b1-b2-b3);
00198
00199 unp = p1-uplus;
00200 unp1 = p2-uplus;
00201 unp2 = p3-uplus;
00202
00203 TriggsM(filter, M);
00204
00205 pix = M[0]*unp+M[1]*unp1+M[2]*unp2 + vplus;
00206 p1 = M[3]*unp+M[4]*unp1+M[5]*unp2 + vplus;
00207 p2 = M[6]*unp+M[7]*unp1+M[8]*unp2 + vplus;
00208 pix *= sum; p1 *= sum; p2 *= sum;
00209
00210 *(--dest) = pix;
00211 p3 = p2; p2 = p1; p1 = pix;
00212
00213 for (j = sx-2; j >= 0; j--) {
00214 pix = sum * *(--dest) + b1*p1 + b2*p2 + b3*p3;
00215 *dest = pix;
00216 p3 = p2; p2 = p1; p1 = pix;
00217 }
00218 dest += sx;
00219 }
00220 }
00221
00222 static void
00223 f_iir_yline_filter(DSTTYPE *src, DSTTYPE *dest, int sx, int sy, double *filter)
00224 {
00225 double *p0, *p1, *p2, *p3, *pswap;
00226 double *buf0, *buf1, *buf2, *buf3;
00227 double *uplusbuf;
00228 int i, j;
00229 double b1, b2, b3;
00230 double pix;
00231 double sum, sumsq;
00232 double uplus, vplus;
00233 double unp, unp1, unp2;
00234 double M[9];
00235
00236 sumsq = filter[3];
00237 sum = sumsq*sumsq;
00238
00239 uplusbuf = (double *)malloc(sx*sizeof(*uplusbuf));
00240
00241 buf0 = (double*)malloc(sx*sizeof(*buf0));
00242 buf1 = (double*)malloc(sx*sizeof(*buf1));
00243 buf2 = (double*)malloc(sx*sizeof(*buf2));
00244 buf3 = (double*)malloc(sx*sizeof(*buf3));
00245
00246 p0 = buf0; p1 = buf1; p2 = buf2; p3 = buf3;
00247
00248
00249 b1 = filter[2]; b2 = filter[1]; b3 = filter[0];
00250
00251
00252 for (j = 0; j < sx; j++) {
00253 pix = *src++/sumsq;
00254 p1[j] = pix; p2[j] = pix; p3[j] = pix;
00255 }
00256
00257 src += (sy-2)*sx;
00258 for (j = 0; j < sx; j++)
00259 uplusbuf[j] = *src++/(1.0-b1-b2-b3);
00260 src -= sy*sx;
00261
00262 for (i = 0; i < sy; i++) {
00263 for (j = 0; j < sx; j++) {
00264 pix = *src++ + b1*p1[j] + b2*p2[j] + b3*p3[j];
00265 *dest++ = pix;
00266 p0[j] = pix;
00267 }
00268
00269
00270 pswap = p3; p3 = p2; p2 = p1; p1 = p0; p0 = pswap;
00271 }
00272
00273
00274
00275
00276
00277 b1 = filter[4]; b2 = filter[5]; b3 = filter[6];
00278 TriggsM(filter, M);
00279
00280
00281 p0 = uplusbuf;
00282 for (j = sx-1; j >= 0; j--) {
00283 uplus = p0[j];
00284 vplus = uplus/(1.0-b1-b2-b3);
00285
00286 unp = p1[j]-uplus;
00287 unp1 = p2[j]-uplus;
00288 unp2 = p3[j]-uplus;
00289 pix = M[0]*unp+M[1]*unp1+M[2]*unp2 + vplus;
00290 pix *= sum;
00291 *(--dest) = pix;
00292 p1[j] = pix;
00293 pix = M[3]*unp+M[4]*unp1+M[5]*unp2 + vplus;
00294 p2[j] = pix*sum;
00295 pix = M[6]*unp+M[7]*unp1+M[8]*unp2 + vplus;
00296 p3[j] = pix*sum;
00297 }
00298
00299 for (i = sy-2; i >= 0; i--) {
00300 for (j = sx-1; j >= 0; j--) {
00301 pix = sum * *(--dest) + b1*p1[j] + b2*p2[j] + b3*p3[j];
00302 *dest = pix;
00303 p0[j] = pix;
00304 }
00305
00306
00307 pswap = p3; p3 = p2; p2 = p1; p1 = p0; p0 = pswap;
00308 }
00309
00310 free(buf0);
00311 free(buf1);
00312 free(buf2);
00313 free(buf3);
00314 free(uplusbuf);
00315 }
00316
00317
00318 static void
00319 f_iir_tline_filter(DSTTYPE *src, DSTTYPE *dest, int sx, int sy,
00320 double *filter, double tanp)
00321 {
00322 double *p0, *p1, *p2, *p3;
00323 double *buf0, *buf1, *buf2, *buf3;
00324 double *uplusbuf;
00325 int i, j;
00326 double b1, b2, b3;
00327 double sum, sumsq;
00328 double uplus, vplus;
00329 double unp, unp1, unp2;
00330 double M[9];
00331 double pix, prev, val;
00332 double res, prevres;
00333 double xf;
00334 int x;
00335 double c, d;
00336 double e, f;
00337 int q4 = 0;
00338
00339
00340 if (tanp <= 0.0) {
00341 q4 = 1;
00342 tanp = -tanp;
00343 }
00344
00345
00346 uplusbuf = (double*)malloc(sx*sizeof(*uplusbuf));
00347
00348
00349 buf0 = (double*)malloc((int)(sx+sy*tanp+2)*sizeof(*buf0));
00350 buf1 = (double*)malloc((int)(sx+sy*tanp+2)*sizeof(*buf1));
00351 buf2 = (double*)malloc((int)(sx+sy*tanp+2)*sizeof(*buf2));
00352 buf3 = (double*)malloc((int)(sx+sy*tanp+2)*sizeof(*buf3));
00353
00354 if (q4) {
00355 buf0 += (int)(sy*tanp+1);
00356 buf1 += (int)(sy*tanp+1);
00357 buf2 += (int)(sy*tanp+1);
00358 buf3 += (int)(sy*tanp+1);
00359 }
00360
00361 sumsq = filter[3];
00362 sum = sumsq*sumsq;
00363
00364
00365 b1 = filter[2]; b2 = filter[1]; b3 = filter[0];
00366
00367
00368 p1 = buf1; p2 = buf2; p3 = buf3;
00369 for (j = 0; j < sx; j++) {
00370 pix = *src++;
00371 *dest++ = pix; *p1++ = pix; *p2++ = pix; *p3++ = pix;
00372 }
00373
00374
00375 src += (sy-2)*sx;
00376 for (j = 0; j < sx; j++)
00377 uplusbuf[j] = *src++ * sumsq/(1.0-b1-b2-b3);
00378 src -= (sy-1)*sx;
00379
00380 x = 0;
00381 for (i = 1; i < sy; i++) {
00382 xf = i*tanp;
00383
00384
00385 if (q4) {
00386 p1 = buf1-x; p2 = buf2-x; p3 = buf3-x;
00387 for (j=1; j <= (int)(xf)-x; j++) {
00388 p1[-j] = p1[0];
00389 p2[-j] = p2[0];
00390 p3[-j] = p3[0];
00391 }
00392 }
00393 else {
00394 p1 = buf1+x; p2 = buf2+x; p3 = buf3+x;
00395 for (j=1; j <= (int)(xf)-x; j++) {
00396 p1[sx+j-1] = p1[sx-1];
00397 p2[sx+j-1] = p2[sx-1];
00398 p3[sx+j-1] = p3[sx-1];
00399 }
00400 }
00401
00402
00403 x = (int)xf;
00404 c = xf-(double)x;
00405 d = 1.0-c;
00406
00407 e = c; f = d;
00408 if (!q4) {
00409 res = d; d = c; c = res;
00410 res = f; f = e; e = res;
00411 }
00412
00413 c *= sumsq; d *= sumsq;
00414
00415
00416 if (q4) {
00417 p0 = buf0-x; p1 = buf1-x; p2 = buf2-x; p3 = buf3-x;
00418 }
00419 else {
00420 p0 = buf0+x; p1 = buf1+x; p2 = buf2+x; p3 = buf3+x;
00421 }
00422 prev = *src;
00423 prevres = sumsq*prev + b1 * *p1 + b2 * *p2 + b3 * *p3;
00424
00425
00426 for (j = 0; j < sx; j++) {
00427 pix = *src++;
00428 val = c*pix+d*prev;
00429 prev = pix;
00430
00431 res = val + b1 * *p1++ + b2 * *p2++ + b3 * *p3++;
00432 *p0++ = res;
00433 *dest++ = f*res+e*prevres;
00434 prevres = res;
00435 }
00436
00437
00438 p0 = buf3; buf3 = buf2; buf2 = buf1; buf1 = buf0; buf0 = p0;
00439 }
00440
00441
00442
00443
00444 b1 = filter[4]; b2 = filter[5]; b3 = filter[6];
00445 TriggsM(filter, M);
00446
00447
00448 x = (int)((sy-1)*tanp);
00449 if (q4) {
00450 p1 = buf1+sx-x; p2 = buf2+sx-x; p3 = buf3+sx-x;
00451 }
00452 else {
00453 p1 = buf1+sx+x; p2 = buf2+sx+x; p3 = buf3+sx+x;
00454 }
00455 p0 = uplusbuf+sx;
00456 for (j = 0; j < sx; j++) {
00457 uplus = *(--p0);
00458 vplus = uplus/(1.0-b1-b2-b3);
00459
00460 unp = *(--p1)-uplus;
00461 unp1 = *(--p2)-uplus;
00462 unp2 = *(--p3)-uplus;
00463 pix = M[0]*unp+M[1]*unp1+M[2]*unp2 + vplus;
00464 pix *= sumsq;
00465 *(--dest) = pix; *p1 = pix;
00466 pix = M[3]*unp+M[4]*unp1+M[5]*unp2 + vplus;
00467 *p2 = pix*sumsq;
00468 pix = M[6]*unp+M[7]*unp1+M[8]*unp2 + vplus;
00469 *p3 = pix*sumsq;
00470 }
00471
00472 for (i = sy-2; i >= 0; i--) {
00473 xf = i*tanp;
00474
00475
00476 if (q4) {
00477 p1 = buf1-x; p2 = buf2-x; p3 = buf3-x;
00478 for (j=1; j <= x-(int)(xf); j++) {
00479 p1[sx+j-1] = p1[sx-1];
00480 p2[sx+j-1] = p2[sx-1];
00481 p3[sx+j-1] = p3[sx-1];
00482 }
00483 }
00484 else {
00485 p1 = buf1+x; p2 = buf2+x; p3 = buf3+x;
00486 for (j=1; j <= x-(int)(xf); j++) {
00487 p1[-j] = p1[0];
00488 p2[-j] = p2[0];
00489 p3[-j] = p3[0];
00490 }
00491 }
00492
00493
00494 x = (int)xf;
00495 c = xf-(double)x;
00496 d = 1.0-c;
00497
00498 e = c; f = d;
00499 c *= sumsq; d *= sumsq;
00500
00501 if (!q4) {
00502 res = d; d = c; c = res;
00503 res = f; f = e; e = res;
00504 }
00505
00506
00507 if (q4) {
00508 p0 = buf0+sx-x; p1 = buf1+sx-x; p2 = buf2+sx-x; p3 = buf3+sx-x;
00509 }
00510 else {
00511 p0 = buf0+sx+x; p1 = buf1+sx+x; p2 = buf2+sx+x; p3 = buf3+sx+x;
00512 }
00513 prev = *(dest-1);
00514 prevres = sumsq*prev + b1 * *(p1-1) + b2 * *(p2-1) + b3 * *(p3-1);
00515
00516
00517 for (j = 0; j < sx; j++) {
00518 pix = *(--dest);
00519 val = d*pix+c*prev;
00520 prev = pix;
00521
00522 res = val + b1 * *(--p1) + b2 * *(--p2) + b3 * *(--p3);
00523 *(--p0) = res;
00524 *dest = e*res+f*prevres;
00525 prevres = res;
00526 }
00527
00528
00529 p0 = buf3; buf3 = buf2; buf2 = buf1; buf1 = buf0; buf0 = p0;
00530 }
00531
00532 if (q4) {
00533 buf0 -= (int)(sy*tanp+1);
00534 buf1 -= (int)(sy*tanp+1);
00535 buf2 -= (int)(sy*tanp+1);
00536 buf3 -= (int)(sy*tanp+1);
00537 }
00538 free(buf0);
00539 free(buf1);
00540 free(buf2);
00541 free(buf3);
00542 free(uplusbuf);
00543 }
00544
00545
00546 static void
00547 f_iir_derivative_filter(DSTTYPE *src, DSTTYPE *dest, int sx, int sy,
00548 double phi, int order)
00549 {
00550 int i, j;
00551 DSTTYPE *prev, *center, *next;
00552 DSTTYPE *buf, *pstore;
00553 double pn, pc, pp;
00554 double cn, cc, cp;
00555 double nn, nc, np;
00556 double cosp, sinp;
00557
00558 buf = (double*)malloc(sx*sizeof(*buf));
00559
00560 sinp = 0.5*sin(phi); cosp = 0.5*cos(phi);
00561
00562 center = src; prev = src; next = src+sx;
00563 for (i = 0; i < sy; i++) {
00564 pstore = buf;
00565 pn = *prev++; cn = *center++; nn = *next++;
00566 pp = pn; pc = pn;
00567 cp = cn; cc = cn;
00568 np = pn; nc = nn;
00569 *pstore++ = cc;
00570 for (j = 1; j < sx; j++) {
00571 pn = *prev++;
00572 cn = *center++;
00573 nn = *next++;
00574 *dest++ = sinp*(pc-nc)+cosp*(cn-cp);
00575 pp = pc; pc = pn;
00576 cp = cc; cc = cn;
00577 np = pc; nc = nn;
00578 *pstore++ = cc;
00579 }
00580 *dest++ = sinp*(pc-nc)+cosp*(cn-cp);
00581 prev = buf;
00582 if (i==sy-2)
00583 next -= sx;
00584 }
00585
00586 free(buf);
00587 }
00588