Merge pull request #1296 from MRPT/feature/upgrade-nanoflann

Upgrade to nanoflann 1.5.1
MRPT · Nov 28, 2023 · 7149358 · 7149358
2 parents ca3473d + e07ab2d
commit 7149358
Show file tree

Hide file tree

Showing 6 changed files with 158 additions and 85 deletions.
diff --git a/3rdparty/nanoflann b/3rdparty/nanoflann
diff --git a/3rdparty/nanogui b/3rdparty/nanogui
diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md
@@ -1,7 +1,14 @@
 \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.
+- BUG FIXES:
+  - Fix missing Threads::Threads downstream due to missing `find_dependency(Threads)` in MRPT cmake config files.
+
 
 # Version 2.11.3: Released Nov 21st, 2023
 - Changes in libraries:

diff --git a/libs/core/CMakeLists.txt b/libs/core/CMakeLists.txt
@@ -5,6 +5,7 @@ define_mrpt_lib(
 	# Lib name
 	core
 	# Dependencies
+	Threads  # invoke find_dependency(Threads) in "*-config.cmake"
 	)
 
 if(BUILD_mrpt-core)

diff --git a/libs/maps/src/maps/CPointsMap.cpp b/libs/maps/src/maps/CPointsMap.cpp
@@ -857,41 +857,7 @@ void CPointsMap::getAllPoints(
 float CPointsMap::squareDistanceToClosestCorrespondence(
 	float x0, float y0) const
 {
-	// Just the closest point:
-
-#if 1
 	return kdTreeClosestPoint2DsqrError(x0, y0);
-#else
-	// The distance to the line that interpolates the TWO closest points:
-	float x1, y1, x2, y2, d1, d2;
-	kdTreeTwoClosestPoint2D(
-		x0, y0,	 // The query
-		x1, y1,	 // Closest point #1
-		x2, y2,	 // Closest point #2
-		d1, d2);
-
-	ASSERT_(d2 >= d1);
-
-	// If the two points are too far, do not interpolate:
-	float d12 = square(x1 - x2) + square(y1 - y2);
-	if (d12 > 0.20f * 0.20f || d12 < 0.03f * 0.03f)
-	{
-		return square(x1 - x0) + square(y1 - y0);
-	}
-	else
-	{  // Interpolate
-		double interp_x, interp_y;
-
-		// math::closestFromPointToSegment(
-		math::closestFromPointToLine(
-			x0, y0,	 // the point
-			x1, y1, x2, y2,	 // The segment
-			interp_x, interp_y	// out
-		);
-
-		return square(interp_x - x0) + square(interp_y - y0);
-	}
-#endif
 }
 
 mrpt::math::TBoundingBoxf CPointsMap::boundingBox() const
@@ -2185,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];
 		}
@@ -2230,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];
 		}
+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