filter_median.c

00001 /*                                                                              *
00002  *   This program is free software; you can redistribute it and/or modify       *
00003  *   it under the terms of the GNU General Public License as published by       *
00004  *   the Free Software Foundation; either version 2 of the License, or          *
00005  *   (at your option) any later version.                                        *
00006  *                                                                              *
00007  *   This program is distributed in the hope that it will be useful,            *
00008  *   but WITHOUT ANY WARRANTY; without even the implied warranty of             *
00009  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              *
00010  *   GNU General Public License for more details.                               *
00011  *                                                                              *
00012  *   You should have received a copy of the GNU General Public License          *
00013  *   along with this program; if not, write to the Free Software                *
00014  *   Foundation, 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA       *
00015  *                                                                              */
00016 
00017 #include <assert.h>
00018 #include <string.h>
00019 #include <stdlib.h>
00020 
00028 #ifndef inline
00029 #define inline /* inline */
00030 #endif
00031 
00032 #ifndef IN_TYPE
00033 #error IN_TYPE not defined
00034 #endif
00035 
00036 #ifndef IN_MAX
00037 #error IN_MAX not defined
00038 #endif
00039 
00040 #ifndef IN_MIN
00041 #error IN_MIN not defined
00042 #endif
00043 
00044 #ifndef OUT_TYPE
00045 #define OUT_TYPE IN_TYPE
00046 #endif
00047 
00048 
00049 #define assure(x) assert(x)
00050 
00051 #ifdef bool
00052 #define mybool bool
00053 #else
00054 #define mybool unsigned char
00055 #endif
00056 
00057 #ifdef true
00058 #define mytrue true
00059 #else
00060 #define mytrue 1
00061 #endif
00062 
00063 #ifdef false
00064 #define myfalse false
00065 #else
00066 #define myfalse 0
00067 #endif
00068 
00070 typedef struct dheap {
00071     unsigned *heaps;
00072     /* Double binary heap and median element. Values are pixel buffer indices in the
00073        current filtering window.
00074        heaps[0] ... heaps[m-1]    : a maximum heap of pixels with values less than
00075                                     median
00076        heaps[m]                   : median
00077        heaps[m+1] ... heaps[2m]   : a minimum heap of pixels with values greater 
00078                                     than median
00079     */
00080     unsigned *B; /* Size Nx x Ny map from image positions to double heap positions.
00081                     This is the inverse map of the heaps[] array, 
00082                        B[heaps[i]] = i.
00083                     It allows constant time look-up of any pixel in the double heap
00084                     array.
00085 
00086                     Notice that only the pixels in the current window need to be
00087                     simultansously stored in B, therefore B could be compressed to
00088                     size (2rx+1)(2ry+1)^2. However, in that case the overall execution time
00089                     would be dominated by mapping pixels to positions in B.
00090                  */
00091     unsigned *SCA; /* Array Nx*(2Ry+1) of sorted column arrays. Values are image pixel
00092                       buffer indices. Each column array is sorted according to image
00093                       buffer pixel values. This buffer is laid out in column major
00094                       order to allow fast iteration within each column array (i.e.
00095                       the x'th column array is at
00096                       {S[x*(2Ry+1)], ..., S[x*(2Ry+1) + 2Ry]}) 
00097                    */
00098     const IN_TYPE *image;
00099     unsigned rx; //kernel radius
00100     unsigned ry; //kernel radius
00101     unsigned Nx; //image width
00102     unsigned Ny; //image height
00103     unsigned m;  //kernel window half size, must be positive
00104     unsigned Rx;  //2rx + 1
00105     unsigned Ry;  //2ry + 1
00106 } dheap;
00117 #define S_SWAP(SCA, i, j) \
00118 do { \
00119     const unsigned SCAi = SCA[i]; \
00120     const unsigned SCAj = SCA[j]; \
00121     SCA[i] = SCAj; \
00122     SCA[j] = SCAi; \
00123 } while(0)
00124 
00125 
00126 #define BITS 64
00127 
00135 static void
00136 qsort_int(unsigned *SCA, 
00137           unsigned n, const IN_TYPE *image)
00138 {
00139     int i, ir, j, k, l;
00140     int i_stack[BITS];
00141     int j_stack ;
00142     int a;
00143 
00144     IN_TYPE ia;
00145 
00146     //assure( n <= 1 << BITS );  /* generates warnings */
00147     /* true because no-one ever wants to filter with a radius larger than 2^BITS */
00148 
00149     ir = n ;
00150     l = 1 ;
00151     j_stack = 0 ;
00152     while (1) {
00153         if (ir-l < 7) {
00154             for (j=l+1 ; j<=ir ; j++) {
00155                 a = SCA[j-1];
00156                 ia = image[a];
00157                 for (i=j-1 ; i>=1 ; i--) {
00158                     if (image[SCA[i-1]] <= ia) break;
00159                     SCA[i] = SCA[i-1];
00160                 }
00161                 SCA[i] = a;
00162             }
00163             if (j_stack == 0) break;
00164             ir = i_stack[j_stack-- -1];
00165             l  = i_stack[j_stack-- -1];
00166         } else {
00167             /* The following is buggy and has O(n^2) behaviour if the
00168                input array is sorted in reverse order.
00169                For maximum performance we should select the pivot randomly,
00170                (however this step is used only in the initialization of 
00171                the sorted column arrays, so be it) */
00172             k = (l+ir) >> 1;
00173             S_SWAP(SCA, k-1, l);
00174             if (image[SCA[l]] > image[SCA[ir-1]]) {
00175                 S_SWAP(SCA, l, ir-1);
00176             }
00177             if (image[SCA[l-1]] > image[SCA[ir-1]]) {
00178                 S_SWAP(SCA, l-1, ir-1);
00179             }
00180             if (image[SCA[l]] > image[SCA[l-1]]) {
00181                 S_SWAP(SCA, l, l-1);
00182             }
00183 
00184             i = l+1;
00185             j = ir;
00186             a = SCA[l-1];
00187             ia = image[a];
00188             for (;;) {
00189                 do i++; while (image[SCA[i-1]] < ia);
00190                 do j--; while (image[SCA[j-1]] > ia);
00191                 if (j < i) break;
00192                 S_SWAP(SCA, i-1, j-1);
00193             }
00194             SCA[l-1] = SCA[j-1];
00195             SCA[j-1] = a;
00196             j_stack += 2;
00197 
00198             assert( j_stack <= BITS );
00199 
00200             if (ir-i+1 >= j-l) {
00201                 i_stack[j_stack-1] = ir;
00202                 i_stack[j_stack-2] = i;
00203                 ir = j-1;
00204             } else {
00205                 i_stack[j_stack-1] = j-1;
00206                 i_stack[j_stack-2] = l;
00207                 l = i;
00208             }
00209         }
00210     }
00211 
00212     return;
00213 }
00214 
00220 static void
00221 SCA_init(dheap *dh, unsigned x)
00222 {
00223     unsigned y;
00224     unsigned *SCAx = dh->SCA + (dh->Ry)*x;
00225 
00226     for (y = 0; y < dh->Ry; y++) {
00227         SCAx[y] = x + y*dh->Nx;
00228     }
00229 
00230     qsort_int(SCAx,
00231               dh->Ry,
00232               dh->image);
00233     
00234     return;
00235 }
00236 
00244 static void
00245 SCA_replace(dheap *dh, unsigned x, 
00246             unsigned yold, unsigned ynew)
00247 {
00248     unsigned n = dh->Ry;
00249     unsigned *SCAx = dh->SCA + n*x; //start of current column array
00250     const IN_TYPE *im = dh->image;
00251 
00252     unsigned i;
00253     unsigned aold = x + yold*dh->Nx;
00254     unsigned anew = x + ynew*dh->Nx;
00255 
00256     /* Do a linear search for the old pixel and replace it with
00257        the new.
00258 
00259        (Since SCAx is sorted according
00260        to pixel value, we could do a binary search here.
00261        This turns out to be no faster in practice,
00262        up to at least r = 64)
00263     */
00264     for (i = 0; i < n; i++) {
00265         if (SCAx[i] == aold) break;
00266     }
00267     SCAx[i] = anew;
00268 
00269     /* The new pixel is not necessarily in it the right place.
00270        Bubble until SCAx is again sorted.
00271     */
00272     if (i+1 < n && 
00273         im[SCAx[i  ]] >
00274         im[SCAx[i+1]]) {
00275         
00276         S_SWAP(SCAx, i  , i+1);
00277         i++;
00278         
00279         while (i+1 < n && 
00280                im[SCAx[i  ]] >
00281                im[SCAx[i+1]]) {
00282             S_SWAP(SCAx, i  , i+1);
00283             i++;
00284         }
00285     }
00286     else {
00287         while (i >= 1 && 
00288                im[SCAx[i  ]] <
00289                im[SCAx[i-1]]) {
00290             S_SWAP(SCAx, i  , i-1);
00291             i--;
00292         }
00293     }
00294     
00295     return;
00296 }
00297 
00298 
00306 static void
00307 HEAP_SWAP(unsigned i, 
00308           unsigned j,
00309           unsigned int *B,
00310           unsigned int *heaps)
00311 {
00312     unsigned ai = heaps[i];
00313     unsigned aj = heaps[j];
00314     heaps[i] = aj;
00315     heaps[j] = ai;
00316 
00317     B[aj] = i;
00318     B[ai] = j;
00319 
00320   return;
00321 }
00322 
00327 static void
00328 median(dheap *dh)
00329 {
00330     unsigned n = 2 * dh->m + 1;
00331     
00332     const unsigned k = (n-1)/2;
00333     IN_TYPE pivot;
00334     unsigned i, j, lo, hi;
00335 
00336     lo = 0;
00337     hi = n - 1;
00338     
00339     while (lo < hi) {
00340         /* Buggy. We should select the pivot randomly,
00341            or there is O(n^2) behaviour for certain inputs */
00342         pivot = dh->image[dh->heaps[k]];
00343         i = lo;
00344         j = hi;
00345         do {
00346             while (dh->image[dh->heaps[i]] < pivot) i++;
00347             while (dh->image[dh->heaps[j]] > pivot) j--;
00348             if (i <= j) {
00349                 if (i != j) HEAP_SWAP(i, j, dh->B, dh->heaps);
00350                 i++;
00351                 j--;
00352             }
00353         } while (i <= j);
00354         
00355         if (j < k) lo = i;
00356         if (k < i) hi = j;
00357     }
00358     
00359     return;
00360 }
00361 
00362 
00373 static unsigned
00374 bubble_down_gt(const IN_TYPE *image,
00375                unsigned *B,
00376                unsigned *heaps,
00377                unsigned m,
00378                unsigned m1,
00379                unsigned root)
00380 {
00381     unsigned child;
00382     unsigned end = m-1;
00383 
00384     while (root * 2 + 1 <= end) {
00385         child = root*2 + 1;
00386 
00387         if (child < end && 
00388             image[heaps[m1 + child]] > 
00389             image[heaps[m1 + child + 1]]) {
00390             child = child + 1;
00391         }
00392         if (image[heaps[m1 + root]] > 
00393             image[heaps[m1 + child]]) {
00394             HEAP_SWAP(m1 + root, m1 + child, B, heaps);
00395             root = child;
00396         }
00397         else {
00398             return root;
00399         }
00400     }
00401     return root;
00402 }
00403 
00413 static unsigned
00414 bubble_up_gt(const IN_TYPE *image,
00415              unsigned *B,
00416              unsigned *heaps, 
00417              unsigned m1,
00418              unsigned child)
00419 {
00420     unsigned parent;
00421 
00422     while (child > 0) {
00423         parent = (child - 1) / 2;
00424 
00425         if (image[heaps[m1 + parent]] >
00426             image[heaps[m1 + child]]) {
00427             HEAP_SWAP(m1 + parent, m1 + child, B, heaps);
00428             child = parent;
00429         }
00430         else return child;
00431     }
00432     return child;
00433 }
00434 
00439 static void
00440 heapify_gt(dheap *dh)
00441 {
00442     int start = dh->m/2-1;
00443 
00444     while (start >= 0) {
00445         bubble_down_gt(dh->image,
00446                        dh->B,
00447                        dh->heaps,
00448                        dh->m,
00449                        dh->m+1, start);
00450         start = start - 1;
00451     }
00452     return;
00453 }
00454 
00464 static unsigned
00465 bubble_down_lt(const IN_TYPE *image,
00466                unsigned *B,
00467                unsigned *heaps,
00468                unsigned end, unsigned root)
00469 {
00470     unsigned child;
00471 
00472     while (root * 2 + 1 <= end) {
00473         child = root*2 + 1;
00474         
00475         if (child < end &&
00476             image[heaps[child]] <
00477             image[heaps[child + 1]]) {
00478             child = child + 1;
00479         }
00480         if (image[heaps[root]] <
00481             image[heaps[child]]) {
00482             HEAP_SWAP(root, child, B, heaps);
00483             root = child;
00484         }
00485         else {
00486             return root;
00487         }
00488     }
00489     return root;
00490 }
00491 
00500 static unsigned
00501 bubble_up_lt(const IN_TYPE *image,
00502              unsigned *B,
00503              unsigned *heaps, unsigned child)
00504 {
00505     unsigned parent;
00506 
00507     while (child > 0) {
00508         parent = (child - 1) / 2;
00509 
00510         if (image[heaps[parent]] <
00511             image[heaps[child]]) {
00512             HEAP_SWAP(parent, child, B, heaps);
00513             child = parent;
00514         }
00515         else return child;
00516     }
00517     return child;
00518 }
00519 
00524 static void
00525 heapify_lt(dheap *dh)
00526 {
00527     int start = dh->m/2-1;
00528 
00529     while (start >= 0) {
00530         bubble_down_lt(dh->image,
00531                        dh->B,
00532                        dh->heaps,
00533                        dh->m-1, start);
00534         start = start - 1;
00535     }
00536     return;
00537 } 
00538 
00543 static void
00544 dheap_establish(dheap *dh)
00545 {
00546     median(dh);
00547     
00548     heapify_lt(dh);
00549     heapify_gt(dh);
00550     
00551     return;
00552 }
00553 
00566 static dheap *
00567 dheap_new(const IN_TYPE *image, unsigned Nx, unsigned Ny, unsigned rx, unsigned ry)
00568 {
00569     dheap *dh = malloc(sizeof(*dh));
00570     assure( dh != NULL );
00571     assure( rx > 0 || ry > 0 );
00572 
00573     dh->heaps = malloc( (2*rx+1)*(2*ry+1) * sizeof(*dh->heaps));
00574     dh->B     = malloc( Nx*Ny             * sizeof(*dh->B));
00575     dh->SCA   = malloc( Nx*(2*ry+1)       * sizeof(*dh->SCA));
00576     assure( dh->heaps != NULL );
00577     assure( dh->B != NULL );
00578     assure( dh->SCA != NULL );
00579     
00580     dh->image = image;
00581     dh->m     = ((2*rx+1)*(2*ry+1))/2;
00582     dh->Nx    = Nx;
00583     dh->Ny    = Ny;
00584     dh->rx    = rx;
00585     dh->ry    = ry;
00586     dh->Rx    = 2*rx+1;
00587     dh->Ry    = 2*ry+1;
00588 
00589     {
00590         unsigned k = 0;
00591         unsigned x, y;
00592         
00593         for (y = 0; y < dh->Ry; y++) {
00594             for (x = 0; x < dh->Rx; x++) {
00595                 unsigned ai = x + y * dh->Nx;
00596                 dh->heaps[k]  = ai;
00597                 dh->B[ai] = k;
00598                 k++;
00599             }
00600         }
00601     }
00602     dheap_establish(dh);
00603 
00604     {
00605         unsigned x;
00606         for (x = 0; x < 2*dh->rx; x++)
00607         {
00608             SCA_init(dh, x);
00609         }
00610     }
00611 
00612     return dh;
00613 }
00614 
00619 static void
00620 dheap_delete(dheap *dh)
00621 {
00622     free(dh->SCA);
00623     free(dh->B);
00624     free(dh->heaps);
00625     free(dh);
00626 
00627     return;
00628 }
00629 
00635 static IN_TYPE
00636 dheap_median(const dheap *dh)
00637 {
00638     return dh->image[dh->heaps[dh->m]];
00639 }
00640 
00658 static void
00659 dheap_replace(const IN_TYPE *image,
00660               unsigned *B,
00661               unsigned *heaps,
00662               unsigned m,
00663               unsigned m1,
00664               unsigned anew,
00665               unsigned aold)
00666 {
00667     static mybool prev_was_larger  = myfalse;
00668     static mybool prev_was_smaller = myfalse;
00669     /* These flags remember if the previously inserted element was larger/smaller
00670        than the one it replaced. Both flags may be false, which means nothing is known.
00671 
00672        If the previous element was larger (smaller), then the current element
00673        we are inserting now is likely to be also larger (smaller). This information is
00674        used to decrease the number of comparisons needed to bubble the current
00675        element in place.
00676 
00677        To make things thread-safe, these flags could be made in/out parameters
00678        (type mybool*) of this function.
00679     */
00680 
00681     unsigned pos;
00682 
00683     /* Lookup position of old pixel in heaps structure, overwrite with
00684        new pixel */
00685     unsigned ci = B[aold]; 
00686     heaps[ci] = anew;
00687     B[anew] = ci;
00688 
00689     /* Move new element to a correct place. 
00690        Swap heaps priority element with median as needed */
00691     if (ci < m) {
00692         /* Lower heap */
00693         if (prev_was_smaller) {
00694             pos = bubble_down_lt(image, B, heaps, m-1,
00695                                  ci);
00696             if (pos == ci) pos = bubble_up_lt(image, B, heaps, ci);
00697         }
00698         else 
00699             /* previous was larger or nothing is known */
00700         {
00701             /* Optimistic guess optimization here: bubble_up requires only one comparison
00702                and is faster than bubble_down. Therefore, in the case where nothing is known
00703                about the previous element, we try to bubble_up before bubble_down.
00704             */
00705             pos = bubble_up_lt(image, B, heaps, ci);
00706             if (pos == ci) pos = bubble_down_lt(image, B, heaps, m-1, ci);
00707         }
00708         
00709         if (pos == 0 &&
00710             image[heaps[0]] > image[heaps[m]]) {
00711             HEAP_SWAP(0, m, B, heaps);
00712             
00713             if (image[heaps[m1]] < 
00714                 image[heaps[m]]) {
00715                 HEAP_SWAP(m1, m, B, heaps);
00716                 bubble_down_gt(image, B, heaps, m, m1, 0);
00717             }
00718         }
00719 
00720         /* Finally prepare flags for the next call of this function.
00721            If final position and initial postions are the same,
00722            we cannot tell if this element was larger/smaller than the
00723            one it replaced (at least not without the cost of a comparison with
00724            the previous element, which may or may not pay off).
00725         */
00726         prev_was_larger  = (pos < ci);
00727         prev_was_smaller = (pos > ci);
00728         return;
00729     }
00730     else if (ci >= m1) {
00731         /* Upper heap */
00732         if (prev_was_larger) {
00733             pos = bubble_down_gt(image, B, heaps, m, m1, ci-(m1));
00734             if (pos == ci-(m1)) 
00735                 pos = bubble_up_gt(image, B, heaps, m1, ci-(m1));
00736         }
00737         else {
00738             pos = bubble_up_gt(image, B, heaps, m1, ci-(m1));
00739             if (pos == ci-(m1)) 
00740                 pos = bubble_down_gt(image, B, heaps, m, m1, ci-(m1));
00741         }
00742 
00743         if (pos == 0 &&
00744             image[heaps[m1]] < image[heaps[m]]) {
00745             HEAP_SWAP(m1, m, B, heaps);
00746             
00747             if (image[heaps[0]] > image[heaps[m]]) {
00748                 HEAP_SWAP(0, m, B, heaps);
00749                 bubble_down_lt(image, B, heaps, m-1, 0);
00750             }
00751         }
00752 
00753         prev_was_smaller = (pos < ci-(m1));
00754         prev_was_larger  = (pos > ci-(m1));
00755         return;
00756     }
00757     else {
00758         /* The median element was replaced */
00759         if (prev_was_smaller) {
00760             if (image[heaps[0]] > image[heaps[m]]) {
00761                 HEAP_SWAP(0, m, B, heaps);
00762                 bubble_down_lt(image, B, heaps, m-1, 0);
00763                 prev_was_smaller = mytrue;
00764                 prev_was_larger = myfalse;
00765                 return;
00766             } 
00767             else if (image[heaps[m1]] < 
00768                      image[heaps[m]]) {
00769                 HEAP_SWAP(m1, m, B, heaps);
00770                 bubble_down_gt(image, B, heaps, m, m1, 0);
00771                 prev_was_smaller = myfalse;
00772                 prev_was_larger = mytrue;
00773                 return;
00774             } else {
00775                 prev_was_smaller = myfalse;
00776                 prev_was_larger  = myfalse;
00777                 return;
00778             }
00779         } else {
00780             if (image[heaps[m1]] < 
00781                 image[heaps[m]]) {
00782                 HEAP_SWAP(m1, m, B, heaps);
00783                 bubble_down_gt(image, B, heaps, m, m1, 0);
00784                 prev_was_smaller = myfalse;
00785                 prev_was_larger = mytrue;
00786                 return;
00787             } else if (image[heaps[0]] > image[heaps[m]]) {
00788                 HEAP_SWAP(0, m, B, heaps);
00789                 bubble_down_lt(image, B, heaps, m-1, 0);
00790                 prev_was_smaller = mytrue;
00791                 prev_was_larger = myfalse;
00792                 return;
00793             } else {
00794                 prev_was_smaller = myfalse;
00795                 prev_was_larger  = myfalse;
00796                 return;
00797             }
00798         }
00799     }
00800 }
00801 
00802 static
00803 IN_TYPE get_kth(IN_TYPE * a, unsigned n, int k)
00804 {
00805     IN_TYPE x;
00806     int     i, j, l, m;
00807 
00808     l=0; m=n-1;
00809     while (l<m) {
00810         x=a[k];
00811         i=l;
00812         j=m;
00813         do {
00814             while (a[i]<x) i++;
00815             while (x<a[j]) j--;
00816             if (i<=j) {
00817                 const IN_TYPE temp = a[i];
00818                 a[i] = a[j];
00819                 a[j] = temp;
00820                 i++; j--;
00821             }
00822         } while (i<=j);
00823         if (j<k) l=i;
00824         if (k<i) m=j;
00825     }
00826     return a[k];
00827 }
00828 
00838 static inline IN_TYPE
00839 max3(IN_TYPE p0,
00840      IN_TYPE p1,
00841      IN_TYPE p2)
00842 {
00843     return (p0 > p1) 
00844         ?
00845         ((p0 > p2) ? p0 : p2)
00846         :
00847         ((p1 > p2) ? p1 : p2);
00848 }
00849 
00859 static inline IN_TYPE
00860 min3(IN_TYPE p0,
00861      IN_TYPE p1,
00862      IN_TYPE p2)
00863 {
00864     return (p0 < p1)
00865         ?
00866         ((p0 < p2) ? p0 : p2)
00867         :
00868         ((p1 < p2) ? p1 : p2);
00869 }
00870 
00887 static inline IN_TYPE
00888 median5(IN_TYPE p0,
00889         IN_TYPE p1,
00890         IN_TYPE p2,
00891         IN_TYPE p3,
00892         IN_TYPE p4)
00893 {
00894     /*
00895       This would be a one-off median filter (i.e. rank 4th, 5th
00896       or 6th of 9). Saves some 35% execution time.
00897 
00898       return p1;
00899     */
00900     
00901     return (p3 < p1)
00902         ?
00903         ((p4 >= p1) ? p1 : max3(p3, p4, p0))
00904         :
00905         ((p4 <= p1) ? p1 : min3(p3, p4, p2));
00906 }
00907 
00932 static inline IN_TYPE
00933 median9_2(IN_TYPE p0,
00934           IN_TYPE p1,
00935           IN_TYPE p2,
00936           IN_TYPE p3,
00937           IN_TYPE p4,
00938           IN_TYPE p5,
00939           IN_TYPE p6,
00940           IN_TYPE p7,
00941           IN_TYPE p8)
00942 {
00943     return (p4 <= p7) 
00944         ? 
00945         median5(p3, p4, p5, p2, p6)
00946         :
00947         ((p1 < p7) ? median5(p6, p7, p8, p2, p3)
00948          : median5(p0, p1, p2, p8, p3));
00949 }
00970 static inline IN_TYPE
00971 median9_1(IN_TYPE p0,
00972           IN_TYPE p1,
00973           IN_TYPE p2,
00974           IN_TYPE p3,
00975           IN_TYPE p4,
00976           IN_TYPE p5,
00977           IN_TYPE p6,
00978           IN_TYPE p7,
00979           IN_TYPE p8)
00980 {
00981     return (p1 < p4) ? median9_2(p0, p1, p2, p3, p4, p5, p6, p7, p8)
00982              : median9_2(p3, p4, p5, p0, p1, p2, p6, p7, p8);
00983 }
00984 
00998 #define sort3(p0, p1, p2, i0, i1, i2)                         \
00999 do {                                                          \
01000     if (i0 <= i1) {                                           \
01001         if (i1 <= i2) {                                       \
01002              p0 = i0; p1 = i1; p2 = i2;                       \
01003         } else if (i0 <= i2) {                                \
01004             /* i0, i2, i1 */                                  \
01005             p0 = i0; p1 = i2; p2 = i1;                        \
01006         } else {                                              \
01007             /*  i2, i0, i1 */                                 \
01008             p0 = i2; p1 = i0; p2 = i1;                        \
01009         }                                                     \
01010     } else {                                                  \
01011         if (i0 <= i2) {                                       \
01012             /* i1, i0, i2 */                                  \
01013             p0 = i1; p1 = i0; p2 = i2;                        \
01014         } else if (i1 < i2) {                                 \
01015             /* i1, i2, i0 */                                  \
01016             p0 = i1; p1 = i2; p2 = i0;                        \
01017         } else {                                              \
01018             /* i2, i1, i0 */                                  \
01019             p0 = i2; p1=i1; p2 = i0;                          \
01020         }                                                     \
01021     }                                                         \
01022 } while(0)
01023 
01035 static void
01036 filter_median_1(const IN_TYPE *in, OUT_TYPE *out,
01037                 unsigned Nx, unsigned Ny, unsigned border_mode)
01038 {
01039     register IN_TYPE p0, p1, p2, p3, p4, p5;
01040     register IN_TYPE p6 = (IN_TYPE)0; /* Fix (false) uninit warning */
01041     register IN_TYPE p7 = (IN_TYPE)0; /* Fix (false) uninit warning */
01042     register IN_TYPE p8 = (IN_TYPE)0; /* Fix (false) uninit warning */
01043     const IN_TYPE * i0 = in;
01044     const IN_TYPE * i1 = i0 + Nx;
01045     const IN_TYPE * i2 = i1 + Nx;
01046     const IN_TYPE * istop = in + Nx*Ny; /* First pixel not to be accessed */
01047     unsigned m = 2;  /* lower median of 6 elements */
01048 
01049     assure( Nx >= 3 );
01050     assure( Ny >= 3 );
01051 
01052     switch (border_mode) {
01053         case IRPLIB_FILTER_BORDER_FILTER: {
01054             IN_TYPE buf6[6];
01055             unsigned j = 0;
01056 
01057             buf6[0] = i0[0];
01058             buf6[1] = i0[1];
01059             buf6[2] = i1[0];
01060             buf6[3] = i1[1];
01061             out[j++] = get_kth(buf6, 4, 2); /* upper median */
01062 
01063             for (; j < Nx-1; j++) {
01064                 memcpy(buf6,   i0+j-1, 3*sizeof(*buf6));
01065                 memcpy(buf6+3, i1+j-1, 3*sizeof(*buf6));
01066                 out[j] = get_kth(buf6, 6, m);
01067                 m = 5-m; /* alternate between lower/upper median */
01068             }
01069             buf6[0] = i0[Nx-2];
01070             buf6[1] = i0[Nx-1];
01071             buf6[2] = i1[Nx-2];
01072             buf6[3] = i1[Nx-1];
01073             out[j] = get_kth(buf6, 4, m-1);
01074             out += Nx;
01075 
01076             break;
01077         }
01078         case IRPLIB_FILTER_BORDER_COPY: {
01079             /* From the first row copy all but the last pixel */
01080             if (sizeof(IN_TYPE) == sizeof(OUT_TYPE)) {
01081                 (void)memcpy(out, in, (Nx-1)*sizeof(*out));
01082             } else {
01083                 unsigned i;
01084                 for (i=0; i < Nx-1; i++) out[i] = (OUT_TYPE)in[i];
01085             }
01086         }
01087         case IRPLIB_FILTER_BORDER_EXTRAPOL_OUT:
01088         case IRPLIB_FILTER_BORDER_NOP:
01089             out += Nx-1;
01090             break;
01091         default:
01092         case IRPLIB_FILTER_BORDER_CROP:
01093             break;
01094     }
01095 
01096     if (border_mode == IRPLIB_FILTER_BORDER_FILTER) m = 2; /* lower median */
01097 
01098     for (;i2 < istop; i0 += Nx, i1 += Nx, i2 += Nx, out += Nx) {
01099 
01100         unsigned char k = 2;
01101         unsigned j = 0;
01102 
01103         /* Start a new row */
01104 
01105         switch (border_mode) {
01106             case IRPLIB_FILTER_BORDER_FILTER: {
01107                 IN_TYPE buf6[6];
01108 
01109                 buf6[0] = i0[0];
01110                 buf6[1] = i1[0];
01111                 buf6[2] = i2[0];
01112                 buf6[3] = i0[1];
01113                 buf6[4] = i1[1];
01114                 buf6[5] = i2[1];
01115                 out[0] = get_kth(buf6, 6, m);
01116 
01117                 /* j must be incremented twice, but out[j] should
01118                    only be incremented once */
01119                 out--;
01120 
01121                 break;
01122             }
01123             case IRPLIB_FILTER_BORDER_CROP:
01124                 out -= 2; /* Nx - 2 medians in each row */
01125                 break;
01126             case IRPLIB_FILTER_BORDER_COPY: {
01127                 /* Copy last pixel from previous row and
01128                    first pixel from current row */
01129                 out[0] = (IN_TYPE)i0[Nx-1];
01130                 out[1] = (IN_TYPE)i0[Nx];
01131                 break;
01132             }
01133             default:
01134                 break;
01135         }
01136 
01137         sort3(p0, p1, p2, i0[j], i1[j], i2[j]);
01138         j++;
01139         
01140         sort3(p3, p4, p5, i0[j], i1[j], i2[j]);
01141         j++;
01142 
01143         for (; j < Nx; j++) {
01144 
01145             if (k == 0) {
01146                 k = 1;
01147                 sort3(p0, p1, p2, i0[j], i1[j], i2[j]);
01148             }
01149             else if (k == 1) {
01150                 k = 2;
01151                 sort3(p3, p4, p5, i0[j], i1[j], i2[j]);
01152             }
01153             else {
01154                 k = 0;
01155                 sort3(p6, p7, p8, i0[j], i1[j], i2[j]);
01156             }
01157         
01158             out[j]
01159                 = (OUT_TYPE)median9_1(p0, p1, p2, p3, p4, p5, p6, p7, p8);
01160 
01161             /* The cost per pixel was determined by considering the 9! possible
01162                permutations of 9 different numbers and subsequently confirmed by
01163                experiment:
01164 
01165                2.66 + 5.52 = 172/21 = 8.19 comparisons per pixel
01166                3 + 3 + 4 = 10  (worst case)
01167                
01168                In the case of two or more equal elements the cost is always the
01169                same or less (down to 6 for all equal elements) due to the way the
01170                comparisons are made.
01171 
01172                If sorted column arrays were used the cost would be one less
01173                1.66 + 5.52 = 151/21 = 7.19
01174                (worst case 9)
01175                
01176                A rank 4th-6th filter would cost 2.86 comparisons less
01177                
01178                2.66 + 2.66 = 16/3 = 5.33
01179                
01180                or only 4.33 comparisons per pixel if SCAs are used.
01181             */
01182         }
01183 
01184         if (border_mode == IRPLIB_FILTER_BORDER_FILTER) {
01185             IN_TYPE buf6[6];
01186 
01187             buf6[0] = i0[j-2];
01188             buf6[1] = i1[j-2];
01189             buf6[2] = i2[j-2];
01190             buf6[3] = i0[j-1];
01191             buf6[4] = i1[j-1];
01192             buf6[5] = i2[j-1];
01193 
01194             out[j] = get_kth(buf6, 6, ((Nx & 1) == 1) ? m : 5-m);
01195             m = 5-m;
01196 
01197             /* out was decremented, increment it back */
01198             out++;
01199         } else if (border_mode == IRPLIB_FILTER_BORDER_EXTRAPOL_OUT) {
01200             /* Extrapolate last and first computed medians of row */
01201             out[1]  = out[2];
01202             out[Nx] = out[Nx-1];
01203 
01204             if (i0 == in) {
01205                 /* Finished first row of medians */
01206                 (void)memcpy(out-(Nx-1), out+1, Nx*sizeof(*out));
01207             }
01208         }
01209     }
01210 
01211     switch (border_mode) {
01212         case IRPLIB_FILTER_BORDER_FILTER: {
01213             IN_TYPE buf6[6];
01214             unsigned j = 0;
01215 
01216             buf6[0] = i0[0];
01217             buf6[1] = i0[1];
01218             buf6[2] = i1[0];
01219             buf6[3] = i1[1];
01220             out[j++] = get_kth(buf6, 4, m-1);
01221             m = 5-m;
01222 
01223             for (; j < Nx-1; j++) {
01224                 memcpy(buf6,   i0+j-1, 3*sizeof(*buf6));
01225                 memcpy(buf6+3, i1+j-1, 3*sizeof(*buf6));
01226                 out[j] = get_kth(buf6, 6, m);
01227                 m = 5-m;
01228             }
01229             buf6[0] = i0[Nx-2];
01230             buf6[1] = i0[Nx-1];
01231             buf6[2] = i1[Nx-2];
01232             buf6[3] = i1[Nx-1];
01233             out[j] = get_kth(buf6, 4, m-1);
01234 
01235             break;
01236         }
01237         default:
01238         case IRPLIB_FILTER_BORDER_CROP:
01239             break;
01240         case IRPLIB_FILTER_BORDER_COPY: {
01241             /* From the 2nd last row copy last pixel */
01242             /* From the last row copy all pixels */
01243             if (sizeof(IN_TYPE) == sizeof(OUT_TYPE)) {
01244                 (void)memcpy(out, istop-(Nx+1), (Nx+1)*sizeof(*out));
01245             } else {
01246                 unsigned i;
01247                 istop -= (Nx+1);
01248                 for (i=0; i < Nx+1; i++) out[i] = (OUT_TYPE)istop[i];
01249             }
01250             break;
01251         }
01252         case IRPLIB_FILTER_BORDER_EXTRAPOL_OUT: {
01253             /* Extrapolate last row of computed medians */
01254 
01255             (void)memcpy(out+1, out-(Nx-1), Nx*sizeof(*out));
01256             break;
01257         }
01258     }
01259 
01260     return;
01261 }
01262 
01275 static void
01276 fill_row(IN_TYPE *in, unsigned nx, unsigned y,
01277          unsigned xmin, unsigned xmax,
01278          IN_TYPE val1, IN_TYPE val2) 
01279 {
01280     unsigned x = xmin;
01281     
01282     while (x+1 < xmax) {
01283         in[(x++) + y*nx] = val1;
01284         in[(x++) + y*nx] = val2;
01285     }
01286     if (x < xmax) {
01287         in[(x++) + y*nx] = val1;
01288     }
01289 
01290     return;
01291 }
01292 
01304 static void
01305 fill_chess(IN_TYPE *in_larger, const IN_TYPE *in,
01306            unsigned Nx_larger, unsigned Ny_larger, 
01307            unsigned Nx, unsigned Ny, 
01308            unsigned rx, unsigned ry)
01309 {
01310     unsigned y;
01311     
01312     /* Fill border with IN_MAX if x+y is even and IN_MIN if x+y is odd. 
01313        Loop over images in cache-friendly order.  */
01314     for (y = 0; y < ry; y++) {
01315 
01316         fill_row(in_larger, Nx_larger, y,
01317                  0, Nx_larger,
01318                  (y & 1) == 0 ? IN_MAX : IN_MIN,
01319                  (y & 1) == 0 ? IN_MIN : IN_MAX);
01320     }
01321 
01322     for (y = ry; y < ry+Ny; y++) {
01323 
01324         fill_row(in_larger, Nx_larger, y,
01325                  0, rx,
01326                  (y & 1) == 0 ? IN_MAX : IN_MIN,
01327                  (y & 1) == 0 ? IN_MIN : IN_MAX);
01328             
01329         (void)memcpy(in_larger + rx + y*Nx_larger,
01330                      in + (y-ry)*Nx, Nx*sizeof(*in));
01331             
01332         fill_row(in_larger, Nx_larger, y,
01333                  rx + Nx, Nx_larger,
01334                  ((y + rx + Nx) & 1) == 0 ? IN_MAX : IN_MIN,
01335                  ((y + rx + Nx) & 1) == 0 ? IN_MIN : IN_MAX);
01336     }
01337 
01338     for (y = ry + Ny; y < Ny_larger; y++) {
01339 
01340         fill_row(in_larger, Nx_larger, y,
01341                  0, Nx_larger,
01342                  (y & 1) == 0 ? IN_MAX : IN_MIN,
01343                  (y & 1) == 0 ? IN_MIN : IN_MAX);
01344     }
01345 }
01346 
01439 static void
01440 filter_median(const IN_TYPE *in, OUT_TYPE *out,
01441               unsigned Nx, unsigned Ny,
01442               unsigned rx, unsigned ry,
01443               unsigned border_mode)
01444 {
01445     unsigned out_Nx;
01446     dheap *dh;
01447     unsigned y;
01448     unsigned x;
01449     int dx;
01450 
01451     assure( 2*rx + 1 <= Nx );
01452     assure( 2*ry + 1 <= Ny );
01453 
01454     border_mode &= IRPLIB_FILTER_BORDER_MODE;
01455 
01456     if (rx == 1 && ry == 1) {
01457         filter_median_1(in, out, Nx, Ny, border_mode);
01458         return;
01459     }
01460 
01461     if (rx == 0 && ry == 0) {
01462         /* The double heap structure must have non-empty heaps,
01463            so handle this (trivial) case explicitly.
01464            
01465            Works for all border modes (because there are no borders!).
01466         */
01467         unsigned i;
01468         for (i = 0; i < Nx * Ny; i++) out[i] = (OUT_TYPE)in[i];
01469         return;
01470     }
01471 
01472     assure( border_mode == IRPLIB_FILTER_BORDER_NOP ||
01473             border_mode == IRPLIB_FILTER_BORDER_CROP ||
01474             border_mode == IRPLIB_FILTER_BORDER_COPY ||
01475             border_mode == IRPLIB_FILTER_BORDER_EXTRAPOL_OUT ||
01476             border_mode == IRPLIB_FILTER_BORDER_FILTER );
01477 
01478     if (border_mode == IRPLIB_FILTER_BORDER_FILTER) {
01479         /* Add a border of (width, height) = (rx, ry) to the input image.
01480            Fill with +-infinity in a chessboard pattern.
01481 
01482            Then filter this larger image using IRPLIB_FILTER_BORDER_CROP,
01483            and the result will come out right (because the added border region
01484            always contains the same number of +infinity and -infinity).
01485         */
01486         unsigned Nx_larger = Nx + 2*rx;
01487         unsigned Ny_larger = Ny + 2*ry;
01488 
01489         IN_TYPE *in_larger = malloc(Nx_larger*Ny_larger*sizeof(*in_larger));
01490         assure( in_larger != NULL );
01491 
01492         fill_chess(in_larger, in, Nx_larger, Ny_larger, Nx, Ny, rx, ry);
01493         
01494         filter_median(in_larger, out,
01495                       Nx_larger, Ny_larger,
01496                       rx, ry,
01497                       IRPLIB_FILTER_BORDER_CROP);
01498 
01499         free(in_larger);
01500         return;
01501     }
01502 
01503     assert( border_mode == IRPLIB_FILTER_BORDER_NOP ||
01504             border_mode == IRPLIB_FILTER_BORDER_CROP ||
01505             border_mode == IRPLIB_FILTER_BORDER_EXTRAPOL_OUT ||
01506             border_mode == IRPLIB_FILTER_BORDER_COPY);
01507 
01508     if (border_mode == IRPLIB_FILTER_BORDER_COPY) {
01509         
01510         if (sizeof(IN_TYPE) == sizeof(OUT_TYPE)) {
01511             (void)memcpy(out, in, (ry*Nx+rx)*sizeof(*out));
01512         } else {
01513             unsigned i;
01514             for (i = 0; i < ry*Nx+rx; i++) out[i] = (OUT_TYPE)in[i];
01515         }
01516     }
01517     
01518     if (border_mode == IRPLIB_FILTER_BORDER_CROP) {
01519         /* Subtract a bit from the out pointer, so that
01520            indexing out[x + y*out_Nx] works */
01521         out_Nx = Nx - 2*rx;
01522         out += -rx -ry*out_Nx;
01523     }
01524     else out_Nx = Nx;
01525 
01526     dh = dheap_new(in, Nx, Ny, rx, ry);
01527     
01528     /* Loop over image like this
01529        -->->->-
01530        -<-<-<--
01531        -->->->-  
01532        etc.
01533        so that the current window always moves by 1 pixel per iteration
01534     */
01535     for (y = 0 + dh->ry, dx = 1;
01536          y < dh->Ny - dh->ry; 
01537          y++, dx = -dx) {
01538 
01539         unsigned xfirst = (dx ==  1) ? dh->rx : dh->Nx-1 - dh->rx;
01540         unsigned xlast  = (dx == -1) ? dh->rx : dh->Nx-1 - dh->rx;
01541 
01542         if (border_mode == IRPLIB_FILTER_BORDER_COPY && dx == -1 &&
01543             y != dh->Ny - dh->ry - 1) {
01544 
01545                  OUT_TYPE *o = out + xfirst + 1 + y*dh->Nx;
01546             const IN_TYPE *i = in  + xfirst + 1 + y*dh->Nx;
01547 
01548             if (sizeof(IN_TYPE) == sizeof(OUT_TYPE)) {
01549                 (void)memcpy(o, i, 2*rx*sizeof(*o));
01550             } else {
01551                 unsigned j;
01552                 for (j = 0; j < 2*rx; j++) o[j] = (OUT_TYPE)i[j];
01553             }
01554         }
01555 
01556         for (x = xfirst; x != xlast + dx; x += dx) {
01557 
01558             if (y == dh->ry) {
01559                 SCA_init(dh, x + dx * dh->rx);
01560             }
01561             else {
01562                 /* Most frequent case. Update just 1 pixel in 1 sorted column array */
01563                 SCA_replace(dh, x + dx * dh->rx,
01564                             y - dh->ry - 1,
01565                             y + dh->ry);
01566             }
01567             
01568             if (x == xfirst) {
01569                 
01570                 if (y != dh->ry) {
01571                     /* Window just moved to next row,
01572                        update pixels at window bottom row */
01573                     unsigned i;
01574                     for (i  = x - dx*dh->rx; 
01575                          i != x + dx*dh->rx; i += dx) {
01576                         SCA_replace(dh, i,
01577                                     y - dh->ry - 1,
01578                                     y + dh->ry);
01579                         
01580                         dheap_replace(dh->image,
01581                                       dh->B,
01582                                       dh->heaps,
01583                                       dh->m,
01584                                       dh->m+1,
01585                                       i + ((y + dh->ry    ) * dh->Nx),
01586                                       i + ((y - dh->ry - 1) * dh->Nx));
01587                     }
01588                     
01589                     dheap_replace(dh->image,
01590                                   dh->B,
01591                                   dh->heaps,
01592                                   dh->m,
01593                                   dh->m+1,
01594                                   i + ((y + dh->ry    ) * dh->Nx),
01595                                   i + ((y - dh->ry - 1) * dh->Nx));
01596                 }
01597             }
01598             else {
01599                 /* Most frequent case. Replace pixels in xold column
01600                    with pixels from xnew column */
01601                 unsigned xnew = x + dx * dh->rx;
01602                 unsigned xold = x - dx * (dh->rx+1);
01603                 unsigned *SCAxnew = dh->SCA + dh->Ry*xnew;
01604                 unsigned *SCAxold = dh->SCA + dh->Ry*xold;
01605                 
01606                 const IN_TYPE *image = dh->image;
01607                 unsigned *B = dh->B;
01608                 unsigned *heaps = dh->heaps;
01609                 unsigned m = dh->m;
01610                 unsigned m1 = m+1;
01611                 unsigned anew, aold;
01612                 unsigned i;
01613                 
01614                 /* Loop through old/new sorted column arrays and replace
01615                    old pixels by new pixels of the same rank (i.e. position in
01616                    sorted column array). 
01617                    
01618                    This heuristic increases the chance that the new pixel is close
01619                    to its final position when inserted into the double heap structure.
01620                 */
01621                 for (i = 0; i < dh->Ry; i++) {
01622                     anew = SCAxnew[i];
01623                     aold = SCAxold[i];
01624                     
01625                     dheap_replace(image, B, heaps, m, m1, 
01626                                   anew, aold);
01627                 }
01628             }
01629 
01630             out[x + y*out_Nx] = (OUT_TYPE)dheap_median(dh);
01631 
01632         } /* for x */
01633         
01634         if (border_mode == IRPLIB_FILTER_BORDER_COPY && dx == 1 &&
01635             y != dh->Ny - dh->ry - 1) {
01636 
01637                  OUT_TYPE *o = out + xlast + 1 + y*dh->Nx;
01638             const IN_TYPE *i = in  + xlast + 1 + y*dh->Nx;
01639 
01640             if (sizeof(IN_TYPE) == sizeof(OUT_TYPE)) {
01641                 (void)memcpy(o, i, 2*rx*sizeof(*o));
01642             } else {
01643                 unsigned j;
01644                 for (j = 0; j < 2*rx; j++) o[j] = (OUT_TYPE)i[j];
01645             }
01646         }
01647         else if (border_mode == IRPLIB_FILTER_BORDER_EXTRAPOL_OUT) {
01648         
01649             /* Fill right border */
01650             OUT_TYPE *o = out + (y+1)*dh->Nx - dh->rx - 1;
01651             unsigned i;
01652             int j;
01653 
01654             for (i = 0; i < dh->rx; i++) {
01655                 o[1+i] = o[0];
01656             }
01657             
01658             /* Fill left border */
01659             o = out + y*dh->Nx;
01660             for (j = dh->rx-1; j >= 0; j--) {
01661                 o[j] = o[dh->rx];
01662             }
01663 
01664             if (y == dh->ry) {
01665                 for (i = 0; i < dh->ry; i++)
01666                     (void)memcpy(out + i*Nx, out + dh->ry*Nx, Nx*sizeof(*out));
01667             }
01668         }
01669 
01670     } /* for y */
01671 
01672     if (border_mode == IRPLIB_FILTER_BORDER_COPY) {
01673         out += (Ny-ry)*dh->Nx - rx;
01674         in  += (Ny-ry)*dh->Nx - rx;
01675         if (sizeof(IN_TYPE) == sizeof(OUT_TYPE)) {
01676             (void)memcpy(out, in, (ry*Nx + rx)*sizeof(*out));
01677         } else {
01678             unsigned i;
01679             for (i = 0; i < ry*Nx; i++) out[i] = (OUT_TYPE)in[i];
01680         }
01681     }
01682     else if (border_mode == IRPLIB_FILTER_BORDER_EXTRAPOL_OUT) {
01683         unsigned i;
01684         for (i = 0; i < dh->ry; i++) {
01685             (void)memcpy(out + (Ny-dh->ry+i)*Nx, 
01686                          out + (Ny-dh->ry-1)*Nx, Nx*sizeof(*out));
01687         }
01688     }
01689 
01690     dheap_delete(dh);
01691     return;
01692 }
01693 

Generated on Mon Apr 21 10:56:53 2008 for UVES Pipeline Reference Manual by  doxygen 1.5.1