Skip to content

Commit

Permalink
Use nanoflann RKNN
Browse files Browse the repository at this point in the history
  • Loading branch information
@jlblancoc
jlblancoc committed Nov 28, 2023
1 parent 443985c commit 3461796
Show file tree
Hide file tree
Showing 4 changed files with 154 additions and 50 deletions.
2 changes: 1 addition & 1 deletion 3rdparty/nanoflann
Submodule nanoflann updated 33 files
+81 −0 .circleci/check_style.sh
+83 −0 .circleci/clang_format_codebase.sh
+40 −0 .circleci/clang_git_format/.gitignore
+202 −0 .circleci/clang_git_format/LICENSE
+116 −0 .circleci/clang_git_format/README.md
+10 −0 .circleci/clang_git_format/clang_git_format/__init__.py
+159 −0 .circleci/clang_git_format/clang_git_format/clang_format.py
+2 −0 .circleci/clang_git_format/clang_git_format/config.py
+38 −0 .circleci/clang_git_format/clang_git_format/custom_exceptions.py
+343 −0 .circleci/clang_git_format/clang_git_format/repo.py
+80 −0 .circleci/clang_git_format/clang_git_format/utils.py
+578 −0 .circleci/clang_git_format/format_code.py
+25 −0 .circleci/clang_git_format/scripts/convert_inline_dox_comments_file.sh
+72 −9 .circleci/config.yml
+3 −0 .circleci/python_reqs.txt
+40 −0 .github/workflows/check-clang-format.yml
+85 −0 .github/workflows/ci-linux.yml
+17 −1 CHANGELOG.md
+10 −1 CMakeLists.txt
+18 −1 README.md
+1 −1 appveyor.yml
+1 −0 examples/CMakeLists.txt
+6 −4 examples/KDTreeVectorOfVectorsAdaptor.h
+16 −4 examples/examples_gui/nanoflann_gui_example_R3/CMakeLists.txt
+57 −5 examples/examples_gui/nanoflann_gui_example_R3/nanoflann_gui_example_R3.cpp
+4 −4 examples/examples_gui/nanoflann_gui_example_bearings/nanoflann_gui_example_bearings.cpp
+2 −2 examples/matrix_example.cpp
+2 −2 examples/pointcloud_custom_metric.cpp
+125 −0 examples/pointcloud_custom_resultset.cpp
+10 −0 examples/saveload_example.cpp
+1 −1 examples/vector_of_vectors_example.cpp
+703 −418 include/nanoflann.hpp
+252 −20 tests/test_main.cpp
7 changes: 6 additions & 1 deletion doc/source/doxygen-docs/changelog.md
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,12 @@
\page changelog Change Log

# Version 2.11.4: UNRELEASED
(None yet)
- Changes in libraries:
- \ref mrpt_maps_grp
- Use nanoflann RKNN search in mrpt::maps::CPointsMap::nn_radius_search()
- \ref mrpt_math_grp
- mrpt::math::KDTreeCapable: Add optional argument maximumSearchDistanceSqr in many API methods to exploit the new nanoflann RKNN search method.


# Version 2.11.3: Released Nov 21st, 2023
- Changes in libraries:
Expand Down
20 changes: 4 additions & 16 deletions libs/maps/src/maps/CPointsMap.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2151,18 +2151,12 @@ void CPointsMap::nn_radius_search(
{
std::vector<size_t> idxs;
kdTreeNClosestPoint3DIdx(
query.x, query.y, query.z, maxPoints, idxs, out_dists_sqr);
query.x, query.y, query.z, maxPoints, idxs, out_dists_sqr,
search_radius_sqr);
results.resize(idxs.size());
resultIndicesOrIDs.resize(idxs.size());
for (size_t i = 0; i < idxs.size(); i++)
{
if (out_dists_sqr[i] > search_radius_sqr) // truncate list?
{
results.resize(i);
out_dists_sqr.resize(i);
resultIndicesOrIDs.resize(i);
break;
}
getPointFast(idxs[i], results[i].x, results[i].y, results[i].z);
resultIndicesOrIDs[i] = idxs[i];
}
Expand Down Expand Up @@ -2196,19 +2190,13 @@ void CPointsMap::nn_radius_search(
{
std::vector<size_t> idxs;
kdTreeNClosestPoint2DIdx(
query.x, query.y, maxPoints, idxs, out_dists_sqr);
query.x, query.y, maxPoints, idxs, out_dists_sqr,
search_radius_sqr);
results.resize(idxs.size());
resultIndicesOrIDs.resize(idxs.size());
float dummyZ = 0;
for (size_t i = 0; i < idxs.size(); i++)
{
if (out_dists_sqr[i] > search_radius_sqr) // truncate list?
{
results.resize(i);
out_dists_sqr.resize(i);
resultIndicesOrIDs.resize(i);
break;
}
getPointFast(idxs[i], results[i].x, results[i].y, dummyZ);
resultIndicesOrIDs[i] = idxs[i];
}
Expand Down
175 changes: 143 additions & 32 deletions libs/math/include/mrpt/math/KDTreeCapable.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <memory> // unique_ptr
#include <mutex>
#include <nanoflann.hpp>
#include <optional>

// Smooth transition to nanoflann>=1.5.0 for older versions:
namespace nanoflann
Expand Down Expand Up @@ -291,14 +292,18 @@ class KDTreeCapable
*correspondences.
* \param out_dist_sqr The vector containing the square distance between
*the query and the returned points.
* \param maximumSearchDistanceSqr If provided, only NN up to that given
*squared distance will be returned.
*
* \return The list of indices
* \sa kdTreeClosestPoint2D
* \sa kdTreeTwoClosestPoint2D
*/
inline std::vector<size_t> kdTreeNClosestPoint2D(
float x0, float y0, size_t knn, std::vector<float>& out_x,
std::vector<float>& out_y, std::vector<float>& out_dist_sqr) const
std::vector<float>& out_y, std::vector<float>& out_dist_sqr,
const std::optional<float>& maximumSearchDistanceSqr =
std::nullopt) const
{
MRPT_START
rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
Expand All @@ -310,11 +315,29 @@ class KDTreeCapable
out_y.resize(knn);
out_dist_sqr.resize(knn);

nanoflann::KNNResultSet<num_t> resultSet(knn);
resultSet.init(&ret_indexes[0], &out_dist_sqr[0]);

const std::array<num_t, 2> query_point{{x0, y0}};
m_kdtree2d_data.index->findNeighbors(resultSet, &query_point[0], {});

if (!maximumSearchDistanceSqr.has_value())
{
nanoflann::KNNResultSet<num_t> resultSet(knn);
resultSet.init(&ret_indexes[0], &out_dist_sqr[0]);

m_kdtree2d_data.index->findNeighbors(
resultSet, &query_point[0], {});
}
else
{
#if NANOFLANN_VERSION >= 0x151
nanoflann::RKNNResultSet<num_t> resultSet(
knn, *maximumSearchDistanceSqr);
resultSet.init(&ret_indexes[0], &out_dist_sqr[0]);

m_kdtree2d_data.index->findNeighbors(
resultSet, &query_point[0], {});
#else
THROW_EXCEPTION("RKNN search requires nanoflann>=1.5.1");
#endif
}

for (size_t i = 0; i < knn; i++)
{
Expand All @@ -327,11 +350,14 @@ class KDTreeCapable

inline std::vector<size_t> kdTreeNClosestPoint2D(
const TPoint2D& p0, size_t N, std::vector<TPoint2D>& pOut,
std::vector<float>& outDistSqr) const
std::vector<float>& outDistSqr,
const std::optional<float>& maximumSearchDistanceSqr =
std::nullopt) const
{
std::vector<float> dmy1, dmy2;
std::vector<size_t> res = kdTreeNClosestPoint2D(
d2f(p0.x), d2f(p0.y), N, dmy1, dmy2, outDistSqr);
d2f(p0.x), d2f(p0.y), N, dmy1, dmy2, outDistSqr,
maximumSearchDistanceSqr);
pOut.resize(dmy1.size());
for (size_t i = 0; i < dmy1.size(); i++)
{
Expand All @@ -354,12 +380,16 @@ class KDTreeCapable
* \param out_idx The indexes of the found closest correspondence.
* \param out_dist_sqr The square distance between the query and the
*returned point.
* \param maximumSearchDistanceSqr If provided, only NN up to that given
*squared distance will be returned.
*
* \sa kdTreeClosestPoint2D
*/
inline void kdTreeNClosestPoint2DIdx(
float x0, float y0, size_t knn, std::vector<size_t>& out_idx,
std::vector<float>& out_dist_sqr) const
std::vector<float>& out_dist_sqr,
const std::optional<float>& maximumSearchDistanceSqr =
std::nullopt) const
{
MRPT_START
rebuild_kdTree_2D(); // First: Create the 2D KD-Tree if required
Expand All @@ -368,11 +398,30 @@ class KDTreeCapable

out_idx.resize(knn);
out_dist_sqr.resize(knn);
nanoflann::KNNResultSet<num_t> resultSet(knn);
resultSet.init(&out_idx[0], &out_dist_sqr[0]);

const std::array<num_t, 2> query_point{{x0, y0}};
m_kdtree2d_data.index->findNeighbors(resultSet, &query_point[0], {});

if (!maximumSearchDistanceSqr.has_value())
{
nanoflann::KNNResultSet<num_t> resultSet(knn);
resultSet.init(&out_idx[0], &out_dist_sqr[0]);

m_kdtree2d_data.index->findNeighbors(
resultSet, &query_point[0], {});
}
else
{
#if NANOFLANN_VERSION >= 0x151
nanoflann::RKNNResultSet<num_t> resultSet(
knn, *maximumSearchDistanceSqr);
resultSet.init(&out_idx[0], &out_dist_sqr[0]);

m_kdtree2d_data.index->findNeighbors(
resultSet, &query_point[0], {});
#else
THROW_EXCEPTION("RKNN search requires nanoflann>=1.5.1");
#endif
}

MRPT_END
}

Expand Down Expand Up @@ -482,13 +531,17 @@ class KDTreeCapable
*correspondences.
* \param out_dist_sqr The vector containing the square distance between
*the query and the returned points.
* \param maximumSearchDistanceSqr If provided, only NN up to that given
*squared distance will be returned.
*
* \sa kdTreeNClosestPoint2D
*/
inline void kdTreeNClosestPoint3D(
float x0, float y0, float z0, size_t knn, std::vector<float>& out_x,
std::vector<float>& out_y, std::vector<float>& out_z,
std::vector<float>& out_dist_sqr) const
std::vector<float>& out_dist_sqr,
const std::optional<float>& maximumSearchDistanceSqr =
std::nullopt) const
{
MRPT_START
rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
Expand All @@ -500,12 +553,27 @@ class KDTreeCapable
out_y.resize(knn);
out_z.resize(knn);
out_dist_sqr.resize(knn);

nanoflann::KNNResultSet<num_t> resultSet(knn);
resultSet.init(&ret_indexes[0], &out_dist_sqr[0]);

const std::array<num_t, 3> query_point{{x0, y0, z0}};
m_kdtree3d_data.index->findNeighbors(resultSet, &query_point[0], {});

if (!maximumSearchDistanceSqr.has_value())
{
nanoflann::KNNResultSet<num_t> resultSet(knn);
resultSet.init(&ret_indexes[0], &out_dist_sqr[0]);
m_kdtree3d_data.index->findNeighbors(
resultSet, &query_point[0], {});
}
else
{
#if NANOFLANN_VERSION >= 0x151
nanoflann::RKNNResultSet<num_t> resultSet(
knn, *maximumSearchDistanceSqr);
resultSet.init(&ret_indexes[0], &out_dist_sqr[0]);
m_kdtree3d_data.index->findNeighbors(
resultSet, &query_point[0], {});
#else
THROW_EXCEPTION("RKNN search requires nanoflann>=1.5.1");
#endif
}

for (size_t i = 0; i < knn; i++)
{
Expand Down Expand Up @@ -542,7 +610,9 @@ class KDTreeCapable
inline void kdTreeNClosestPoint3DWithIdx(
float x0, float y0, float z0, size_t knn, std::vector<float>& out_x,
std::vector<float>& out_y, std::vector<float>& out_z,
std::vector<size_t>& out_idx, std::vector<float>& out_dist_sqr) const
std::vector<size_t>& out_idx, std::vector<float>& out_dist_sqr,
const std::optional<float>& maximumSearchDistanceSqr =
std::nullopt) const
{
MRPT_START
rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
Expand All @@ -554,12 +624,28 @@ class KDTreeCapable
out_z.resize(knn);
out_idx.resize(knn);
out_dist_sqr.resize(knn);
const std::array<num_t, 3> query_point{{x0, y0, z0}};

nanoflann::KNNResultSet<num_t> resultSet(knn);
resultSet.init(&out_idx[0], &out_dist_sqr[0]);
if (!maximumSearchDistanceSqr.has_value())
{
nanoflann::KNNResultSet<num_t> resultSet(knn);
resultSet.init(&out_idx[0], &out_dist_sqr[0]);

const std::array<num_t, 3> query_point{{x0, y0, z0}};
m_kdtree3d_data.index->findNeighbors(resultSet, &query_point[0], {});
m_kdtree3d_data.index->findNeighbors(
resultSet, &query_point[0], {});
}
else
{
#if NANOFLANN_VERSION >= 0x151
nanoflann::RKNNResultSet<num_t> resultSet(
knn, *maximumSearchDistanceSqr);
resultSet.init(&out_idx[0], &out_dist_sqr[0]);
m_kdtree3d_data.index->findNeighbors(
resultSet, &query_point[0], {});
#else
THROW_EXCEPTION("RKNN search requires nanoflann>=1.5.1");
#endif
}

for (size_t i = 0; i < knn; i++)
{
Expand All @@ -572,11 +658,14 @@ class KDTreeCapable

inline void kdTreeNClosestPoint3D(
const TPoint3D& p0, size_t N, std::vector<TPoint3D>& pOut,
std::vector<float>& outDistSqr) const
std::vector<float>& outDistSqr,
const std::optional<float>& maximumSearchDistanceSqr =
std::nullopt) const
{
std::vector<float> dmy1, dmy2, dmy3;
kdTreeNClosestPoint3D(
d2f(p0.x), d2f(p0.y), d2f(p0.z), N, dmy1, dmy2, dmy3, outDistSqr);
d2f(p0.x), d2f(p0.y), d2f(p0.z), N, dmy1, dmy2, dmy3, outDistSqr,
maximumSearchDistanceSqr);
pOut.resize(dmy1.size());
for (size_t i = 0; i < dmy1.size(); i++)
{
Expand Down Expand Up @@ -675,7 +764,9 @@ class KDTreeCapable
*/
inline void kdTreeNClosestPoint3DIdx(
float x0, float y0, float z0, size_t knn, std::vector<size_t>& out_idx,
std::vector<float>& out_dist_sqr) const
std::vector<float>& out_dist_sqr,
const std::optional<float>& maximumSearchDistanceSqr =
std::nullopt) const
{
MRPT_START
rebuild_kdTree_3D(); // First: Create the 3D KD-Tree if required
Expand All @@ -684,20 +775,40 @@ class KDTreeCapable

out_idx.resize(knn);
out_dist_sqr.resize(knn);
nanoflann::KNNResultSet<num_t> resultSet(knn);
resultSet.init(&out_idx[0], &out_dist_sqr[0]);

const std::array<num_t, 3> query_point{{x0, y0, z0}};
m_kdtree3d_data.index->findNeighbors(resultSet, &query_point[0], {});

if (!maximumSearchDistanceSqr.has_value())
{
nanoflann::KNNResultSet<num_t> resultSet(knn);
resultSet.init(&out_idx[0], &out_dist_sqr[0]);
m_kdtree3d_data.index->findNeighbors(
resultSet, &query_point[0], {});
}
else
{
#if NANOFLANN_VERSION >= 0x151
nanoflann::RKNNResultSet<num_t> resultSet(
knn, *maximumSearchDistanceSqr);
resultSet.init(&out_idx[0], &out_dist_sqr[0]);
m_kdtree3d_data.index->findNeighbors(
resultSet, &query_point[0], {});
#else
THROW_EXCEPTION("RKNN search requires nanoflann>=1.5.1");
#endif
}

MRPT_END
}

inline void kdTreeNClosestPoint3DIdx(
const TPoint3D& p0, size_t N, std::vector<size_t>& outIdx,
std::vector<float>& outDistSqr) const
std::vector<float>& outDistSqr,
const std::optional<float>& maximumSearchDistanceSqr =
std::nullopt) const
{
kdTreeNClosestPoint3DIdx(
d2f(p0.x), d2f(p0.y), d2f(p0.z), N, outIdx, outDistSqr);
d2f(p0.x), d2f(p0.y), d2f(p0.z), N, outIdx, outDistSqr,
maximumSearchDistanceSqr);
}

inline void kdTreeEnsureIndexBuilt3D() { rebuild_kdTree_3D(); }
Expand Down

0 comments on commit 3461796

Please sign in to comment.