Add FilterSOR: Statistical Outlier Rejection

mp2p_icp_filters/CMakeLists.txt

@@ -22,6 +22,7 @@ set(LIB_SRCS
   src/FilterNormalizeIntensity.cpp
   src/FilterPoleDetector.cpp
   src/FilterRemoveByVoxelOccupancy.cpp
+  src/FilterSOR.cpp
   src/FilterVoxelSlice.cpp
   src/Generator.cpp
   src/GeneratorEdgesFromCurvature.cpp
@@ -54,6 +55,7 @@ set(LIB_PUBLIC_HDRS
   include/mp2p_icp_filters/FilterNormalizeIntensity.h
   include/mp2p_icp_filters/FilterPoleDetector.h
   include/mp2p_icp_filters/FilterRemoveByVoxelOccupancy.h
+  include/mp2p_icp_filters/FilterSOR.h
   include/mp2p_icp_filters/FilterVoxelSlice.h
   include/mp2p_icp_filters/Generator.h
   include/mp2p_icp_filters/GeneratorEdgesFromCurvature.h

mp2p_icp_filters/include/mp2p_icp_filters/FilterSOR.h

@@ -0,0 +1,76 @@
+/*               _
+ _ __ ___   ___ | | __ _
+| '_ ` _ \ / _ \| |/ _` | Modular Optimization framework for
+| | | | | | (_) | | (_| | Localization and mApping (MOLA)
+|_| |_| |_|\___/|_|\__,_| https://github.com/MOLAorg/mola
+
+ A repertory of multi primitive-to-primitive (MP2P) ICP algorithms
+ and map building tools. mp2p_icp is part of MOLA.
+
+ Copyright (C) 2018-2025 Jose Luis Blanco, University of Almeria,
+                         and individual contributors.
+ SPDX-License-Identifier: BSD-3-Clause
+*/
+
+/**
+ * @file   FilterSOR.h
+ * @brief  Statistical Outlier Removal (SOR) filter.
+ * @author Jose Luis Blanco Claraco
+ * @date   Dec 28, 2025
+ */
+
+#pragma once
+
+#include <mp2p_icp/metricmap.h>
+#include <mp2p_icp_filters/FilterBase.h>
+#include <mrpt/core/pimpl.h>
+#include <mrpt/maps/CPointsMap.h>
+
+namespace mp2p_icp_filters
+{
+/**
+ * @brief Statistical Outlier Removal (SOR) filter.
+ * * For each point, it computes the average distance to its k-nearest neighbors.
+ * Points are considered outliers if their average distance is greater than
+ * (mean + std_dev_multiplier * std_dev) of the entire cloud's average distances.
+ *
+ * \ingroup mp2p_icp_filters_grp
+ */
+class FilterSOR : public mp2p_icp_filters::FilterBase
+{
+    DEFINE_MRPT_OBJECT(FilterSOR, mp2p_icp_filters)
+
+   public:
+    FilterSOR();
+
+    void filter(mp2p_icp::metric_map_t& inOut) const override;
+
+    struct Parameters
+    {
+        void load_from_yaml(const mrpt::containers::yaml& c);
+
+        std::string input_pointcloud_layer = mp2p_icp::metric_map_t::PT_LAYER_RAW;
+        std::string output_layer_inliers;
+        std::string output_layer_outliers;
+
+        /** Number of neighbors to analyze for each point. */
+        unsigned int mean_k = 20;
+
+        /** Standard deviation multiplier threshold. */
+        double std_dev_mul = 1.0;
+
+        /** When TBB is enabled, the grainsize for parallel processing. */
+        size_t parallelization_grain_size = 1024UL;
+    };
+
+    Parameters params;
+
+   protected:
+    void initialize_filter(const mrpt::containers::yaml& c) override;
+
+   private:
+    struct Impl;
+    mutable mrpt::pimpl<Impl> impl_;
+};
+
+}  // namespace mp2p_icp_filters

mp2p_icp_filters/src/FilterSOR.cpp

@@ -0,0 +1,201 @@
+/*               _
+ _ __ ___   ___ | | __ _
+| '_ ` _ \ / _ \| |/ _` | Modular Optimization framework for
+| | | | | | (_) | | (_| | Localization and mApping (MOLA)
+|_| |_| |_|\___/|_|\__,_| https://github.com/MOLAorg/mola
+
+ A repertory of multi primitive-to-primitive (MP2P) ICP algorithms
+ and map building tools. mp2p_icp is part of MOLA.
+
+ Copyright (C) 2018-2025 Jose Luis Blanco, University of Almeria,
+                         and individual contributors.
+ SPDX-License-Identifier: BSD-3-Clause
+*/
+
+/**
+ * @file   FilterSOR.cpp
+ * @brief  Statistical Outlier Removal (SOR) filter.
+ * @author Jose Luis Blanco Claraco
+ * @date   Dec 28, 2025
+ */
+
+#include <mp2p_icp_filters/FilterSOR.h>
+#include <mp2p_icp_filters/GetOrCreatePointLayer.h>
+#include <mrpt/containers/yaml.h>
+#include <mrpt/math/ops_containers.h>  // meanAndCov
+#include <mrpt/version.h>
+
+#if defined(MP2P_HAS_TBB)
+#include <tbb/blocked_range.h>
+#include <tbb/parallel_for.h>
+#endif
+
+IMPLEMENTS_MRPT_OBJECT(FilterSOR, mp2p_icp_filters::FilterBase, mp2p_icp_filters)
+
+using namespace mp2p_icp_filters;
+
+FilterSOR::FilterSOR()
+{
+#if MRPT_VERSION < 0x020f04
+    throw std::runtime_error("FilterSOR requires MRPT >= 2.15.4");
+#endif
+}
+
+void FilterSOR::Parameters::load_from_yaml(const mrpt::containers::yaml& c)
+{
+    MCP_LOAD_REQ(c, input_pointcloud_layer);
+    MCP_LOAD_OPT(c, output_layer_inliers);
+    MCP_LOAD_OPT(c, output_layer_outliers);
+    MCP_LOAD_OPT(c, mean_k);
+    MCP_LOAD_OPT(c, std_dev_mul);
+    MCP_LOAD_OPT(c, parallelization_grain_size);
+
+    ASSERTMSG_(
+        !output_layer_inliers.empty() || !output_layer_outliers.empty(),
+        "FilterSOR: At least one output layer must be specified.");
+}
+
+void FilterSOR::initialize_filter(const mrpt::containers::yaml& c) { params.load_from_yaml(c); }
+
+void FilterSOR::filter(mp2p_icp::metric_map_t& inOut) const
+{
+    auto pcPtr = inOut.layer<mrpt::maps::CPointsMap>(params.input_pointcloud_layer);
+    if (!pcPtr || pcPtr->empty())
+    {
+        return;
+    }
+
+    const auto&  pc = *pcPtr;
+    const size_t N  = pc.size();
+
+    // 1. Prepare KD-Tree for neighbor search
+    pc.nn_prepare_for_3d_queries();
+
+    // 2. Pass 1: Compute mean distances to K neighbors
+    std::vector<float> avg_distances(N, 0.0f);
+
+    auto lambda_process = [&](size_t i)
+    {
+        mrpt::math::TPoint3Df q;
+        pc.getPoint(i, q.x, q.y, q.z);
+
+        std::vector<float>  dists_sq;
+        std::vector<size_t> indices;
+        // PCL logic: nearestKSearch includes the query point itself at index 0
+        // So we search for K+1 points.
+        pc.kdTreeNClosestPoint3DIdx(q, params.mean_k + 1, indices, dists_sq);
+
+        const auto found = indices.size();
+
+        if (found <= 1)
+        {
+            avg_distances[i] = 0.0f;  // Isolated points are treated as inliers
+            return;
+        }
+
+        double sum_dist = 0;
+        for (size_t k = 1; k < found; ++k)
+        {
+            sum_dist += std::sqrt(dists_sq[k]);
+        }
+        avg_distances[i] = static_cast<float>(sum_dist / static_cast<float>(found - 1));
+    };
+
+#if defined(MP2P_HAS_TBB)
+    tbb::parallel_for(
+        tbb::blocked_range<size_t>(0, N, params.parallelization_grain_size),
+        [&](const tbb::blocked_range<size_t>& r)
+        {
+            for (size_t i = r.begin(); i < r.end(); ++i)
+            {
+                lambda_process(i);
+            }
+        });
+#else
+    for (size_t i = 0; i < N; ++i)
+    {
+        lambda_process(i);
+    }
+#endif
+
+    // 3. Compute Global Statistics
+    double mean_dist, std_dev;
+    mrpt::math::meanAndStd(avg_distances, mean_dist, std_dev);
+    const double threshold = mean_dist + params.std_dev_mul * std_dev;
+
+    // 4. Pass 2: Dispatch points to output layers
+    mrpt::maps::CPointsMap::Ptr outInliers = GetOrCreatePointLayer(
+        inOut, params.output_layer_inliers,
+        /*do allow empty*/
+        true,
+        /* create cloud of the same type */
+        pcPtr->GetRuntimeClass()->className);
+
+    mrpt::maps::CPointsMap::Ptr outOutliers = GetOrCreatePointLayer(
+        inOut, params.output_layer_outliers,
+        /*do allow empty*/
+        true,
+        /* create cloud of the same type */
+        pcPtr->GetRuntimeClass()->className);
+
+    ASSERTMSG_(
+        outInliers || outOutliers,
+        "At least one of 'output_layer_inliers' or 'output_layer_outliers' must be provided.");
+
+    std::optional<mrpt::maps::CPointsMap::InsertCtx> ctxI, ctxO;
+    if (outInliers)
+    {
+        outInliers->registerPointFieldsFrom(pc);
+        ctxI = outInliers->prepareForInsertPointsFrom(pc);
+    }
+    if (outOutliers)
+    {
+        outOutliers->registerPointFieldsFrom(pc);
+        ctxO = outOutliers->prepareForInsertPointsFrom(pc);
+    }
+
+    for (size_t i = 0; i < N; ++i)
+    {
+        const bool isInlier = (avg_distances[i] <= threshold);
+        if (isInlier)
+        {
+#if MRPT_VERSION >= 0x020f03  // 2.15.3
+            if (outInliers)
+            {
+                outInliers->insertPointFrom(i, *ctxI);
+            }
+        }
+        else
+        {
+            if (outOutliers)
+            {
+                outOutliers->insertPointFrom(i, *ctxO);
+            }
+#elif MRPT_VERSION >= 0x020f00  // 2.15.0
+            if (outInliers)
+            {
+                outInliers->insertPointFrom(pc, i, *ctxI);
+            }
+        }
+        else
+        {
+            if (outOutliers)
+            {
+                outOutliers->insertPointFrom(pc, i, *ctxO);
+            }
+#else
+            if (outInliers)
+            {
+                outInliers->insertPointFrom(pc, i);
+            }
+        }
+        else
+        {
+            if (outOutliers)
+            {
+                outOutliers->insertPointFrom(pc, i);
+            }
+#endif
+        }
+    }
+}

mp2p_icp_filters/src/register.cpp

@@ -41,6 +41,7 @@
 #include <mp2p_icp_filters/FilterNormalizeIntensity.h>
 #include <mp2p_icp_filters/FilterPoleDetector.h>
 #include <mp2p_icp_filters/FilterRemoveByVoxelOccupancy.h>
+#include <mp2p_icp_filters/FilterSOR.h>
 #include <mp2p_icp_filters/FilterVoxelSlice.h>
 #include <mp2p_icp_filters/Generator.h>
 #include <mp2p_icp_filters/GeneratorEdgesFromCurvature.h>
@@ -76,5 +77,6 @@ MRPT_INITIALIZER(register_mola_lidar_segmentation)
     registerClass(CLASS_ID(mp2p_icp_filters::FilterNormalizeIntensity));
     registerClass(CLASS_ID(mp2p_icp_filters::FilterPoleDetector));
     registerClass(CLASS_ID(mp2p_icp_filters::FilterRemoveByVoxelOccupancy));
+    registerClass(CLASS_ID(mp2p_icp_filters::FilterSOR));
     registerClass(CLASS_ID(mp2p_icp_filters::FilterVoxelSlice));
 }

tests/CMakeLists.txt

@@ -59,9 +59,11 @@ mp2p_add_test(mp2p_optimize_pt2ln)
 mp2p_add_test(mp2p_optimize_pt2pl)
 mp2p_add_test(mp2p_optimize_with_prior)
 mp2p_add_test(mp2p_quality_reproject_ranges)
+
 mp2p_add_test(mp2p_FilterDecimateVoxels EXTRA_LIBS mp2p_icp_filters)
 mp2p_add_test(mp2p_Filters EXTRA_LIBS mp2p_icp_filters)
 mp2p_add_test(mp2p_FilterByExpression EXTRA_LIBS mp2p_icp_filters)
+mp2p_add_test(mp2p_FilterSOR EXTRA_LIBS mp2p_icp_filters)
 
 if (mola_test_datasets_FOUND)
   mp2p_add_test(mp2p_quality_voxels)