From 9335b9d816873cc5464b9af36612982a40235aa7 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sat, 21 Jun 2025 13:55:14 -0700
Subject: [PATCH] added test for util2d::depthBleedingFiltering

---
 corelib/include/rtabmap/core/util2d.h | 15 +++++++
 corelib/test/test_util2d.cpp          | 57 +++++++++++++++++++++++++++
 2 files changed, 72 insertions(+)

diff --git a/corelib/include/rtabmap/core/util2d.h b/corelib/include/rtabmap/core/util2d.h
index 18dcefc5..326071c7 100644
--- a/corelib/include/rtabmap/core/util2d.h
+++ b/corelib/include/rtabmap/core/util2d.h
@@ -606,6 +606,21 @@ cv::Mat RTABMAP_CORE_EXPORT fastBilateralFiltering(
 		float sigmaR = 0.05f,
 		bool earlyDivision = false);
 
+/**
+ * @brief Filters out depth bleeding artifacts in a depth image.
+ *
+ * This function sets depth values to zero (invalid) if their value significantly differs
+ * from both neighboring pixels in either horizontal or vertical direction. It works on
+ * depth images of type `CV_32FC1` (32-bit float, in meters) or `CV_16UC1` (16-bit unsigned int, in millimeters).
+ * 
+ * The function also ignores the image border by setting the first and last rows and columns to zero.
+ *
+ * @param depth Input/output depth image. Must be of type `CV_32FC1` or `CV_16UC1`.
+ *              The filtering is done in-place.
+ * @param maxDepthError Maximum allowed depth difference between a pixel and its neighbors
+ *                      before it is considered invalid and filtered out. For `CV_32FC1`,
+ *                      this value is in meters; for `CV_16UC1`, it's converted to millimeters.
+ */
 void RTABMAP_CORE_EXPORT depthBleedingFiltering(
 	cv::Mat & depth,
 	float maxDepthError);
diff --git a/corelib/test/test_util2d.cpp b/corelib/test/test_util2d.cpp
index 62ebe6fb..9bf48372 100644
--- a/corelib/test/test_util2d.cpp
+++ b/corelib/test/test_util2d.cpp
@@ -994,6 +994,63 @@ TEST(Util2dTest, fastBilateralFilteringEarlyDivisionOptionConsistency) {
     }
 }
 
+// Test for empty input
+TEST(Util2dTest, depthBleedingFilteringHandlesEmptyInput)
+{
+    cv::Mat empty;
+    EXPECT_NO_THROW(util2d::depthBleedingFiltering(empty, 0.1f));
+}
+
+// Test that borders are zeroed out
+TEST(Util2dTest, depthBleedingFilteringBordersAreZeroed)
+{
+    cv::Mat depth = cv::Mat::ones(5, 5, CV_32FC1);
+    util2d::depthBleedingFiltering(depth, 0.1f);
+
+    for(int i = 0; i < 5; ++i)
+    {
+        EXPECT_EQ(depth.at<float>(0, i), 0.0f);
+        EXPECT_EQ(depth.at<float>(4, i), 0.0f);
+        EXPECT_EQ(depth.at<float>(i, 0), 0.0f);
+        EXPECT_EQ(depth.at<float>(i, 4), 0.0f);
+    }
+}
+
+// Test that valid depths are not removed
+TEST(Util2dTest, depthBleedingFilteringKeepsValidDepths)
+{
+    cv::Mat depth = cv::Mat::ones(5, 5, CV_32FC1);
+    depth.at<float>(2,2) = 1.01f; // Within threshold of 0.1
+    util2d::depthBleedingFiltering(depth, 0.1f);
+    EXPECT_GT(depth.at<float>(2,2), 0.0f);
+}
+
+// Test that invalid depth is removed
+TEST(Util2dTest, depthBleedingFilteringFiltersInvalidDepths)
+{
+    cv::Mat depth = cv::Mat::ones(5, 5, CV_32FC1);
+    depth.at<float>(2,2) = 5.0f; // Large depth jump
+    util2d::depthBleedingFiltering(depth, 0.1f);
+    EXPECT_EQ(depth.at<float>(2,2), 0.0f);
+}
+
+// Repeat the above for CV_16UC1
+TEST(Util2dTest, depthBleedingFilteringFiltersInvalidDepths16U)
+{
+    cv::Mat depth = cv::Mat::ones(5, 5, CV_16UC1) * 1000; // 1.0m in mm
+    depth.at<uint16_t>(2,2) = 5000; // 5.0m
+    util2d::depthBleedingFiltering(depth, 0.1f);
+    EXPECT_EQ(depth.at<uint16_t>(2,2), 0);
+}
+
+TEST(Util2dTest, depthBleedingFilteringKeepsValidDepths16U)
+{
+    cv::Mat depth = cv::Mat::ones(5, 5, CV_16UC1) * 1000;
+    depth.at<uint16_t>(2,2) = 1090; // 0.09m difference, within 0.1m
+    util2d::depthBleedingFiltering(depth, 0.1f);
+    EXPECT_GT(depth.at<uint16_t>(2,2), 0);
+}
+
 TEST(Util2dTest, HSVtoRGBPureRed) {
     float r, g, b;
     util2d::HSVtoRGB(&r, &g, &b, 0.0f, 1.0f, 1.0f);