Don't calculate distance when we already have it.

PDAL · Dec 16, 2019 · a73a65a · a73a65a
1 parent 0c414ad
commit a73a65a
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 12 deletions.
diff --git a/filters/HAGFilter.cpp b/filters/HAGFilter.cpp
@@ -224,7 +224,9 @@ void HAGFilter::filter(PointView& view)
         double x0 = point.getFieldAs<double>(Id::X);
         double y0 = point.getFieldAs<double>(Id::Y);
         double z0 = point.getFieldAs<double>(Id::Z);
-        PointIdList ids = kdi.neighbors(point, m_count);
+        PointIdList ids(m_count);
+        std::vector<double> sqr_dists(m_count);
+        kdi.knnSearch(x0, y0, m_count, &ids, &sqr_dists);
 
         double z1 = gView->getFieldAs<double>(Id::Z, ids[0]);
         assert(ids.size() > 0);
@@ -244,8 +246,8 @@ void HAGFilter::filter(PointView& view)
                 auto x = gView->getFieldAs<double>(Id::X, ids[j]);
                 auto y = gView->getFieldAs<double>(Id::Y, ids[j]);
                 auto z = gView->getFieldAs<double>(Id::Z, ids[j]);
-                auto distance2 = std::pow(x - x0, 2) + std::pow(y - y0, 2);
-                if (distance2 == 0)
+                double sqr_dist = sqr_dists[j];
+                if (sqr_dist == 0)
                 {
                     //ABELL - This doesn't make sense.  Z1 is set, but if
                     // exact match is set, it gets overwritten outside of
@@ -254,10 +256,10 @@ void HAGFilter::filter(PointView& view)
                     z1 = z;
                     break;
                 }
-                if (maxDistance2 > 0 && distance2 > maxDistance2) {
+                if (maxDistance2 > 0 && sqr_dist > maxDistance2) {
                     break;
                 } else {
-                    auto weight = 1 / distance2;
+                    auto weight = 1 / sqr_dist;
                     weights += weight;
                     z_accumulator += weight * z;
                 }

diff --git a/pdal/KDIndex.hpp b/pdal/KDIndex.hpp
@@ -153,7 +153,7 @@ class PDAL_DLL KD2Index : public KDIndex<2>
     }
 
     void knnSearch(double x, double y, point_count_t k,
-        PointIdList *indices, std::vector<double> *sqr_dists)
+        PointIdList *indices, std::vector<double> *sqr_dists) const
     {
         k = (std::min)(m_buf.size(), k);
         nanoflann::KNNResultSet<double, PointId, point_count_t> resultSet(k);
@@ -167,14 +167,23 @@ class PDAL_DLL KD2Index : public KDIndex<2>
     }
 
     void knnSearch(PointId idx, point_count_t k, PointIdList *indices,
-        std::vector<double> *sqr_dists)
+        std::vector<double> *sqr_dists) const
     {
         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);
     }
 
+    void knnSearch(PointRef& point, point_count_t k, PointIdList *indices,
+        std::vector<double> *sqr_dists) const
+    {
+        double x = point.getFieldAs<double>(Dimension::Id::X);
+        double y = point.getFieldAs<double>(Dimension::Id::Y);
+
+        knnSearch(x, y, k, indices, sqr_dists);
+    }
+
     PointIdList radius(double const& x, double const& y,
         double const& r) const
     {
@@ -289,7 +298,8 @@ class PDAL_DLL KD3Index : public KDIndex<3>
         return neighbors(x, y, z, k, stride);
     }
 
-    PointIdList neighbors(PointRef &point, point_count_t k, size_t stride=1) const
+    PointIdList neighbors(PointRef &point, point_count_t k,
+        size_t stride=1) const
     {
         double x = point.getFieldAs<double>(Dimension::Id::X);
         double y = point.getFieldAs<double>(Dimension::Id::Y);
@@ -299,7 +309,7 @@ class PDAL_DLL KD3Index : public KDIndex<3>
     }
 
     void knnSearch(double x, double y, double z, point_count_t k,
-        PointIdList *indices, std::vector<double> *sqr_dists)
+        PointIdList *indices, std::vector<double> *sqr_dists) const
     {
         k = (std::min)(m_buf.size(), k);
         nanoflann::KNNResultSet<double, PointId, point_count_t> resultSet(k);
@@ -314,7 +324,7 @@ class PDAL_DLL KD3Index : public KDIndex<3>
     }
 
     void knnSearch(PointId idx, point_count_t k, PointIdList *indices,
-        std::vector<double> *sqr_dists)
+        std::vector<double> *sqr_dists) const
     {
         double x = m_buf.getFieldAs<double>(Dimension::Id::X, idx);
         double y = m_buf.getFieldAs<double>(Dimension::Id::Y, idx);
@@ -324,7 +334,7 @@ class PDAL_DLL KD3Index : public KDIndex<3>
     }
 
     void knnSearch(PointRef &point, point_count_t k,
-        PointIdList *indices, std::vector<double> *sqr_dists)
+        PointIdList *indices, std::vector<double> *sqr_dists) const
     {
         double x = point.getFieldAs<double>(Dimension::Id::X);
         double y = point.getFieldAs<double>(Dimension::Id::Y);
@@ -372,7 +382,6 @@ class PDAL_DLL KD3Index : public KDIndex<3>
 
         return radius(x, y, z, r);
     }
-
 };
 
 template<>