bilateral filter

Author: neka-nat

CMakeLists.txt

@@ -166,6 +166,7 @@ set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS}
     --expt-relaxed-constexpr
     --expt-extended-lambda
     --default-stream per-thread
+    --use_fast_math
     -Xcudafe "--diag_suppress=integer_sign_change"
     -Xcudafe "--diag_suppress=partial_override"
     -Xcudafe "--diag_suppress=virtual_function_decl_hidden")

src/cupoch/geometry/image.cu

@@ -33,52 +33,32 @@ std::pair<utility::device_vector<float>, utility::device_vector<float>>
 GetFilterKernel(Image::FilterType ftype) {
     switch (ftype) {
         case Image::FilterType::Gaussian3: {
-            utility::device_vector<float> g3(3);
-            g3[0] = 0.25;
-            g3[1] = 0.5;
-            g3[2] = 0.25;
+            const float k[3] = {0.25, 0.5, 0.25};
+            utility::device_vector<float> g3(k, k + 3);
             return std::make_pair(g3, g3);
         }
         case Image::FilterType::Gaussian5: {
-            utility::device_vector<float> g5(5);
-            g5[0] = 0.0625;
-            g5[1] = 0.25;
-            g5[2] = 0.375;
-            g5[3] = 0.25;
-            g5[4] = 0.0625;
+            const float k[5] = {0.0625, 0.25, 0.375, 0.25, 0.0625};
+            utility::device_vector<float> g5(k, k + 5);
             return std::make_pair(g5, g5);
         }
         case Image::FilterType::Gaussian7: {
-            utility::device_vector<float> g7(7);
-            g7[0] = 0.03125;
-            g7[1] = 0.109375;
-            g7[2] = 0.21875;
-            g7[3] = 0.28125;
-            g7[4] = 0.21875;
-            g7[5] = 0.109375;
-            g7[6] = 0.03125;
+            const float k[7] = {0.03125, 0.109375, 0.21875, 0.28125, 0.21875, 0.109375, 0.03125};
+            utility::device_vector<float> g7(k, k + 7);
             return std::make_pair(g7, g7);
         }
         case Image::FilterType::Sobel3Dx: {
-            utility::device_vector<float> s31(3);
-            utility::device_vector<float> s32(3);
-            s31[0] = -1.0;
-            s31[1] = 0.0;
-            s31[2] = 1.0;
-            s32[0] = 1.0;
-            s32[1] = 2.0;
-            s32[2] = 1.0;
+            const float k1[3] = {-1.0, 0.0, 1.0};
+            const float k2[3] = {1.0, 2.0, 1.0};
+            utility::device_vector<float> s31(k1, k1 + 3);
+            utility::device_vector<float> s32(k2, k2 + 3);
             return std::make_pair(s31, s32);
         }
         case Image::FilterType::Sobel3Dy: {
-            utility::device_vector<float> s31(3);
-            utility::device_vector<float> s32(3);
-            s31[0] = -1.0;
-            s31[1] = 0.0;
-            s31[2] = 1.0;
-            s32[0] = 1.0;
-            s32[1] = 2.0;
-            s32[2] = 1.0;
+            const float k1[3] = {-1.0, 0.0, 1.0};
+            const float k2[3] = {1.0, 2.0, 1.0};
+            utility::device_vector<float> s31(k1, k1 + 3);
+            utility::device_vector<float> s32(k2, k2 + 3);
             return std::make_pair(s32, s31);
         }
         default: {
@@ -185,9 +165,7 @@ struct filter_horizontal_functor {
         float *po = (float *)(dst_ + idx * sizeof(float));
         float temp = 0;
         for (int i = -half_kernel_size_; i <= half_kernel_size_; i++) {
-            int x_shift = x + i;
-            if (x_shift < 0) x_shift = 0;
-            if (x_shift > width_ - 1) x_shift = width_ - 1;
+            int x_shift = min(max(0, x + i), width_ - 1);
             float *pi =
                     (float *)(src_ + (y * width_ + x_shift) * sizeof(float));
             temp += (*pi * kernel_[i + half_kernel_size_]);
@@ -243,6 +221,52 @@ struct horizontal_flip_functor {
     }
 };
 
+struct bilateral_filter_functor {
+    bilateral_filter_functor(const uint8_t *src,
+                             int width,
+                             int height,
+                             int diameter,
+                             float sigma_color,
+                             const float* gaussian_const,
+                             uint8_t *dst)
+        : src_(src),
+          width_(width),
+          height_(height),
+          diameter_(diameter),
+          sigma_color_(sigma_color),
+          gaussian_const_(gaussian_const),
+          dst_(dst){};
+    const uint8_t *src_;
+    const int width_;
+    const int height_;
+    const int diameter_;
+    const float sigma_color_;
+    const float* gaussian_const_;
+    uint8_t *dst_;
+    __device__ float gaussian(float x, float sig) const {
+        return exp(-(powf(x, 2)) / (2 * powf(sig, 2)));
+    }
+    __device__ void operator() (size_t idx) {
+        const int y = idx / width_;
+        const int x = idx % width_;
+		float filtered = 0;
+        float total_w = 0;
+        float center_p = *(float *)(src_ + idx * sizeof(float));
+        for (int dy = -diameter_; dy <= diameter_; dy++) {
+            for (int dx = -diameter_; dx <= diameter_; dx++) {
+                int mdy = min(max(0, dy), height_);
+                int mdx = min(max(0, dx), width_);
+                float cur_p = *(float *)(src_ + ((y + mdy) * width_ + x + mdx) * sizeof(float));
+                float w = gaussian_const_[dy + diameter_] * gaussian_const_[dx + diameter_] * gaussian(center_p - cur_p, sigma_color_);
+                filtered += w * cur_p;
+                total_w += w; 
+            }
+        }
+        float* p = (float *)(dst_ + idx * sizeof(float));
+        *p = filtered / total_w;
+    }
+};
+
 struct depth_to_float_functor {
     depth_to_float_functor(int depth_scale, int depth_trunc)
         : depth_scale_(depth_scale),
@@ -483,6 +507,32 @@ std::shared_ptr<Image> Image::FlipHorizontal() const {
     return output;
 }
 
+std::shared_ptr<Image> Image::BilateralFilter(
+    int diameter, float sigma_color, float sigma_space) const {
+    auto output = std::make_shared<Image>();
+    if (diameter >= 64) {
+        utility::LogError("[BilateralFilter] Diameter should be less than 64.");
+        return output;
+    }
+    output->Prepare(width_, height_, num_of_channels_, bytes_per_channel_);
+    float fgaussian[64];
+    const float sigma2 = sigma_space * sigma_space;
+	for (int i = 0; i < 2 * diameter + 1; i++) {
+        const float x = i - diameter;
+        fgaussian[i] = std::exp(-(x * x) / (2 * sigma2));
+    }
+    utility::device_vector<float> gaussian_const(fgaussian, fgaussian + 64);
+    bilateral_filter_functor func(
+        thrust::raw_pointer_cast(data_.data()), width_,
+        height_, diameter, sigma_color,
+        thrust::raw_pointer_cast(gaussian_const.data()),
+        thrust::raw_pointer_cast(output->data_.data()));
+    thrust::for_each(thrust::make_counting_iterator<size_t>(0),
+                     thrust::make_counting_iterator<size_t>(width_ * height_),
+                     func);
+    return output;
+}
+
 void Image::AllocateDataBuffer() {
     data_.resize(width_ * height_ * num_of_channels_ * bytes_per_channel_);
 }

src/cupoch/geometry/image.h

@@ -170,6 +170,9 @@ class Image : public GeometryBase<2> {
     std::shared_ptr<Image> FilterHorizontal(
             const utility::device_vector<float> &kernel) const;
 
+    std::shared_ptr<Image> BilateralFilter(
+            int diameter, float sigma_color, float sigma_space) const;
+
     /// Function to 2x image downsample using simple 2x2 averaging.
     std::shared_ptr<Image> Downsample() const;
 

src/tests/geometry/image.cpp

@@ -228,14 +228,14 @@ void TEST_CreateFloatImage(
 TEST(Image, CreateFloatImage_1_1) {
     // reference data used to validate the creation of the float image
     uint8_t raw_ref[] = {
-            215, 214, 86,  63,  201, 200, 200, 62,  200, 199, 71,  63,  204,
-            203, 75,  63,  233, 232, 104, 63,  201, 200, 72,  62,  171, 170,
-            170, 62,  196, 195, 67,  63,  141, 140, 140, 62,  142, 141, 13,
-            63,  243, 242, 242, 62,  161, 160, 32,  63,  187, 186, 186, 62,
-            131, 130, 2,   63,  243, 242, 114, 63,  234, 233, 105, 63,  163,
+            216, 214, 86,  63,  202, 200, 200, 62,  201, 199, 71,  63,  205,
+            203, 75,  63,  234, 232, 104, 63,  202, 200, 72,  62,  171, 170,
+            170, 62,  197, 195, 67,  63,  141, 140, 140, 62,  142, 141, 13,
+            63,  244, 242, 242, 62,  161, 160, 32,  63,  187, 186, 186, 62,
+            131, 130, 2,   63,  244, 242, 114, 63,  235, 233, 105, 63,  163,
             162, 34,  63,  183, 182, 54,  63,  145, 144, 16,  62,  155, 154,
-            26,  63,  129, 128, 128, 60,  245, 244, 116, 62,  137, 136, 8,
-            62,  206, 205, 77,  63,  157, 156, 28,  62};
+            26,  63,  129, 128, 128, 60,  246, 244, 116, 62,  137, 136, 8,
+            62,  207, 205, 77,  63,  157, 156, 28,  62};
     thrust::host_vector<uint8_t> ref;
     for (int i = 0; i < 100; ++i) ref.push_back(raw_ref[i]);
 
@@ -278,13 +278,13 @@ TEST(Image, CreateFloatImage_1_4) {
 TEST(Image, CreateFloatImage_3_1_Weighted) {
     // reference data used to validate the creation of the float image
     uint8_t raw_ref[] = {
-            44,  241, 17,  63,  29,  96,  75,  63,  154, 112, 20,  63,  255,
-            240, 3,   63,  180, 56,  4,   63,  139, 60,  58,  63,  115, 8,
-            204, 62,  215, 59,  119, 62,  64,  47,  151, 62,  251, 20,  36,
-            63,  194, 101, 54,  63,  138, 51,  5,   63,  54,  35,  64,  63,
-            94,  59,  32,  63,  28,  161, 44,  63,  137, 77,  46,  63,  199,
-            12,  35,  63,  121, 21,  90,  62,  100, 168, 243, 62,  209, 97,
-            143, 62,  9,   228, 61,  63,  223, 255, 239, 62,  58,  33,  29,
+            45,  241, 17,  63,  30,  96,  75,  63,  154, 112, 20,  63,  0,
+            241, 3,   63,  180, 56,  4,   63,  139, 60,  58,  63,  116, 8,
+            204, 62,  216, 59,  119, 62,  65,  47,  151, 62,  252, 20,  36,
+            63,  194, 101, 54,  63,  138, 51,  5,   63,  55,  35,  64,  63,
+            95,  59,  32,  63,  29,  161, 44,  63,  137, 77,  46,  63,  199,
+            12,  35,  63,  122, 21,  90,  62,  101, 168, 243, 62,  209, 97,
+            143, 62,  10,   228, 61,  63,  224, 255, 239, 62,  59,  33,  29,
             63,  197, 186, 3,   63,  145, 27,  72,  63};
     thrust::host_vector<uint8_t> ref;
     for (int i = 0; i < 100; ++i) ref.push_back(raw_ref[i]);
@@ -295,13 +295,13 @@ TEST(Image, CreateFloatImage_3_1_Weighted) {
 TEST(Image, CreateFloatImage_3_1_Equal) {
     // reference data used to validate the creation of the float image
     uint8_t raw_ref[] = {
-            44,  241, 17,  63,  29,  96,  75,  63,  154, 112, 20,  63,  255,
-            240, 3,   63,  180, 56,  4,   63,  139, 60,  58,  63,  115, 8,
-            204, 62,  215, 59,  119, 62,  64,  47,  151, 62,  251, 20,  36,
-            63,  194, 101, 54,  63,  138, 51,  5,   63,  54,  35,  64,  63,
-            94,  59,  32,  63,  28,  161, 44,  63,  137, 77,  46,  63,  199,
-            12,  35,  63,  121, 21,  90,  62,  100, 168, 243, 62,  209, 97,
-            143, 62,  9,   228, 61,  63,  223, 255, 239, 62,  58,  33,  29,
+            45,  241, 17,  63,  30,  96,  75,  63,  154, 112, 20,  63,  0,
+            241, 3,   63,  180, 56,  4,   63,  139, 60,  58,  63,  116, 8,
+            204, 62,  216, 59,  119, 62,  65,  47,  151, 62,  252, 20,  36,
+            63,  194, 101, 54,  63,  138, 51,  5,   63,  55,  35,  64,  63,
+            95,  59,  32,  63,  29,  161, 44,  63,  137, 77,  46,  63,  199,
+            12,  35,  63,  122, 21,  90,  62,  101, 168, 243, 62,  209, 97,
+            143, 62,  10,   228, 61,  63,  224, 255, 239, 62,  59,  33,  29,
             63,  197, 186, 3,   63,  145, 27,  72,  63};
     thrust::host_vector<uint8_t> ref;
     for (int i = 0; i < 100; ++i) ref.push_back(raw_ref[i]);
@@ -380,14 +380,14 @@ TEST(Image, CreateFloatImage_3_4_Equal) {
 TEST(Image, ConvertDepthToFloatImage) {
     // reference data used to validate the creation of the float image
     uint8_t raw_ref[] = {
-            208, 254, 91,  58,  103, 154, 205, 57,  59,  147, 76,  58,  236,
-            175, 80,  58,  232, 127, 110, 58,  103, 154, 77,  57,  62,  195,
-            174, 57,  139, 118, 72,  58,  22,  236, 143, 57,  66,  243, 16,
-            58,  161, 199, 248, 57,  134, 123, 36,  58,  255, 53,  191, 57,
-            93,  164, 5,   58,  161, 199, 120, 58,  20,  135, 111, 58,  222,
+            210, 254, 91,  58,  105, 154, 205, 57,  61,  147, 76,  58,  237,
+            175, 80,  58,  234, 127, 110, 58,  105, 154, 77,  57,  63,  195,
+            174, 57,  141, 118, 72,  58,  22,  236, 143, 57,  66,  243, 16,
+            58,  163, 199, 248, 57,  135, 123, 36,  58,  0, 54,  191, 57,
+            94,  164, 5,   58,  163, 199, 120, 58,  22,  135, 111, 58,  223,
             137, 38,  58,  79,  25,  59,  58,  198, 8,   20,  57,  126, 80,
-            30,  58,  5,   150, 131, 55,  249, 213, 122, 57,  101, 207, 11,
-            57,  68,  190, 82,  58,  214, 94,  32,  57};
+            30,  58,  6,   150, 131, 55,  251, 213, 122, 57,  102, 207, 11,
+            57,  69,  190, 82,  58,  215, 94,  32,  57};
     thrust::host_vector<uint8_t> ref;
     for (int i = 0; i < 100; ++i) ref.push_back(raw_ref[i]);
     geometry::Image image;