New NearestNeighborsCapable interface

doc/source/doxygen-docs/changelog.md

@@ -4,6 +4,11 @@
 - Changes in libraries:
   - \ref mrpt_maps_grp
     - mrpt::maps::COccupancyGridMap3D::insertObservation() now also handles mrpt::obs::CObservationPointCloud
+    - New virtual interface mrpt::maps::NearestNeighborsCapable, implemented in:
+      - All mrpt::maps::CPointsMap classes
+      - All classes derived from mrpt::maps::CVoxelMapOccupancyBase
+      - mrpt::maps::COccupancyGridMap2D
+      - mrpt::maps::COccupancyGridMap2D
 
 # Version 2.11.2: Released Oct 25th, 2023
 - Changes in libraries:

libs/maps/include/mrpt/maps/COccupancyGridMap2D.h

@@ -15,6 +15,7 @@
 #include <mrpt/maps/CLogOddsGridMap2D.h>
 #include <mrpt/maps/CLogOddsGridMapLUT.h>
 #include <mrpt/maps/CMetricMap.h>
+#include <mrpt/maps/NearestNeighborsCapable.h>
 #include <mrpt/maps/OccupancyGridCellType.h>
 #include <mrpt/obs/CObservation2DRangeScanWithUncertainty.h>
 #include <mrpt/obs/obs_frwds.h>
@@ -52,7 +53,8 @@ namespace mrpt::maps
  **/
 class COccupancyGridMap2D
 	: public CMetricMap,
-	  public CLogOddsGridMap2D<OccGridCellTraits::cellType>
+	  public CLogOddsGridMap2D<OccGridCellTraits::cellType>,
+	  public mrpt::maps::NearestNeighborsCapable
 {
 	DEFINE_SERIALIZABLE(COccupancyGridMap2D, mrpt::maps)
    public:
@@ -1161,6 +1163,33 @@ class COccupancyGridMap2D
 			getXMin(), getYMin(), getXMax(), getYMax(), getResolution());
 	}
 
+	/** @name API of the NearestNeighborsCapable virtual interface
+		@{ */
+	// See docs in base class
+	[[nodiscard]] bool nn_single_search(
+		const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result,
+		float& out_dist_sqr) const override;
+	[[nodiscard]] bool nn_single_search(
+		const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result,
+		float& out_dist_sqr) const override;
+	void nn_multiple_search(
+		const mrpt::math::TPoint3Df& query, const size_t N,
+		std::vector<mrpt::math::TPoint3Df>& results,
+		std::vector<float>& out_dists_sqr) const override;
+	void nn_multiple_search(
+		const mrpt::math::TPoint2Df& query, const size_t N,
+		std::vector<mrpt::math::TPoint2Df>& results,
+		std::vector<float>& out_dists_sqr) const override;
+	void nn_radius_search(
+		const mrpt::math::TPoint3Df& query, const float search_radius_sqr,
+		std::vector<mrpt::math::TPoint3Df>& results,
+		std::vector<float>& out_dists_sqr) const override;
+	void nn_radius_search(
+		const mrpt::math::TPoint2Df& query, const float search_radius_sqr,
+		std::vector<mrpt::math::TPoint2Df>& results,
+		std::vector<float>& out_dists_sqr) const override;
+	/** @} */
+
    private:
 	// See docs in base class
 	double internal_computeObservationLikelihood(

libs/maps/include/mrpt/maps/CPointsMap.h

@@ -15,6 +15,7 @@
 #include <mrpt/core/safe_pointers.h>
 #include <mrpt/img/color_maps.h>
 #include <mrpt/maps/CMetricMap.h>
+#include <mrpt/maps/NearestNeighborsCapable.h>
 #include <mrpt/math/CMatrixFixed.h>
 #include <mrpt/math/KDTreeCapable.h>
 #include <mrpt/math/TBoundingBox.h>
@@ -70,7 +71,8 @@ struct pointmap_traits;
 class CPointsMap : public CMetricMap,
 				   public mrpt::math::KDTreeCapable<CPointsMap>,
 				   public mrpt::opengl::PLY_Importer,
-				   public mrpt::opengl::PLY_Exporter
+				   public mrpt::opengl::PLY_Exporter,
+				   public mrpt::maps::NearestNeighborsCapable
 {
 	DEFINE_VIRTUAL_SERIALIZABLE(CPointsMap)
 	// This must be added for declaration of MEX-related functions
@@ -1124,6 +1126,33 @@ class CPointsMap : public CMetricMap,
 			boundingBox().asString().c_str());
 	}
 
+	/** @name API of the NearestNeighborsCapable virtual interface
+		@{ */
+	// See docs in base class
+	[[nodiscard]] bool nn_single_search(
+		const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result,
+		float& out_dist_sqr) const override;
+	[[nodiscard]] bool nn_single_search(
+		const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result,
+		float& out_dist_sqr) const override;
+	void nn_multiple_search(
+		const mrpt::math::TPoint3Df& query, const size_t N,
+		std::vector<mrpt::math::TPoint3Df>& results,
+		std::vector<float>& out_dists_sqr) const override;
+	void nn_multiple_search(
+		const mrpt::math::TPoint2Df& query, const size_t N,
+		std::vector<mrpt::math::TPoint2Df>& results,
+		std::vector<float>& out_dists_sqr) const override;
+	void nn_radius_search(
+		const mrpt::math::TPoint3Df& query, const float search_radius_sqr,
+		std::vector<mrpt::math::TPoint3Df>& results,
+		std::vector<float>& out_dists_sqr) const override;
+	void nn_radius_search(
+		const mrpt::math::TPoint2Df& query, const float search_radius_sqr,
+		std::vector<mrpt::math::TPoint2Df>& results,
+		std::vector<float>& out_dists_sqr) const override;
+	/** @} */
+
    protected:
 	/** The point coordinates */
 	mrpt::aligned_std_vector<float> m_x, m_y, m_z;

libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h

@@ -13,6 +13,7 @@
 #include <mrpt/maps/CLogOddsGridMapLUT.h>
 #include <mrpt/maps/CSimplePointsMap.h>
 #include <mrpt/maps/CVoxelMapBase.h>
+#include <mrpt/maps/NearestNeighborsCapable.h>
 #include <mrpt/maps/OccupancyGridCellType.h>
 #include <mrpt/maps/logoddscell_traits.h>
 #include <mrpt/obs/obs_frwds.h>
@@ -112,7 +113,8 @@ struct has_color<T, std::void_t<decltype(T::color)>> : std::true_type
  */
 template <typename voxel_node_t, typename occupancy_t = int8_t>
 class CVoxelMapOccupancyBase : public CVoxelMapBase<voxel_node_t>,
-							   public detail::logoddscell_traits<occupancy_t>
+							   public detail::logoddscell_traits<occupancy_t>,
+							   public mrpt::maps::NearestNeighborsCapable
 {
    protected:
 	using occupancy_value_t = occupancy_t;
@@ -286,6 +288,57 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase<voxel_node_t>,
 		return get_logodd_lut().p2l(p);
 	}
 
+	/** @name API of the NearestNeighborsCapable virtual interface
+		@{ */
+	// See docs in base class
+	[[nodiscard]] bool nn_single_search(
+		const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result,
+		float& out_dist_sqr) const override
+	{
+		return getOccupiedVoxels()->nn_single_search(
+			query, result, out_dist_sqr);
+	}
+	[[nodiscard]] bool nn_single_search(
+		const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result,
+		float& out_dist_sqr) const override
+	{
+		return getOccupiedVoxels()->nn_single_search(
+			query, result, out_dist_sqr);
+	}
+	void nn_multiple_search(
+		const mrpt::math::TPoint3Df& query, const size_t N,
+		std::vector<mrpt::math::TPoint3Df>& results,
+		std::vector<float>& out_dists_sqr) const override
+	{
+		getOccupiedVoxels()->nn_multiple_search(
+			query, N, results, out_dists_sqr);
+	}
+	void nn_multiple_search(
+		const mrpt::math::TPoint2Df& query, const size_t N,
+		std::vector<mrpt::math::TPoint2Df>& results,
+		std::vector<float>& out_dists_sqr) const override
+	{
+		getOccupiedVoxels()->nn_multiple_search(
+			query, N, results, out_dists_sqr);
+	}
+	void nn_radius_search(
+		const mrpt::math::TPoint3Df& query, const float search_radius_sqr,
+		std::vector<mrpt::math::TPoint3Df>& results,
+		std::vector<float>& out_dists_sqr) const override
+	{
+		getOccupiedVoxels()->nn_radius_search(
+			query, search_radius_sqr, results, out_dists_sqr);
+	}
+	void nn_radius_search(
+		const mrpt::math::TPoint2Df& query, const float search_radius_sqr,
+		std::vector<mrpt::math::TPoint2Df>& results,
+		std::vector<float>& out_dists_sqr) const override
+	{
+		getOccupiedVoxels()->nn_radius_search(
+			query, search_radius_sqr, results, out_dists_sqr);
+	}
+	/** @} */
+
    protected:
 	void internal_clear() override;
 

libs/maps/include/mrpt/maps/NearestNeighborsCapable.h

@@ -0,0 +1,93 @@
+/* +------------------------------------------------------------------------+
+   |                     Mobile Robot Programming Toolkit (MRPT)            |
+   |                          https://www.mrpt.org/                         |
+   |                                                                        |
+   | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file     |
+   | See: https://www.mrpt.org/Authors - All rights reserved.               |
+   | Released under BSD License. See: https://www.mrpt.org/License          |
+   +------------------------------------------------------------------------+ */
+#pragma once
+
+#include <mrpt/math/TPoint2D.h>
+#include <mrpt/math/TPoint3D.h>
+
+namespace mrpt::maps
+{
+/** Virtual interface for maps having the capability of searching the closest
+ * neighbor(s) of a given query 2D or 3D point.
+ *
+ * Note this is more generic than mrpt::math::KDTreeCapable since it does not
+ * assume the use of KD-trees, and it is also non templatized, so users can use
+ * dynamic casting to interact with maps in a generic way.
+ *
+ * \note New in MRPT 2.11.3
+ * \ingroup mrpt_maps_grp
+ */
+class NearestNeighborsCapable
+{
+   public:
+	NearestNeighborsCapable() = default;
+	virtual ~NearestNeighborsCapable() = default;
+
+	/** @name API of the NearestNeighborsCapable virtual interface
+		@{ */
+
+	/** Search for the closest 3D point to a given one.
+	 *
+	 * \param[in]  query The query input point.
+	 * \param[out] result The found closest point.
+	 * \param[out] out_dist_sqr The square Euclidean distance between the query
+	 * and the returned point.
+	 *
+	 * \return True if successful, false if no point was found.
+	 */
+	[[nodiscard]] virtual bool nn_single_search(
+		const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result,
+		float& out_dist_sqr) const = 0;
+
+	/// \overload for 2D points
+	[[nodiscard]] virtual bool nn_single_search(
+		const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result,
+		float& out_dist_sqr) const = 0;
+
+	/** Search for the `N` closest 3D points to a given one.
+	 *
+	 * \param[in]  query The query input point.
+	 * \param[out] results The found closest points.
+	 * \param[out] out_dists_sqr The square Euclidean distances between the
+	 * query and the returned point.
+	 */
+	virtual void nn_multiple_search(
+		const mrpt::math::TPoint3Df& query, const size_t N,
+		std::vector<mrpt::math::TPoint3Df>& results,
+		std::vector<float>& out_dists_sqr) const = 0;
+
+	/// \overload for 2D points
+	virtual void nn_multiple_search(
+		const mrpt::math::TPoint2Df& query, const size_t N,
+		std::vector<mrpt::math::TPoint2Df>& results,
+		std::vector<float>& out_dists_sqr) const = 0;
+
+	/** Radius search for closest 3D points to a given one.
+	 *
+	 * \param[in]  query The query input point.
+	 * \param[in]  search_radius_sqr The search radius, **squared**.
+	 * \param[out] results The found closest points.
+	 * \param[out] out_dists_sqr The square Euclidean distances between the
+	 * query and the returned point.
+	 */
+	virtual void nn_radius_search(
+		const mrpt::math::TPoint3Df& query, const float search_radius_sqr,
+		std::vector<mrpt::math::TPoint3Df>& results,
+		std::vector<float>& out_dists_sqr) const = 0;
+
+	/// \overload for 2D points
+	virtual void nn_radius_search(
+		const mrpt::math::TPoint2Df& query, const float search_radius_sqr,
+		std::vector<mrpt::math::TPoint2Df>& results,
+		std::vector<float>& out_dists_sqr) const = 0;
+
+	/** @} */
+};
+
+}  // namespace mrpt::maps

libs/maps/src/maps/COccupancyGridMap2D_common.cpp

@@ -639,8 +639,7 @@ void COccupancyGridMap2D::determineMatching2D(
 			for (int cy = cy_min; cy <= cy_max; cy++)
 			{
 				// Is an occupied cell?
-				if (m_map[cx + cy * m_size_x] <
-					thresholdCellValue)	 //  getCell(cx,cy)<0.49)
+				if (m_map[cx + cy * m_size_x] < thresholdCellValue)
 				{
 					const float residual_x = idx2x(cx) - x_local;
 					const float residual_y = idx2y(cy) - y_local;
@@ -768,3 +767,65 @@ float COccupancyGridMap2D::compute3DMatchingRatio(
 {
 	return 0;
 }
+
+bool COccupancyGridMap2D::nn_single_search(
+	const mrpt::math::TPoint3Df& query, mrpt::math::TPoint3Df& result,
+	float& out_dist_sqr) const
+{
+	// delegate to the 2D version:
+	mrpt::math::TPoint2Df r;
+	bool res = nn_single_search({query.x, query.y}, r, out_dist_sqr);
+	result = {r.x, r.y, .0f};
+	return res;
+}
+void COccupancyGridMap2D::nn_multiple_search(
+	const mrpt::math::TPoint3Df& query, const size_t N,
+	std::vector<mrpt::math::TPoint3Df>& results,
+	std::vector<float>& out_dists_sqr) const
+{
+	// delegate to the 2D version:
+	std::vector<mrpt::math::TPoint2Df> r;
+	nn_multiple_search({query.x, query.y}, N, r, out_dists_sqr);
+	results.resize(r.size());
+	for (size_t i = 0; i < r.size(); i++)
+		results[i] = {r[i].x, r[i].y, .0f};
+}
+void COccupancyGridMap2D::nn_radius_search(
+	const mrpt::math::TPoint3Df& query, const float search_radius_sqr,
+	std::vector<mrpt::math::TPoint3Df>& results,
+	std::vector<float>& out_dists_sqr) const
+{
+	// delegate to the 2D version:
+	std::vector<mrpt::math::TPoint2Df> r;
+	nn_radius_search({query.x, query.y}, search_radius_sqr, r, out_dists_sqr);
+	results.resize(r.size());
+	for (size_t i = 0; i < r.size(); i++)
+		results[i] = {r[i].x, r[i].y, .0f};
+}
+
+bool COccupancyGridMap2D::nn_single_search(
+	const mrpt::math::TPoint2Df& query, mrpt::math::TPoint2Df& result,
+	float& out_dist_sqr) const
+{
+	std::vector<mrpt::math::TPoint2Df> r;
+	std::vector<float> dist_sqr;
+	nn_multiple_search(query, 1, r, dist_sqr);
+	if (r.empty()) return false;  // none found
+	result = r[0];
+	out_dist_sqr = dist_sqr[0];
+	return true;
+}
+
+void COccupancyGridMap2D::nn_multiple_search(
+	const mrpt::math::TPoint2Df& query, const size_t N,
+	std::vector<mrpt::math::TPoint2Df>& results,
+	std::vector<float>& out_dists_sqr) const
+{
+}
+
+void COccupancyGridMap2D::nn_radius_search(
+	const mrpt::math::TPoint2Df& query, const float search_radius_sqr,
+	std::vector<mrpt::math::TPoint2Df>& results,
+	std::vector<float>& out_dists_sqr) const
+{
+}

libs/maps/src/maps/COccupancyGridMap2D_unittest.cpp

@@ -37,6 +37,20 @@ TEST(COccupancyGridMap2DTests, insert2DScan)
 	}
 }
 
+TEST(COccupancyGridMap2DTests, NearestNeighborsCapable)
+{
+	// low freeness=occupied
+	const float occupied = 0.2f;
+	const std::vector<mrpt::math::TPoint2Df> occupiedPoints = {
+		{1.0f, 2.0f}, {1.1f, 2.0f}, {1.2f, 2.0f}, {2.0f, 3.0f}};
+
+	COccupancyGridMap2D grid(-10.0f, 10.0f, -10.0f, 10.0f, 0.10f);
+	for (const auto& pt : occupiedPoints)
+		grid.setCell(grid.x2idx(pt.x), grid.x2idx(pt.y), occupied);
+
+	MRPT_TODO("continue");
+}
+
 // We need OPENCV to read the image.
 #if MRPT_HAS_OPENCV && MRPT_HAS_FYAML
 
@@ -56,4 +70,4 @@ TEST(COccupancyGridMap2DTests, loadFromROSMapServerYAML)
 	ASSERT_EQUAL_(grid.getPos(2.0, 2.0), 0.5f);
 }
 
-#endif
+#endif