Port PMF to use the new morphological opening (iterative diamond kernel)

filters/PMFFilter.cpp

@@ -1,5 +1,5 @@
 /******************************************************************************
-* Copyright (c) 2015, Bradley J Chambers (brad.chambers@gmail.com)
+* Copyright (c) 2015-2017, Bradley J Chambers (brad.chambers@gmail.com)
 *
 * All rights reserved.
 *
@@ -35,6 +35,7 @@
 #include "PMFFilter.hpp"
 
 #include <pdal/EigenUtils.hpp>
+#include <pdal/KDIndex.hpp>
 #include <pdal/pdal_macros.hpp>
 #include <pdal/QuadIndex.hpp>
 #include <pdal/util/ProgramArgs.hpp>
@@ -75,6 +76,7 @@ void PMFFilter::addDimensions(PointLayoutPtr layout)
     layout->registerDim(Dimension::Id::Classification);
 }
 
+
 std::vector<double> PMFFilter::morphOpen(PointViewPtr view, float radius)
 {
     point_count_t np(view->size());
@@ -90,7 +92,7 @@ std::vector<double> PMFFilter::morphOpen(PointViewPtr view, float radius)
         double y = view->getFieldAs<double>(Dimension::Id::Y, i);
 
         std::vector<PointId> ids = idx.getPoints(x - radius, y - radius,
-            x + radius, y + radius);
+                                   x + radius, y + radius);
 
         double localMin(std::numeric_limits<double>::max());
         for (auto const& j : ids)
@@ -109,7 +111,7 @@ std::vector<double> PMFFilter::morphOpen(PointViewPtr view, float radius)
         double y = view->getFieldAs<double>(Dimension::Id::Y, i);
 
         std::vector<PointId> ids = idx.getPoints(x - radius, y - radius,
-            x + radius, y + radius);
+                                   x + radius, y + radius);
 
         double localMax(std::numeric_limits<double>::lowest());
         for (auto const& j : ids)
@@ -147,7 +149,7 @@ std::vector<PointId> PMFFilter::processGround(PointViewPtr view)
             ht = m_initialDistance;
         else
             ht = m_slope * (ws - wsvec[iter-1]) * m_cellSize +
-                m_initialDistance;
+                 m_initialDistance;
 
         // Enforce max distance on height threshold
         if (ht > m_maxDistance)
@@ -200,6 +202,59 @@ std::vector<PointId> PMFFilter::processGround(PointViewPtr view)
     return groundIdx;
 }
 
+
+Eigen::MatrixXd PMFFilter::fillNearest(PointViewPtr view, Eigen::MatrixXd cz,
+                                       double cell_size, BOX2D bounds)
+{
+    using namespace Dimension;
+    using namespace Eigen;
+
+    // convert cz into PointView
+    PointViewPtr temp = view->makeNew();
+    PointId i(0);
+    for (int c = 0; c < cz.cols(); ++c)
+    {
+        for (int r = 0; r < cz.rows(); ++r)
+        {
+            if (std::isnan(cz(r, c)))
+                continue;
+
+            temp->setField(Id::X, i, bounds.minx + (c+0.5) * cell_size);
+            temp->setField(Id::Y, i, bounds.miny + (r+0.5) * cell_size);
+            temp->setField(Id::Z, i, cz(r, c));
+            i++;
+        }
+    }
+
+    // make a 2D KDIndex
+    KD2Index kdi(*temp);
+    kdi.build();
+
+    MatrixXd out = cz;
+    for (int c = 0; c < cz.cols(); ++c)
+    {
+        for (int r = 0; r < cz.rows(); ++r)
+        {
+            if (!std::isnan(out(r, c)))
+                continue;
+
+            // find k nearest points
+            double x = bounds.minx + (c+0.5) * cell_size;
+            double y = bounds.miny + (r+0.5) * cell_size;
+            int k = 1;
+            std::vector<PointId> neighbors(k);
+            std::vector<double> sqr_dists(k);
+            kdi.knnSearch(x, y, k, &neighbors, &sqr_dists);
+
+            out(r, c) = temp->getFieldAs<double>(Dimension::Id::Z,
+                                                 neighbors[0]);
+        }
+    }
+
+    return out;
+};
+
+
 std::vector<PointId> PMFFilter::processGroundApprox(PointViewPtr view)
 {
     using namespace Eigen;
@@ -230,7 +285,7 @@ std::vector<PointId> PMFFilter::processGroundApprox(PointViewPtr view)
             ht = m_initialDistance;
         else
             ht = m_slope * (ws - wsvec[iter-1]) * m_cellSize +
-                m_initialDistance;
+                 m_initialDistance;
 
         // Enforce max distance on height threshold
         if (ht > m_maxDistance)
@@ -249,6 +304,8 @@ std::vector<PointId> PMFFilter::processGroundApprox(PointViewPtr view)
     MatrixXd ZImin = eigen::createMinMatrix(*view.get(), rows, cols, m_cellSize,
                                             bounds);
 
+    ZImin = fillNearest(view, ZImin, m_cellSize, bounds);
+
     // Progressively filter ground returns using morphological open
     for (size_t j = 0; j < wsvec.size(); ++j)
     {
@@ -257,7 +314,7 @@ std::vector<PointId> PMFFilter::processGroundApprox(PointViewPtr view)
                                     << ", window size = " << wsvec[j]
                                     << ")...\n";
 
-        MatrixXd mo = eigen::matrixOpen(ZImin, 0.5*(wsvec[j]-1));
+        MatrixXd mo = eigen::openDiamond(ZImin, 0.5*(wsvec[j]-1));
 
         std::vector<PointId> groundNewIdx;
         for (auto p_idx : groundIdx)
@@ -266,15 +323,10 @@ std::vector<PointId> PMFFilter::processGroundApprox(PointViewPtr view)
             double y = view->getFieldAs<double>(Dimension::Id::Y, p_idx);
             double z = view->getFieldAs<double>(Dimension::Id::Z, p_idx);
 
-            int c = Utils::clamp(
-                static_cast<int>(floor((x - bounds.minx) / m_cellSize)), 0,
-                static_cast<int>(cols-1));
-            int r = Utils::clamp(
-                static_cast<int>(floor((y - bounds.miny) / m_cellSize)), 0,
-                static_cast<int>(rows-1));
+            int c = static_cast<int>(floor((x - bounds.minx) / m_cellSize));
+            int r = static_cast<int>(floor((y - bounds.miny) / m_cellSize));
 
-            float diff = z - mo(r, c);
-            if (diff < htvec[j])
+            if ((z - mo(r, c)) < htvec[j])
                 groundNewIdx.push_back(p_idx);
         }
 
@@ -288,6 +340,7 @@ std::vector<PointId> PMFFilter::processGroundApprox(PointViewPtr view)
     return groundIdx;
 }
 
+
 PointViewSet PMFFilter::run(PointViewPtr input)
 {
     bool logOutput = log()->getLevel() > LogLevel::Debug1;
@@ -340,11 +393,11 @@ PointViewSet PMFFilter::run(PointViewPtr input)
     {
         if (idx.empty())
             log()->get(LogLevel::Debug2) << "Filtered cloud has no "
-                "ground returns!\n";
+                                         "ground returns!\n";
 
         if (!(m_classify || m_extract))
             log()->get(LogLevel::Debug2) << "Must choose --classify "
-                "or --extract\n";
+                                         "or --extract\n";
 
         // return the input buffer unchanged
         viewSet.insert(input);

filters/PMFFilter.hpp

@@ -1,5 +1,5 @@
 /******************************************************************************
-* Copyright (c) 2016, Bradley J Chambers (brad.chambers@gmail.com)
+* Copyright (c) 2016-2017, Bradley J Chambers (brad.chambers@gmail.com)
 *
 * All rights reserved.
 *
@@ -37,6 +37,8 @@
 #include <pdal/Filter.hpp>
 #include <pdal/plugin.hpp>
 
+#include <Eigen/Dense>
+
 #include <memory>
 
 extern "C" int32_t PMFFilter_ExitFunc();
@@ -72,6 +74,8 @@ class PDAL_DLL PMFFilter : public Filter
 
     virtual void addDimensions(PointLayoutPtr layout);
     virtual void addArgs(ProgramArgs& args);
+    Eigen::MatrixXd fillNearest(PointViewPtr view, Eigen::MatrixXd cz,
+                                double cell_size, BOX2D bounds);
     std::vector<double> morphOpen(PointViewPtr view, float radius);
     std::vector<PointId> processGround(PointViewPtr view);
     std::vector<PointId> processGroundApprox(PointViewPtr view);

pdal/EigenUtils.cpp

@@ -43,6 +43,7 @@
 #include <Eigen/Dense>
 
 #include <cfloat>
+#include <numeric>
 #include <vector>
 
 namespace pdal
@@ -467,6 +468,96 @@ Eigen::MatrixXd matrixOpen(Eigen::MatrixXd data, int radius)
     return maxZ.block(radius, radius, data.rows(), data.cols());
 }
 
+Eigen::MatrixXd openDiamond(Eigen::MatrixXd data, int iterations)
+{
+    using namespace Eigen;
+
+    MatrixXd prev(data.rows()+2, data.cols()+2);
+    prev.setConstant(std::numeric_limits<double>::max());
+    prev.block(1, 1, data.rows(), data.cols()) = data;
+
+    // Construct a list of indices relative to the current pixel to be used in
+    // the diamond kernel.
+    std::list<int> idx {
+      static_cast<int>(-prev.rows()),
+      -1,
+      0,
+      1,
+      static_cast<int>(prev.rows())
+    };
+
+    // Generate lists of rows, cols, and iterations to be used in subsequent for
+    // loops.
+    std::list<int> rows(data.rows());
+    std::iota(rows.begin(), rows.end(), 1);
+
+    std::list<int> cols(data.cols());
+    std::iota(cols.begin(), cols.end(), 1);
+
+    std::list<int> iters(iterations);
+    std::iota(iters.begin(), iters.end(), 0);
+
+    // Implementation note. We tried a number of different approaches here,
+    // including using Eigen block extraction and min/maxCoeff, but those always
+    // resulted in a ~2x increase in runtime.
+
+    // Begin by initializing the min Z matrix to max double, so that we always
+    // find a value smaller than the initial value.
+    MatrixXd minZ(prev.rows(), prev.cols());
+    minZ.setConstant(std::numeric_limits<double>::max());
+
+    // To repeat a morphological opening, we actually repeat the erosion step
+    // first, followed by the repeated dilation step.
+    for (auto const& i : iters)
+    {
+        for (auto const& c : cols)
+        {
+            int index_base = c*prev.rows();
+            for (auto const& r : rows)
+            {              
+                int index = index_base + r;
+                for (auto const& j : idx)
+                {
+                    double v = prev(index+j);
+                    if (v < minZ(r, c))
+                        minZ(r, c) = v;
+                }
+            }
+        }
+
+        prev = minZ;
+    }
+
+    // Begin by initializing max Z matrix to lowest double, so that we always
+    // find a value larger than the initial value.
+    MatrixXd maxZ(prev.rows(), prev.cols());
+    maxZ.setConstant(std::numeric_limits<double>::lowest());
+
+    // Complete the repeated opening by repeating the dilation step.
+    for (auto const& i : iters)
+    {
+        for (auto const& c : cols)
+        {
+            int index_base = c*prev.rows();
+            for (auto const& r : rows)
+            {                
+                int index = index_base + r;
+                for (auto const& j : idx)
+                {
+                    double v = prev(index+j);
+                    if (v > maxZ(r, c))
+                        maxZ(r, c) = v;
+                }
+            }
+        }
+
+        prev = maxZ;
+    }
+
+    // Do not return the padded borders.
+    return prev.block(1, 1, data.rows(), data.cols());
+}
+
 Eigen::MatrixXd pointViewToEigen(const PointView& view)
 {
     Eigen::MatrixXd matrix(view.size(), 3);

pdal/EigenUtils.hpp

@@ -268,7 +268,7 @@ PDAL_DLL Eigen::MatrixXd createMaxMatrix2(PointView& view, int rows, int cols,
 */
 PDAL_DLL Eigen::MatrixXd createMinMatrix(PointView& view, int rows, int cols,
         double cell_size, BOX2D bounds);
-
+        
 /**
   Find local minimum elevations by extended local minimum.
 
@@ -316,6 +316,21 @@ PDAL_DLL Eigen::MatrixXd matrixClose(Eigen::MatrixXd data, int radius);
 */
 PDAL_DLL Eigen::MatrixXd matrixOpen(Eigen::MatrixXd data, int radius);
 
+/**
+  Perform a morphological opening of the input matrix.
+
+  Performs a morphological opening of the input matrix using a diamond
+  structuring element. Larger structuring elements are approximated by applying
+  multiple iterations of the opening operation. Data will be symmetrically
+  padded at its edges.
+
+  \param data the input matrix.
+  \param iterations the number of iterations used to approximate a larger 
+         structuring element.
+  \return the morphological opening of the input radius.
+*/
+PDAL_DLL Eigen::MatrixXd openDiamond(Eigen::MatrixXd data, int iterations);
+
 /**
   Pad input matrix symmetrically.
 

pdal/KDIndex.hpp

@@ -149,6 +149,29 @@ class PDAL_DLL KD2Index : public KDIndex<2>
 
         return neighbors(x, y, k);
     }
+
+    void knnSearch(double x, double y, point_count_t k,
+        std::vector<PointId> *indices, std::vector<double> *sqr_dists)
+    {
+        k = std::min(m_buf.size(), k);
+        nanoflann::KNNResultSet<double, PointId, point_count_t> resultSet(k);
+
+        resultSet.init(&indices->front(), &sqr_dists->front());
+
+        std::vector<double> pt;
+        pt.push_back(x);
+        pt.push_back(y);
+        m_index->findNeighbors(resultSet, &pt[0], nanoflann::SearchParams(10));
+    }
+
+    void knnSearch(PointId idx, point_count_t k, std::vector<PointId> *indices,
+        std::vector<double> *sqr_dists)
+    {
+        double x = m_buf.getFieldAs<double>(Dimension::Id::X, idx);
+        double y = m_buf.getFieldAs<double>(Dimension::Id::Y, idx);
+
+        knnSearch(x, y, k, indices, sqr_dists);
+    }
 
     std::vector<PointId> radius(double const& x, double const& y,
         double const& r) const