diff --git a/include/pdal/KDIndex.hpp b/include/pdal/KDIndex.hpp index 4bb9755f9b..2ea3372a74 100644 --- a/include/pdal/KDIndex.hpp +++ b/include/pdal/KDIndex.hpp @@ -34,14 +34,15 @@ #pragma once +#include "nanoflann.hpp" + #include #include namespace nanoflann { template - class KDTreeSingleIndexAdaptor; + typename IndexType> class KDTreeSingleIndexAdaptor; template struct L2_Adaptor; @@ -50,71 +51,282 @@ namespace nanoflann namespace pdal { +template class PDAL_DLL KDIndex { -public: - KDIndex(const PointView& buf); - ~KDIndex(); +protected: + KDIndex(const PointView& buf) : m_buf(buf) + {} + + ~KDIndex() + {} - std::size_t kdtree_get_point_count() const; +public: + std::size_t kdtree_get_point_count() const + { return m_buf.size(); } double kdtree_get_pt(const PointId idx, int dim) const; + double kdtree_distance(const double *p1, const PointId p2_idx, + size_t /*numDims*/) const; + template bool kdtree_get_bbox(BBOX& bb) const; + void build() + { + m_index.reset(new my_kd_tree_t(DIM, *this, + nanoflann::KDTreeSingleIndexAdaptorParams(10, DIM))); + m_index->buildIndex(); + } - double kdtree_distance( - const PointId idx_p2, - point_count_t size) const; +protected: + const PointView& m_buf; - template bool kdtree_get_bbox(BBOX &bb) const + typedef nanoflann::KDTreeSingleIndexAdaptor, KDIndex, -1, std::size_t> my_kd_tree_t; + + std::unique_ptr m_index; + +private: + KDIndex(const KDIndex&); + KDIndex& operator=(KDIndex&); +}; + +class PDAL_DLL KD2Index : public KDIndex<2> +{ +public: + KD2Index(const PointView& buf) : KDIndex<2>(buf) { - BOX3D bounds = m_buf.calculateBounds(); - if (bounds.empty()) - return false; + if (!buf.hasDim(Dimension::Id::X)) + throw pdal_error("KD2Index: point view missing 'X' dimension."); + if (!buf.hasDim(Dimension::Id::Y)) + throw pdal_error("KD2Index: point view missing 'Y' dimension."); + } - bb[0].low = bounds.minx; - bb[0].high = bounds.maxx; - bb[1].low = bounds.miny; - bb[1].high = bounds.maxy; + PointId neighbor(double x, double y) + { + std::vector ids = neighbors(x, y, 1); + return (ids.size() ? ids[0] : 0); + } - if (m_3d) - { - bb[2].low = bounds.minx; - bb[2].high = bounds.maxx; - } - return true; + std::vector neighbors(double x, double y, point_count_t k) + { + k = std::min(m_buf.size(), k); + std::vector output(k); + std::vector out_dist_sqr(k); + nanoflann::KNNResultSet resultSet(k); + + resultSet.init(&output[0], &out_dist_sqr[0]); + + std::vector pt; + pt.push_back(x); + pt.push_back(y); + m_index->findNeighbors(resultSet, &pt[0], nanoflann::SearchParams(10)); + return output; + } + + std::vector radius(double const& x, double const& y, + double const& r) const + { + std::vector output; + std::vector> ret_matches; + nanoflann::SearchParams params; + params.sorted = true; + + std::vector pt; + pt.push_back(x); + pt.push_back(y); + + // Our distance metric is square distance, so we use the square of + // the radius. + const std::size_t count = + m_index->radiusSearch(&pt[0], r * r, ret_matches, params); + + for (std::size_t i = 0; i < count; ++i) + output.push_back(ret_matches[i].first); + return output; } +}; - std::vector radius( - double const& x, - double const& y, - double const& z, - double const& r) const; +class PDAL_DLL KD3Index : public KDIndex<3> +{ +public: + KD3Index(const PointView& buf) : KDIndex<3>(buf) + { + if (!buf.hasDim(Dimension::Id::X)) + throw pdal_error("KD3Index: point view missing 'X' dimension."); + if (!buf.hasDim(Dimension::Id::Y)) + throw pdal_error("KD3Index: point view missing 'Y' dimension."); + if (!buf.hasDim(Dimension::Id::Z)) + throw pdal_error("KD3Index: point view missing 'Z' dimension."); + } - PointId neighbor(double const& x, double const& y, double const& z) const + PointId neighbor(double x, double y, double z) { std::vector ids = neighbors(x, y, z, 1); - return ids[0]; + return (ids.size() ? ids[0] : 0); } - std::vector neighbors( - double const& x, - double const& y, - double const& z, - point_count_t count) const; + std::vector neighbors(double x, double y, double z, + point_count_t k) + { + k = std::min(m_buf.size(), k); + std::vector output(k); + std::vector out_dist_sqr(k); + nanoflann::KNNResultSet resultSet(k); - void build(bool b3d = true); + resultSet.init(&output[0], &out_dist_sqr[0]); -private: - const PointView& m_buf; - bool m_3d; + std::vector pt; + pt.push_back(x); + pt.push_back(y); + pt.push_back(z); + m_index->findNeighbors(resultSet, &pt[0], nanoflann::SearchParams(10)); + return output; + } - typedef nanoflann::KDTreeSingleIndexAdaptor, KDIndex, -1, std::size_t> my_kd_tree_t; + std::vector radius(double x, double y, double z, double r) const + { + std::vector output; + std::vector> ret_matches; + nanoflann::SearchParams params; + params.sorted = true; - std::unique_ptr m_index; + std::vector pt; + pt.push_back(x); + pt.push_back(y); + pt.push_back(z); - KDIndex(const KDIndex&); - KDIndex& operator=(KDIndex&); + // Our distance metric is square distance, so we use the square of + // the radius. + const std::size_t count = + m_index->radiusSearch(&pt[0], r * r, ret_matches, params); + + for (std::size_t i = 0; i < count; ++i) + output.push_back(ret_matches[i].first); + return output; + } }; +template<> +inline +double KDIndex<2>::kdtree_get_pt(const PointId idx, int dim) const +{ + if (idx >= m_buf.size()) + return 0.0; + + Dimension::Id::Enum id = Dimension::Id::Unknown; + switch (dim) + { + case 0: + id = Dimension::Id::X; + break; + case 1: + id = Dimension::Id::Y; + break; + default: + throw pdal_error("kdtree_get_pt: Request for invalid dimension " + "from nanoflann"); + } + return m_buf.getFieldAs(id, idx); +} + +template<> +inline +double KDIndex<3>::kdtree_get_pt(const PointId idx, int dim) const +{ + if (idx >= m_buf.size()) + return 0.0; + + Dimension::Id::Enum id = Dimension::Id::Unknown; + switch (dim) + { + case 0: + id = Dimension::Id::X; + break; + case 1: + id = Dimension::Id::Y; + break; + case 2: + id = Dimension::Id::Z; + break; + default: + throw pdal_error("kdtree_get_pt: Request for invalid dimension " + "from nanoflann"); + } + return m_buf.getFieldAs(id, idx); +} + +// nanoflann hands us a vector that represents the position of p1. We fetch +// the position of p2 and and compute the square distance. +template<> +inline double KDIndex<2>::kdtree_distance(const double *p1, const PointId idx, + size_t /*numDims*/) const +{ + double d0 = p1[0] - m_buf.getFieldAs(Dimension::Id::X, idx); + double d1 = p1[1] - m_buf.getFieldAs(Dimension::Id::Y, idx); + + return (d0 * d0 + d1 * d1); +} + +template<> +inline double KDIndex<3>::kdtree_distance(const double *p1, const PointId idx, + size_t /*numDims*/) const +{ + double d0 = p1[0] - m_buf.getFieldAs(Dimension::Id::X, idx); + double d1 = p1[1] - m_buf.getFieldAs(Dimension::Id::Y, idx); + double d2 = p1[2] - m_buf.getFieldAs(Dimension::Id::Z, idx); + + return (d0 * d0 + d1 * d1 + d2 * d2); +} + + +template<> +template +bool KDIndex<2>::kdtree_get_bbox(BBOX& bb) const +{ + if (m_buf.empty()) + { + bb[0].low = 0.0; + bb[0].high = 0.0; + bb[1].low = 0.0; + bb[1].high = 0.0; + } + else + { + BOX3D bounds = m_buf.calculateBounds(); + + bb[0].low = bounds.minx; + bb[0].high = bounds.maxx; + bb[1].low = bounds.miny; + bb[1].high = bounds.maxy; + } + return true; +} + +template<> +template +bool KDIndex<3>::kdtree_get_bbox(BBOX& bb) const +{ + if (m_buf.empty()) + { + bb[0].low = 0.0; + bb[0].high = 0.0; + bb[1].low = 0.0; + bb[1].high = 0.0; + bb[2].low = 0.0; + bb[2].high = 0.0; + } + else + { + BOX3D bounds = m_buf.calculateBounds(); + + bb[0].low = bounds.minx; + bb[0].high = bounds.maxx; + bb[1].low = bounds.miny; + bb[1].high = bounds.maxy; + bb[2].low = bounds.minz; + bb[2].high = bounds.maxz; + } + return true; +} + } // namespace pdal diff --git a/io/ply/PlyReader.cpp b/io/ply/PlyReader.cpp index bf2e0c0fbd..27b9e0eaf1 100644 --- a/io/ply/PlyReader.cpp +++ b/io/ply/PlyReader.cpp @@ -90,6 +90,7 @@ int readPlyCallback(p_ply_argument argument) long numToRead; p_ply_property property; const char * propertyName; + if (!ply_get_argument_element(argument, &element, &index)) { std::stringstream ss; @@ -229,9 +230,14 @@ point_count_t PlyReader::read(PointViewPtr view, point_count_t num) context.view = view; context.dimensionMap = m_vertexDimensions; + // It's possible that point_count_t holds a value that's larger than the + // long that is the maximum rply (don't know about ply) point count. + long cnt; + cnt = Utils::inRange(num) ? num : (std::numeric_limits::max)(); for (auto it : m_vertexDimensions) { - ply_set_read_cb(m_ply, "vertex", it.first.c_str(), readPlyCallback, &context, num); + ply_set_read_cb(m_ply, "vertex", it.first.c_str(), readPlyCallback, + &context, cnt); } if (!ply_read(m_ply)) { diff --git a/kernels/delta/DeltaKernel.cpp b/kernels/delta/DeltaKernel.cpp index 5b5bcf2fb5..29d2e736f1 100644 --- a/kernels/delta/DeltaKernel.cpp +++ b/kernels/delta/DeltaKernel.cpp @@ -152,8 +152,8 @@ int DeltaKernel::execute() } // Index the candidate data. - KDIndex index(*candView); - index.build(m_3d); + KD3Index index(*candView); + index.build(); MetadataNode root; @@ -168,7 +168,7 @@ int DeltaKernel::execute() MetadataNode DeltaKernel::dump(PointViewPtr& srcView, PointViewPtr& candView, - KDIndex& index, DimIndexMap& dims) + KD3Index& index, DimIndexMap& dims) { MetadataNode root; @@ -217,7 +217,7 @@ void DeltaKernel::accumulate(DimIndex& d, double v) MetadataNode DeltaKernel::dumpDetail(PointViewPtr& srcView, - PointViewPtr& candView, KDIndex& index, DimIndexMap& dims) + PointViewPtr& candView, KD3Index& index, DimIndexMap& dims) { MetadataNode root; diff --git a/kernels/delta/DeltaKernel.hpp b/kernels/delta/DeltaKernel.hpp index a97fc8f332..499cde08cf 100644 --- a/kernels/delta/DeltaKernel.hpp +++ b/kernels/delta/DeltaKernel.hpp @@ -78,9 +78,9 @@ class PDAL_DLL DeltaKernel : public Kernel void addSwitches(); PointViewPtr loadSet(const std::string& filename, PointTable& table); MetadataNode dump(PointViewPtr& srcView, PointViewPtr& candView, - KDIndex& index, DimIndexMap& dims); + KD3Index& index, DimIndexMap& dims); MetadataNode dumpDetail(PointViewPtr& srcView, PointViewPtr& candView, - KDIndex& index, DimIndexMap& dims); + KD3Index& index, DimIndexMap& dims); void accumulate(DimIndex& d, double v); std::string m_sourceFile; diff --git a/kernels/info/InfoKernel.cpp b/kernels/info/InfoKernel.cpp index 53a3e2cdc5..d49436ffcb 100644 --- a/kernels/info/InfoKernel.cpp +++ b/kernels/info/InfoKernel.cpp @@ -406,17 +406,22 @@ MetadataNode InfoKernel::dumpQuery(PointViewPtr inView) const if (values.size() != 2 && values.size() != 3) throw app_runtime_error("--points must be two or three values"); - bool is3d = (values.size() >= 3); - - double x = values[0]; - double y = values[1]; - double z = is3d ? values[2] : 0.0; - PointViewPtr outView = inView->makeNew(); - KDIndex kdi(*inView); - kdi.build(is3d); - std::vector ids = kdi.neighbors(x, y, z, inView->size()); + std::vector ids; + if (values.size() >= 3) + { + KD3Index kdi(*inView); + kdi.build(); + ids = kdi.neighbors(values[0], values[1], values[2], inView->size()); + } + else + { + KD2Index kdi(*inView); + kdi.build(); + ids = kdi.neighbors(values[0], values[1], inView->size()); + } + for (auto i = ids.begin(); i != ids.end(); ++i) outView->appendPoint(*inView.get(), *i); diff --git a/plugins/pcl/filters/GroundFilter.cpp b/plugins/pcl/filters/GroundFilter.cpp index 3106f519d7..5d6db5f9d7 100644 --- a/plugins/pcl/filters/GroundFilter.cpp +++ b/plugins/pcl/filters/GroundFilter.cpp @@ -128,7 +128,6 @@ PointViewSet GroundFilter::run(PointViewPtr input) pmf.setCellSize(m_cellSize); // run the PMF filter, grabbing indices of ground returns - pcl::PointIndicesPtr idx(new pcl::PointIndices); pmf.extract(idx->indices); } else { diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e38c03cde1..8665fef536 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -79,7 +79,6 @@ set(PDAL_BASE_CPP gitsha.cpp GDALUtils.cpp GlobalEnvironment.cpp - KDIndex.cpp Kernel.cpp KernelFactory.cpp KernelSupport.cpp diff --git a/src/KDIndex.cpp b/src/KDIndex.cpp deleted file mode 100644 index 8788f9bd64..0000000000 --- a/src/KDIndex.cpp +++ /dev/null @@ -1,150 +0,0 @@ -/****************************************************************************** -* Copyright (c) 2011, Michael P. Gerlek (mpg@flaxen.com) -* -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following -* conditions are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in -* the documentation and/or other materials provided -* with the distribution. -* * Neither the name of Hobu, Inc. or Flaxen Geo Consulting nor the -* names of its contributors may be used to endorse or promote -* products derived from this software without specific prior -* written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT -* OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY -* OF SUCH DAMAGE. -****************************************************************************/ - -#include - -#include "nanoflann.hpp" - -namespace pdal -{ - -KDIndex::KDIndex(const PointView& buf) - : m_buf(buf) - , m_3d(buf.hasDim(Dimension::Id::Z)) - , m_index() -{ } - -KDIndex::~KDIndex() -{ } - -std::size_t KDIndex::kdtree_get_point_count() const -{ - return m_buf.size(); -} - -double KDIndex::kdtree_get_pt(const PointId idx, int dim) const -{ - Dimension::Id::Enum id = Dimension::Id::Unknown; - switch (dim) - { - case 0: - id = Dimension::Id::X; - break; - case 1: - id = Dimension::Id::Y; - break; - case 2: - id = Dimension::Id::Z; - break; - } - return m_buf.getFieldAs(id, idx); -} - -double KDIndex::kdtree_distance( - const PointId idx_p2, - point_count_t size) const -{ - double d0 = m_buf.getFieldAs(Dimension::Id::X, idx_p2) - - m_buf.getFieldAs(Dimension::Id::X, size - 1); - double d1 = m_buf.getFieldAs(Dimension::Id::Y, idx_p2) - - m_buf.getFieldAs(Dimension::Id::Y, size - 1); - - double output(d0 * d0 + d1 * d1); - if (m_3d) - { - double d2 = m_buf.getFieldAs(Dimension::Id::Z, idx_p2) - - m_buf.getFieldAs(Dimension::Id::Z, size - 1); - output += d2 * d2; - } - return output; -} - -void KDIndex::build(bool b3D) -{ - m_3d = b3D; - int nDims = m_3d && m_buf.hasDim(Dimension::Id::Z) ? 3 : 2; - m_index.reset( - new my_kd_tree_t( - nDims, - *this, - nanoflann::KDTreeSingleIndexAdaptorParams(10, nDims))); - m_index->buildIndex(); -} - -std::vector KDIndex::radius( - double const& x, - double const& y, - double const& z, - double const& r) const -{ - std::vector output; - std::vector> ret_matches; - nanoflann::SearchParams params; - params.sorted = true; - - std::vector pt; - pt.push_back(x); - pt.push_back(y); - pt.push_back(z); - const std::size_t count( - m_index->radiusSearch(&pt[0], r, ret_matches, params)); - - for (std::size_t i = 0; i < count; ++i) - output.push_back(ret_matches[i].first); - return output; -} - -std::vector KDIndex::neighbors( - double const& x, - double const& y, - double const& z, - point_count_t k) const -{ - std::vector output(k); - std::vector out_dist_sqr(k); - nanoflann::KNNResultSet resultSet(k); - - resultSet.init(&output[0], &out_dist_sqr[0]); - - std::vector pt; - pt.push_back(x); - pt.push_back(y); - if (m_3d) - pt.push_back(z); - m_index->findNeighbors(resultSet, &pt[0], nanoflann::SearchParams(10)); - return output; -} - -} // namespace pdal - diff --git a/test/unit/CMakeLists.txt b/test/unit/CMakeLists.txt index 6bfea17a9c..5803341b65 100644 --- a/test/unit/CMakeLists.txt +++ b/test/unit/CMakeLists.txt @@ -52,6 +52,7 @@ PDAL_ADD_TEST(pdal_config_test FILES ConfigTest.cpp) PDAL_ADD_TEST(pdal_environment_test FILES EnvironmentTest.cpp) PDAL_ADD_TEST(pdal_file_utils_test FILES FileUtilsTest.cpp) PDAL_ADD_TEST(pdal_georeference_test FILES GeoreferenceTest.cpp) +PDAL_ADD_TEST(pdal_kdindex_test FILES KDIndexTest.cpp) PDAL_ADD_TEST(pdal_log_test FILES LogTest.cpp) PDAL_ADD_TEST(pdal_metadata_test FILES MetadataTest.cpp) PDAL_ADD_TEST(pdal_options_test FILES OptionsTest.cpp) diff --git a/test/unit/KDIndexTest.cpp b/test/unit/KDIndexTest.cpp new file mode 100644 index 0000000000..608ca37a38 --- /dev/null +++ b/test/unit/KDIndexTest.cpp @@ -0,0 +1,290 @@ +/****************************************************************************** +* Copyright (c) 2015, Hobu Inc. (info@hobu.co) +* +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following +* conditions are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Hobu, Inc. nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT +* OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY +* OF SUCH DAMAGE. +****************************************************************************/ + +#include + +#include + +using namespace pdal; + +TEST(KDIndex, neighbors2D) +{ + PointTable table; + PointLayoutPtr layout = table.layout(); + PointView view(table); + + layout->registerDim(Dimension::Id::X); + layout->registerDim(Dimension::Id::Y); + + view.setField(Dimension::Id::X, 0, 0); + view.setField(Dimension::Id::Y, 0, 0); + + view.setField(Dimension::Id::X, 1, 1); + view.setField(Dimension::Id::Y, 1, 1); + + view.setField(Dimension::Id::X, 2, 3); + view.setField(Dimension::Id::Y, 2, 3); + + view.setField(Dimension::Id::X, 3, 6); + view.setField(Dimension::Id::Y, 3, 6); + + view.setField(Dimension::Id::X, 4, 10); + view.setField(Dimension::Id::Y, 4, 10); + + KD2Index index(view); + index.build(); + + EXPECT_EQ(index.neighbor(0, 0), 0u); + EXPECT_EQ(index.neighbor(1.1, 1.1), 1u); + EXPECT_EQ(index.neighbor(3.3, 3.3), 2u); + EXPECT_EQ(index.neighbor(6.1, 6.1), 3u); + EXPECT_EQ(index.neighbor(15, 15), 4u); + + std::vector ids; + ids = index.neighbors(0, 0, 5); + EXPECT_EQ(ids.size(), 5u); + EXPECT_EQ(ids[0], 0u); + EXPECT_EQ(ids[1], 1u); + EXPECT_EQ(ids[2], 2u); + EXPECT_EQ(ids[3], 3u); + EXPECT_EQ(ids[4], 4u); + + ids = index.neighbors(0, 0, 25); + EXPECT_EQ(ids.size(), 5u); + EXPECT_EQ(ids[0], 0u); + EXPECT_EQ(ids[1], 1u); + EXPECT_EQ(ids[2], 2u); + EXPECT_EQ(ids[3], 3u); + EXPECT_EQ(ids[4], 4u); + + ids = index.neighbors(3.1, 3.1, 5); + EXPECT_EQ(ids.size(), 5u); + EXPECT_EQ(ids[0], 2u); + EXPECT_EQ(ids[1], 1u); + EXPECT_EQ(ids[2], 3u); + EXPECT_EQ(ids[3], 0u); + EXPECT_EQ(ids[4], 4u); +} + +TEST(KDIndex, neighbors3D) +{ + PointTable table; + PointLayoutPtr layout = table.layout(); + PointView view(table); + + layout->registerDim(Dimension::Id::X); + layout->registerDim(Dimension::Id::Y); + layout->registerDim(Dimension::Id::Z); + + view.setField(Dimension::Id::X, 0, 0); + view.setField(Dimension::Id::Y, 0, 0); + view.setField(Dimension::Id::Z, 0, 0); + + view.setField(Dimension::Id::X, 1, 1); + view.setField(Dimension::Id::Y, 1, 1); + view.setField(Dimension::Id::Z, 1, 1); + + view.setField(Dimension::Id::X, 2, 3); + view.setField(Dimension::Id::Y, 2, 3); + view.setField(Dimension::Id::Z, 2, 3); + + view.setField(Dimension::Id::X, 3, 6); + view.setField(Dimension::Id::Y, 3, 6); + view.setField(Dimension::Id::Z, 3, 6); + + view.setField(Dimension::Id::X, 4, 10); + view.setField(Dimension::Id::Y, 4, 10); + view.setField(Dimension::Id::Z, 4, 10); + + KD3Index index(view); + index.build(); + + EXPECT_EQ(index.neighbor(0, 0, 0), 0u); + EXPECT_EQ(index.neighbor(1.1, 1.1, 1.1), 1u); + EXPECT_EQ(index.neighbor(3.3, 3.3, 3.3), 2u); + EXPECT_EQ(index.neighbor(6.1, 6.1, 6.1), 3u); + EXPECT_EQ(index.neighbor(15, 15, 15), 4u); + + std::vector ids; + ids = index.neighbors(0, 0, 0, 5); + EXPECT_EQ(ids.size(), 5u); + EXPECT_EQ(ids[0], 0u); + EXPECT_EQ(ids[1], 1u); + EXPECT_EQ(ids[2], 2u); + EXPECT_EQ(ids[3], 3u); + EXPECT_EQ(ids[4], 4u); + + ids = index.neighbors(0, 0, 0, 25); + EXPECT_EQ(ids.size(), 5u); + EXPECT_EQ(ids[0], 0u); + EXPECT_EQ(ids[1], 1u); + EXPECT_EQ(ids[2], 2u); + EXPECT_EQ(ids[3], 3u); + EXPECT_EQ(ids[4], 4u); + + ids = index.neighbors(3.1, 3.1, 3.1, 5); + EXPECT_EQ(ids.size(), 5u); + EXPECT_EQ(ids[0], 2u); + EXPECT_EQ(ids[1], 1u); + EXPECT_EQ(ids[2], 3u); + EXPECT_EQ(ids[3], 0u); + EXPECT_EQ(ids[4], 4u); +} + +TEST(KDIndex, neighbordims) +{ + PointTable table; + PointLayoutPtr layout = table.layout(); + PointView view(table); + + layout->registerDim(Dimension::Id::X); + layout->registerDim(Dimension::Id::Z); + + view.setField(Dimension::Id::X, 0, 0); + view.setField(Dimension::Id::Z, 0, 0); + + view.setField(Dimension::Id::X, 1, 1); + view.setField(Dimension::Id::Z, 1, 1); + + EXPECT_THROW(KD2Index index(view), pdal_error); + + PointTable table2; + PointLayoutPtr layout2 = table.layout(); + PointView view2(table2); + + layout->registerDim(Dimension::Id::X); + layout->registerDim(Dimension::Id::Y); + + view.setField(Dimension::Id::X, 0, 0); + view.setField(Dimension::Id::Y, 0, 0); + + view.setField(Dimension::Id::X, 1, 1); + view.setField(Dimension::Id::Y, 1, 1); + + EXPECT_THROW(KD3Index index(view2), pdal_error); +} + +TEST(KDIndex, radius2D) +{ + PointTable table; + PointLayoutPtr layout = table.layout(); + PointView view(table); + + layout->registerDim(Dimension::Id::X); + layout->registerDim(Dimension::Id::Y); + + view.setField(Dimension::Id::X, 0, 0); + view.setField(Dimension::Id::Y, 0, 0); + + view.setField(Dimension::Id::X, 1, 1); + view.setField(Dimension::Id::Y, 1, 1); + + view.setField(Dimension::Id::X, 2, 3); + view.setField(Dimension::Id::Y, 2, 3); + + view.setField(Dimension::Id::X, 3, 6); + view.setField(Dimension::Id::Y, 3, 6); + + view.setField(Dimension::Id::X, 4, 10); + view.setField(Dimension::Id::Y, 4, 10); + + KD2Index index(view); + index.build(); + + std::vector ids; + ids = index.radius(0, 0, 4.25); + + EXPECT_EQ(ids.size(), 3u); + EXPECT_EQ(ids[0], 0u); + EXPECT_EQ(ids[1], 1u); + EXPECT_EQ(ids[2], 2u); + + ids = index.radius(3.1, 3.1, 10); + EXPECT_EQ(ids.size(), 5u); + EXPECT_EQ(ids[0], 2u); + EXPECT_EQ(ids[1], 1u); + EXPECT_EQ(ids[2], 3u); + EXPECT_EQ(ids[3], 0u); + EXPECT_EQ(ids[4], 4u); +} + +TEST(KDIndex, radius3D) +{ + PointTable table; + PointLayoutPtr layout = table.layout(); + PointView view(table); + + layout->registerDim(Dimension::Id::X); + layout->registerDim(Dimension::Id::Y); + layout->registerDim(Dimension::Id::Z); + + view.setField(Dimension::Id::X, 0, 0); + view.setField(Dimension::Id::Y, 0, 0); + view.setField(Dimension::Id::Z, 0, 0); + + view.setField(Dimension::Id::X, 1, 1); + view.setField(Dimension::Id::Y, 1, 1); + view.setField(Dimension::Id::Z, 1, 1); + + view.setField(Dimension::Id::X, 2, 3); + view.setField(Dimension::Id::Y, 2, 3); + view.setField(Dimension::Id::Z, 2, 3); + + view.setField(Dimension::Id::X, 3, 6); + view.setField(Dimension::Id::Y, 3, 6); + view.setField(Dimension::Id::Z, 3, 6); + + view.setField(Dimension::Id::X, 4, 10); + view.setField(Dimension::Id::Y, 4, 10); + view.setField(Dimension::Id::Z, 4, 10); + + KD3Index index(view); + index.build(); + + std::vector ids; + ids = index.radius(0, 0, 0, 5.2); + + EXPECT_EQ(ids.size(), 3u); + EXPECT_EQ(ids[0], 0u); + EXPECT_EQ(ids[1], 1u); + EXPECT_EQ(ids[2], 2u); + + ids = index.radius(3.1, 3.1, 3.1, 12.2); + EXPECT_EQ(ids.size(), 5u); + EXPECT_EQ(ids[0], 2u); + EXPECT_EQ(ids[1], 1u); + EXPECT_EQ(ids[2], 3u); + EXPECT_EQ(ids[3], 0u); + EXPECT_EQ(ids[4], 4u); +} + diff --git a/vendor/nanoflann-1.1.8/nanoflann.hpp b/vendor/nanoflann-1.1.8/nanoflann.hpp index 011042cbef..c651934a2d 100644 --- a/vendor/nanoflann-1.1.8/nanoflann.hpp +++ b/vendor/nanoflann-1.1.8/nanoflann.hpp @@ -78,7 +78,8 @@ namespace nanoflann indices = indices_; dists = dists_; count = 0; - dists[capacity-1] = (std::numeric_limits::max)(); + if (capacity) + dists[capacity-1] = (std::numeric_limits::max)(); } inline CountType size() const @@ -886,7 +887,8 @@ namespace nanoflann init_vind(); computeBoundingBox(root_bbox); freeIndex(); - root_node = divideTree(0, m_size, root_bbox ); // construct the tree + if (size()) + root_node = divideTree(0, m_size, root_bbox); // construct the tree } /** @@ -932,6 +934,8 @@ namespace nanoflann void findNeighbors(RESULTSET& result, const ElementType* vec, const SearchParams& searchParams) const { assert(vec); + if (size() == 0) + return; if (!root_node) throw std::runtime_error("[nanoflann] findNeighbors() called before building the index."); float epsError = 1+searchParams.eps;