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 b9ecab11f82..2749fae9dd2 100644 --- a/cpp/open3d/geometry/PointCloud.cpp +++ b/cpp/open3d/geometry/PointCloud.cpp @@ -544,6 +544,85 @@ std::shared_ptr PointCloud::FarthestPointDownSample( return SelectByIndex(selected_indices); } +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); + + 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; + + Eigen::Vector3d mean; + Eigen::Matrix3d cov; + Eigen::SelfAdjointEigenSolver solver; + + for (size_t i = 0; i < points_.size(); ++i) { + const auto &point = points_[i]; + + std::vector indices; + std::vector distances; + const auto nb_count = + kdtree.SearchRadius(point, radius, indices, distances); + if (nb_count <= 1) { + continue; + } + + 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_.at(npoint_index); + + const auto distance = (npoint - point).squaredNorm(); + const auto normal_distance = (npoint - mean).dot(normal_vector); + + 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; + } + + sum += weight * normal_distance; + normalizing_factor += weight; + } + + if (normalizing_factor < eps) { + sum = 0.0; + } else { + sum /= normalizing_factor; + } + + output->points_[i] = point + sum * normal_vector; + } + + 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..19d296c7cc7 100644 --- a/cpp/open3d/geometry/PointCloud.h +++ b/cpp/open3d/geometry/PointCloud.h @@ -8,13 +8,13 @@ #pragma once #include +#include #include #include #include #include "open3d/geometry/Geometry3D.h" #include "open3d/geometry/KDTreeSearchParam.h" -#include "open3d/utility/Optional.h" namespace open3d { @@ -180,6 +180,23 @@ class PointCloud : public Geometry3D { std::shared_ptr FarthestPointDownSample( const size_t num_samples, const size_t start_index = 0) 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 /// /// All points with coordinates outside the bounding box \p bbox are 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},