From 90ecf54e847de66a883c8b9eb467609a90623a0e Mon Sep 17 00:00:00 2001 From: sfdd Date: Wed, 16 Jul 2025 00:48:19 +0300 Subject: [PATCH 1/3] Add a baseline implementation of bilateral filter for point clouds --- cpp/open3d/geometry/PointCloud.cpp | 43 ++++++++++++++++++++++++++++++ cpp/open3d/geometry/PointCloud.h | 7 ++++- 2 files changed, 49 insertions(+), 1 deletion(-) diff --git a/cpp/open3d/geometry/PointCloud.cpp b/cpp/open3d/geometry/PointCloud.cpp index b9ecab11f82..f2a655a936c 100644 --- a/cpp/open3d/geometry/PointCloud.cpp +++ b/cpp/open3d/geometry/PointCloud.cpp @@ -544,6 +544,49 @@ std::shared_ptr PointCloud::FarthestPointDownSample( return SelectByIndex(selected_indices); } +std::shared_ptr PointCloud::FilterBilateral( + double radius, + double sigma_s, + double sigma_r) const { + auto output = std::make_shared(*this); + KDTreeFlann kdtree(*this); + + const double two_sigma_spatial2 = 2.0 * sigma_s * sigma_s; + const double two_sigma_range2 = 2.0 * sigma_r * sigma_r; + + for (size_t i = 0; i < points_.size(); ++i) { + const auto& point = points_[i]; + const auto& color = colors_[i]; + + std::vector indices; + std::vector distances; + const auto nb_count = kdtree.SearchRadius(point, radius, indices, distances); + if (nb_count <= 1) { + continue; + } + + double weighted_sum {0.0}; + Eigen::Vector3d point_sum {}; + + for (const auto npoint_index : indices) { + const auto& npoint = points_[npoint_index]; + const auto& ncolor = colors_[npoint_index]; + + const auto spatial_distance = (npoint - point).squaredNorm(); + // TODO: which ColorToIntensityConversionType to use? Equal or Weighted? + const auto range_distance = (ncolor - color).squaredNorm(); + + // TODO: add a reference for this formula + const auto w = std::exp(-spatial_distance/two_sigma_spatial2 - range_distance/two_sigma_range2); + point_sum += w * npoint; + weighted_sum += w; + } + output->points_[i] = point_sum / weighted_sum; + } + + return output; +} + std::shared_ptr PointCloud::Crop(const AxisAlignedBoundingBox &bbox, bool invert) const { if (bbox.IsEmpty()) { diff --git a/cpp/open3d/geometry/PointCloud.h b/cpp/open3d/geometry/PointCloud.h index e55d89e4b88..d55bf099526 100644 --- a/cpp/open3d/geometry/PointCloud.h +++ b/cpp/open3d/geometry/PointCloud.h @@ -14,7 +14,6 @@ #include "open3d/geometry/Geometry3D.h" #include "open3d/geometry/KDTreeSearchParam.h" -#include "open3d/utility/Optional.h" namespace open3d { @@ -180,6 +179,12 @@ class PointCloud : public Geometry3D { std::shared_ptr FarthestPointDownSample( const size_t num_samples, const size_t start_index = 0) const; + /// \brief Bilateral filter for point cloud. + /// TODO: write a proper description. + std::shared_ptr FilterBilateral(double radius, + double sigma_s, + double sigma_r) const; + /// \brief Function to crop pointcloud into output pointcloud /// /// All points with coordinates outside the bounding box \p bbox are From 4748f3a68aa5982765a976ead6851e8dd982acb7 Mon Sep 17 00:00:00 2001 From: sfdd Date: Tue, 29 Jul 2025 00:39:25 +0300 Subject: [PATCH 2/3] Change a baseline implementation to use a regression plane estimation --- cpp/open3d/geometry/PointCloud.cpp | 52 ++++++++++++++++++++---------- 1 file changed, 35 insertions(+), 17 deletions(-) diff --git a/cpp/open3d/geometry/PointCloud.cpp b/cpp/open3d/geometry/PointCloud.cpp index f2a655a936c..90b3bdcef00 100644 --- a/cpp/open3d/geometry/PointCloud.cpp +++ b/cpp/open3d/geometry/PointCloud.cpp @@ -548,40 +548,58 @@ std::shared_ptr PointCloud::FilterBilateral( double radius, double sigma_s, double sigma_r) const { - auto output = std::make_shared(*this); + auto output = std::make_shared(); KDTreeFlann kdtree(*this); - const double two_sigma_spatial2 = 2.0 * sigma_s * sigma_s; - const double two_sigma_range2 = 2.0 * sigma_r * sigma_r; - + output->points_.reserve(points_.size()); + + const auto two_sigma_spatial2 = 2.0 * sigma_s * sigma_s; + const auto two_sigma_range2 = 2.0 * sigma_r * sigma_r; + + Eigen::Vector3d mean; + Eigen::Matrix3d cov; + Eigen::SelfAdjointEigenSolver solver; + for (size_t i = 0; i < points_.size(); ++i) { const auto& point = points_[i]; - const auto& color = colors_[i]; std::vector indices; std::vector distances; const auto nb_count = kdtree.SearchRadius(point, radius, indices, distances); if (nb_count <= 1) { + output->points_.push_back(point); continue; } - double weighted_sum {0.0}; - Eigen::Vector3d point_sum {}; + std::tie(mean, cov) = utility::ComputeMeanAndCovariance(points_, indices); + + solver.computeDirect(cov, Eigen::ComputeEigenvectors); + Eigen::Matrix normal_vector = solver.eigenvectors().col(0).cast().normalized(); + + double sum {0.0}; + double normalizing_factor {0.0}; for (const auto npoint_index : indices) { - const auto& npoint = points_[npoint_index]; - const auto& ncolor = colors_[npoint_index]; + const auto& npoint = points_.at(npoint_index); + + const auto distance = (npoint - point).squaredNorm(); + const auto normal_distance = (npoint - mean).dot(normal_vector); + + const auto distance_weight = std::exp(-distance * distance / two_sigma_spatial2); + const auto normal_distance_weight = std::exp(-normal_distance * normal_distance / two_sigma_range2); - const auto spatial_distance = (npoint - point).squaredNorm(); - // TODO: which ColorToIntensityConversionType to use? Equal or Weighted? - const auto range_distance = (ncolor - color).squaredNorm(); + const auto weight = distance_weight * normal_distance_weight; + sum += weight * normal_distance; + normalizing_factor += weight; + } - // TODO: add a reference for this formula - const auto w = std::exp(-spatial_distance/two_sigma_spatial2 - range_distance/two_sigma_range2); - point_sum += w * npoint; - weighted_sum += w; + if (normalizing_factor < std::numeric_limits::epsilon()) { + sum = 0.0; + } else { + sum /= normalizing_factor; } - output->points_[i] = point_sum / weighted_sum; + + output->points_.push_back(point + sum * normal_vector); } return output; From cbdc0ba62eef6db35a47ff67ebf9cf254a9a1799 Mon Sep 17 00:00:00 2001 From: sfdd Date: Mon, 4 Aug 2025 21:02:27 +0300 Subject: [PATCH 3/3] Add C++ tests and python bindings. Handle edge cases and improve the doc --- CHANGELOG.md | 1 + cpp/open3d/geometry/PointCloud.cpp | 56 ++++++++++++++++++---------- cpp/open3d/geometry/PointCloud.h | 22 ++++++++--- cpp/pybind/geometry/pointcloud.cpp | 12 ++++++ cpp/tests/geometry/PointCloud.cpp | 59 ++++++++++++++++++++++++++++++ 5 files changed, 126 insertions(+), 24 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 76aee620117..b72e7191c1e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,4 +1,5 @@ ## Main +- Add BilateralFilter for PointCloud - Corrected documentation for Link Open3D in C++ projects (broken links). - Fix DLLs not being found in Python-package. Also prevent PATH from being searched for DLLs, except CUDA (PR #7108) - Fix MSAA sample count not being copied when FilamentView is copied diff --git a/cpp/open3d/geometry/PointCloud.cpp b/cpp/open3d/geometry/PointCloud.cpp index 90b3bdcef00..2749fae9dd2 100644 --- a/cpp/open3d/geometry/PointCloud.cpp +++ b/cpp/open3d/geometry/PointCloud.cpp @@ -544,14 +544,21 @@ std::shared_ptr PointCloud::FarthestPointDownSample( return SelectByIndex(selected_indices); } -std::shared_ptr PointCloud::FilterBilateral( - double radius, - double sigma_s, - double sigma_r) const { - auto output = std::make_shared(); +std::shared_ptr PointCloud::FilterBilateral(double radius, + double sigma_s, + double sigma_r) const { + constexpr auto eps = std::numeric_limits::epsilon(); + + auto output = std::make_shared(*this); KDTreeFlann kdtree(*this); - output->points_.reserve(points_.size()); + if (std::abs(sigma_s + std::numeric_limits::max()) < eps) { + sigma_s = radius / 3.0; + } + + if (std::abs(sigma_r + std::numeric_limits::max()) < eps) { + sigma_r = radius / 3.0; + } const auto two_sigma_spatial2 = 2.0 * sigma_s * sigma_s; const auto two_sigma_range2 = 2.0 * sigma_r * sigma_r; @@ -561,45 +568,56 @@ std::shared_ptr PointCloud::FilterBilateral( Eigen::SelfAdjointEigenSolver solver; for (size_t i = 0; i < points_.size(); ++i) { - const auto& point = points_[i]; + const auto &point = points_[i]; std::vector indices; std::vector distances; - const auto nb_count = kdtree.SearchRadius(point, radius, indices, distances); + const auto nb_count = + kdtree.SearchRadius(point, radius, indices, distances); if (nb_count <= 1) { - output->points_.push_back(point); continue; } - std::tie(mean, cov) = utility::ComputeMeanAndCovariance(points_, indices); + std::tie(mean, cov) = + utility::ComputeMeanAndCovariance(points_, indices); solver.computeDirect(cov, Eigen::ComputeEigenvectors); - Eigen::Matrix normal_vector = solver.eigenvectors().col(0).cast().normalized(); + Eigen::Matrix normal_vector = + solver.eigenvectors().col(0).cast().normalized(); - double sum {0.0}; - double normalizing_factor {0.0}; + double sum{0.0}; + double normalizing_factor{0.0}; for (const auto npoint_index : indices) { - const auto& npoint = points_.at(npoint_index); + const auto &npoint = points_.at(npoint_index); const auto distance = (npoint - point).squaredNorm(); const auto normal_distance = (npoint - mean).dot(normal_vector); - const auto distance_weight = std::exp(-distance * distance / two_sigma_spatial2); - const auto normal_distance_weight = std::exp(-normal_distance * normal_distance / two_sigma_range2); + double weight{1.0}; + if (std::abs(two_sigma_spatial2) > eps) { + const auto distance_weight = + std::exp(-distance * distance / two_sigma_spatial2); + weight *= distance_weight; + } + + if (std::abs(two_sigma_range2) > eps) { + const auto normal_distance_weight = std::exp( + -normal_distance * normal_distance / two_sigma_range2); + weight *= normal_distance_weight; + } - const auto weight = distance_weight * normal_distance_weight; sum += weight * normal_distance; normalizing_factor += weight; } - if (normalizing_factor < std::numeric_limits::epsilon()) { + if (normalizing_factor < eps) { sum = 0.0; } else { sum /= normalizing_factor; } - output->points_.push_back(point + sum * normal_vector); + output->points_[i] = point + sum * normal_vector; } return output; diff --git a/cpp/open3d/geometry/PointCloud.h b/cpp/open3d/geometry/PointCloud.h index d55bf099526..19d296c7cc7 100644 --- a/cpp/open3d/geometry/PointCloud.h +++ b/cpp/open3d/geometry/PointCloud.h @@ -8,6 +8,7 @@ #pragma once #include +#include #include #include #include @@ -179,11 +180,22 @@ class PointCloud : public Geometry3D { std::shared_ptr FarthestPointDownSample( const size_t num_samples, const size_t start_index = 0) const; - /// \brief Bilateral filter for point cloud. - /// TODO: write a proper description. - std::shared_ptr FilterBilateral(double radius, - double sigma_s, - double sigma_r) const; + /// \brief Function to apply a bilateral filter for point cloud as described + /// in Delon, Julie, Agnès Desolneux, Jean-Luc Lisani, and Luis Baumela, "A + /// Non-Local Algorithm for Image Denoising.", 2017. + /// + /// Read the paper for more details on the choice of the parameters. + /// If either of the parameters \p sigma_s or \p sigma_r is not provided, + /// the function will use a stable heuristic by setting both of the + /// variables to the third of the specified radius. + /// + /// \param radius Radius of the neighborhood to consider. + /// \param sigma_s Gaussian weight for the euclidean distance. + /// \param sigma_r Gaussian weight for the distance to the tangent plane. + std::shared_ptr FilterBilateral( + double radius, + double sigma_s = -std::numeric_limits::max(), + double sigma_r = -std::numeric_limits::max()) const; /// \brief Function to crop pointcloud into output pointcloud /// diff --git a/cpp/pybind/geometry/pointcloud.cpp b/cpp/pybind/geometry/pointcloud.cpp index 566cdd6b50d..bc5837a14f0 100644 --- a/cpp/pybind/geometry/pointcloud.cpp +++ b/cpp/pybind/geometry/pointcloud.cpp @@ -7,6 +7,7 @@ #include "open3d/geometry/PointCloud.h" +#include #include #include "open3d/camera/PinholeCameraIntrinsic.h" @@ -95,6 +96,17 @@ void pybind_pointcloud_definitions(py::module &m) { "non-negative number less than number of points in the " "input pointcloud.", "start_index"_a = 0) + .def("filter_bilateral", &PointCloud::FilterBilateral, + "Function to filter input pointcloud into output pointcloud " + "using bilateral filter. The filter is applied to each point " + "independently, and the result is a new point cloud." + "Based on Julie Digne1, Carlo de Franchis " + "'The Bilateral Filter for Point Clouds', 2017. " + "The choice of the parameters can be found in the " + "aforementioned paper." + "radius"_a, + "sigma_s"_a = -std::numeric_limits::max(), + "sigma_r"_a = -std::numeric_limits::max()) .def("crop", (std::shared_ptr(PointCloud::*)( const AxisAlignedBoundingBox &, bool) const) & diff --git a/cpp/tests/geometry/PointCloud.cpp b/cpp/tests/geometry/PointCloud.cpp index 64e1bee0248..6b63c4b7aae 100644 --- a/cpp/tests/geometry/PointCloud.cpp +++ b/cpp/tests/geometry/PointCloud.cpp @@ -890,6 +890,65 @@ TEST(PointCloud, FarthestPointDownSample) { ExpectEQ(pcd_down_2->points_, expected_2); } +TEST(PointCloud, FilterBilateral) { + // If every point lies on a perfect plane, bilateral filtering should leave + // it unchanged. + { + auto cloud = std::make_shared(); + constexpr auto side{10}; + constexpr auto z{0.0}; + for (int i = 0; i < side; ++i) { + for (int j = 0; j < side; ++j) { + cloud->points_.push_back(Eigen::Vector3d( + double(i) / (side - 1), double(j) / (side - 1), z)); + } + } + const auto result = cloud->FilterBilateral(2.0, 2 / 3.0, 0.0); + ASSERT_EQ(result->points_.size(), cloud->points_.size()); + for (size_t i = 0; i < cloud->points_.size(); ++i) { + EXPECT_NEAR(result->points_[i].x(), cloud->points_[i].x(), 1e-8); + EXPECT_NEAR(result->points_[i].y(), cloud->points_[i].y(), 1e-8); + EXPECT_NEAR(result->points_[i].z(), cloud->points_[i].z(), 1e-8); + } + } + + // Edge-preservation test. + { + constexpr auto n{50}; + // Empirical margin indicating that the original step (1.0) hasn't been + // smoothed out and still differs by the set margin + constexpr auto edge_diff{0.3}; + std::mt19937 gen(42); + std::normal_distribution noise(0.0, 0.5); + auto cloud = std::make_shared(); + for (size_t i = 0; i < n; ++i) { + double x = static_cast(i) / (n - 1); + double z = (i < n / 2 ? 0.0 : 1.0) + noise(gen); + cloud->points_.push_back({x, 0.0, z}); + } + const auto result = cloud->FilterBilateral(2); + // Check difference at boundary + double zL = result->points_[n / 2 - 1].z(); + double zR = result->points_[n / 2].z(); + EXPECT_GT(std::abs(zR - zL), edge_diff); + } + + // Edge cases. + { + auto empty = std::make_shared(); + auto out_empty = empty->FilterBilateral(2.0); + EXPECT_TRUE(out_empty->points_.empty()); + + auto single = std::make_shared(); + single->points_.push_back({0, 0, 0}); + auto out_single = single->FilterBilateral(2.0); + ASSERT_EQ(out_single->points_.size(), 1u); + EXPECT_NEAR(out_single->points_[0].x(), 0.0, 1e-8); + EXPECT_NEAR(out_single->points_[0].y(), 0.0, 1e-8); + EXPECT_NEAR(out_single->points_[0].z(), 0.0, 1e-8); + } +} + TEST(PointCloud, Crop_AxisAlignedBoundingBox) { geometry::AxisAlignedBoundingBox aabb({0, 0, 0}, {2, 2, 2}); geometry::PointCloud pcd({{0, 0, 0},