Listing 1: Textbook implementation of median filtering.

void medianFilter(const Image &in, size_t subWindowSize, Image &out) {
  const int offset = subWindowSize / 2;
  const size_t center = (subWindowSize * subWindowSize) / 2;
  std::vector<unsigned char> tmp(SubWindowSize * SubWindowSize);
  for (int y = 0; y < in.height(); ++y) {
    for (int x = 0; x < in.width(); ++x) {
      for (int yp = y - offset, i = 0; yp <= y + offset; ++yp) {
        for (int xp = x - offset; xp <= x + offset; ++xp, ++i) {
          if ((xp < in.width()) && 
              (xp >= 0) && 
              (yp < in.height()) && 
              (yp >= 0)) {
            tmp[i] = in.get(xp, yp);
          } else  {
            tmp[i] = 0;
          }
        }
      }
      std::nth_element(tmp.begin(), tmp.begin() + center, tmp.end());
      out.set(x, y) = tmp[center];
    }
  }
}