1-D Median Filter

medianfilter.cpp

std::vector<double> medianFilter_(const std::vector<double>& array, unsigned long window)
{
    std::vector<double> filtered;
    
    filtered.reserve(array.size());
    
    unsigned long middle = window / 2;
    
    std::vector<double>::const_iterator itr, end, first, last, firstMid, lastMid;
    
    // first is the start of the current
    // median filter window and itr is
    // and iterator to check for positioning
    first = itr = array.begin();
    
    // After this point the first iterator
    // starts incrementing
    firstMid = array.begin() + middle;
    
    // The end of the current median
    // filter window
    last = firstMid + 1;
    
    // The end of the array
    end = array.end();
    
    // After this point the last iterator
    // stops incrementing
    lastMid = end - middle;
    
    // For odd windows the last iterator
    // stops incrementing earlier, because
    // for even windows the iterator starts
    // at the right of the two middle values
    // and thus needs an extra incrementing
    if (window % 2) --lastMid;
    
    for( ; itr != end; ++itr)
    {
        // Current window
        std::vector<double> temp(first,last);
        
        std::sort(temp.begin(), temp.end());
        
        // This middle value cannot be precomputed
        // because the window size varies at the
        // begininning and end
        middle = temp.size() / 2;
        
        double median;
        
        // If the temporary array has an even size
        if (! (temp.size() % 2))
        {
            // Median for even sizes
            median = (temp[middle] + temp[middle - 1]) / 2.0;
        }
        
        else median = temp[middle]; // Median for odd numbers
        
        filtered.push_back(median);
        
        // Start incrmenting first after firstMid
        if (itr >= firstMid) ++first;
        
        // Stop incrementing last after lastMid
        if (itr < lastMid) ++last;
    }
    
    return filtered;
}