Median/Bilateral Filter

#define H 0
#define S 1
#define V 2

std::vector<Vec3b> sortByChannel(std::vector<Vec3b> kernel, int size, int channel) {
    int minIndex;
    for (int i = 0; i < size - 1; i++) {
        minIndex = i;
        for (int j = i + 1; j < size; j++) {
            if (kernel[j][channel] < kernel[minIndex][channel])
                minIndex = j;
        }
        //Swap
        Vec3b aux = kernel[i];
        kernel[i] = kernel[minIndex];
        kernel[minIndex] = aux;
    }
    return kernel;
}

std::vector<Vec3b> sortByChannelAndChannelEqualSV(std::vector<Vec3b> kernel, int size, int eqCh, int eqCh2, int channel) {
    int minIndex = UINT_MAX;
    for (int i = 0; i < size - 1; i++) {
        minIndex = i;
        for (int j = i + 1; j < size; j++) {
            if (kernel[j][eqCh] == kernel[i][eqCh] && kernel[j][eqCh2] == kernel[i][eqCh2]) {
                if (kernel[j][channel] < kernel[minIndex][channel])
                    minIndex = j;
            }
            else
                break;
        }
        //Swap
        Vec3b aux = kernel[i];
        kernel[i] = kernel[minIndex];
        kernel[minIndex] = aux;
    }
    return kernel;
}

std::vector<Vec3b> sortByChannelAndChannelEqualMaxToLow(std::vector<Vec3b> kernel, int size, int eqCh, int channel) {
    int maxIndex = -1;
    for (int i = 0; i < size - 1; i++) {
        maxIndex = i;
        for (int j = i + 1; j < size; j++) {
            if (kernel[j][eqCh] == kernel[i][eqCh]) {
                if (kernel[j][channel] > kernel[maxIndex][channel])
                    maxIndex = j;
            }
            else
                break;
        }
        //Swap
        Vec3b aux = kernel[i];
        kernel[i] = kernel[maxIndex];
        kernel[maxIndex] = aux;
    }
    return kernel;
}

Mat medianFilterColor(Mat src, int kernelSize, bool print = false) {
    Mat matHSV;
    cvtColor(src, matHSV, CV_BGR2HSV);
    Mat dst = matHSV.clone();
    int range = kernelSize / 2,
        size = kernelSize * kernelSize,
        height = src.rows - range,
        width = src.cols - range;
    std::vector<Vec3b> kernel;
    kernel.resize(size);
    for (int i = range; i < height; i++) {
        for (int j = range; j < width; j++) {
            int count = 0;
            for (int x = 0; x < kernelSize; x++) {
                for (int y = 0; y < kernelSize; y++) {
                    int matX = i + x - range,
                        matY = j + y - range;
                    kernel[count++] = matHSV.at<Vec3b>(matX, matY);
                }
            }
            kernel = sortByChannel(kernel, count, V);
            //kernel = sortByChannelAndChannelEqualMaxToLow(kernel, size, V, S);
            //kernel = sortByChannelAndChannelEqualSV(kernel, size, V, S, H);
            dst.at<Vec3b>(i, j) = kernel[count / 2];
        }
    }
    //
    Mat matRGB;
    cvtColor(dst, matRGB, CV_HSV2BGR);
    //
    if (print) {
        imshow("Original image", src);
        imshow("Median filter", matRGB);
        waitKey();
    }
    //
    return matRGB;
}

void testMedianFilterColor() {
    char fname[MAX_PATH];
    if (openFileDlg(fname)) {
        Mat src = imread(fname, CV_LOAD_IMAGE_COLOR);
        medianFilterColor(src, 7, true);
    }
}

//Bilaterial filter

float distanceBF(int x, int y, int i, int j) {
    return float(sqrt(pow(x - i, 2) + pow(y - j, 2)));
}

float gaussianBF(float x, double sigma) {
    return exp(-(pow(x, 2))/(2 * pow(sigma, 2))) / (2 * CV_PI * pow(sigma, 2));
}

#define BF_SIZE 5
#define BF_SIGMA_R (12.0f)
#define BF_SIGMA_S (16.0f)

Mat bilateralFilter(Mat src, int size, float sigmaR, float sigmaS, bool print = false) {
    int height = src.rows, width = src.cols;
    Mat matLAB;
    cvtColor(src, matLAB, CV_BGR2Lab);
    Mat bfImage = Mat::zeros(height, width, CV_64FC3);
    for (int i = 0; i < height; i++) {
        for (int j = 0; j < width; j++) {
            bfImage.at<Vec3d>(i, j) = matLAB.at<Vec3b>(i, j);
        }
    }
    //
    int range = size / 2;
    for (int i = range; i < height - range; i++) {
        for (int j = range; j < width - range; j++) {
            double wP[3] = { 0.0 }, sumTotal[3] = { 0.0 };
            for (int x = 0; x < range; x++) {
                for (int y = 0; y < range; y++) {
                    int matX = i + x - range,
                        matY = j + y - range;
                    double gS = gaussianBF(distanceBF(i, j, matX, matY), sigmaS);
                    for (int c = 0; c < 3; c++) {
                        float diff = matLAB.at<Vec3b>(matX, matY)[c] - matLAB.at<Vec3b>(i, j)[c];
                        double gR = gaussianBF(diff, sigmaR);
                        double w = gS * gR;
                        sumTotal[c] += matLAB.at<Vec3b>(matX, matY)[c] * w;
                        wP[c] += w;
                    }
                }
            }
            //
            for (int c = 0; c < 3; c++) {
                sumTotal[c] /= wP[c];
                bfImage.at<Vec3d>(i, j)[c] = sumTotal[c];
            }
            //
        }
    }
    //
    Mat dstLAB;
    normalize(bfImage, dstLAB, 0, 255, NORM_MINMAX, CV_8UC3);
    //Convert to BGR
    Mat dstBGR;
    cvtColor(dstLAB, dstBGR, CV_Lab2BGR);
    if (print) {
        imshow("Original image", src);
        imshow("Bilateral filter", dstBGR);
        waitKey();
    }
    return dstBGR;
}

void testBilaterFilter() {
    char fname[MAX_PATH];
    if (openFileDlg(fname)) {
        Mat src = imread(fname, CV_LOAD_IMAGE_COLOR);
        bilateralFilter(src, BF_SIZE, BF_SIGMA_R, BF_SIGMA_S, true);
    }
}

//