Merge pull request #1294 from MRPT/feature/new-nn-api

WIP: New NearestNeighborsCapable interface

doc/source/doxygen-docs/changelog.md

@@ -4,6 +4,14 @@
 - 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
+    - New virtual method mrpt::maps::CMetricMap::boundingBox()
+  - \ref mrpt_core_grp
+    - Add the `[[nodiscard]]` attribute to all functions returning a value in `<mrpt/core/bits_math.h>`
 
 # Version 2.11.2: Released Oct 25th, 2023
 - Changes in libraries:

libs/core/include/mrpt/core/bits_math.h

@@ -94,21 +94,21 @@ using namespace mrpt::literals;	 // for backwards compatib.
 
 /** Returns the sign of X as "1" or "-1" */
 template <typename T>
-inline int sign(T x)
+[[nodiscard]] inline int sign(T x)
 {
 	return x < 0 ? -1 : 1;
 }
 
 /** Returns the sign of X as "0", "1" or "-1" */
 template <typename T>
-inline int signWithZero(T x)
+[[nodiscard]] inline int signWithZero(T x)
 {
 	return (x == 0 || x == -0) ? 0 : sign(x);
 }
 
 /** Returns the smallest positive number among a and b */
 template <typename T>
-T lowestPositive(const T a, const T b)
+[[nodiscard]] T lowestPositive(const T a, const T b)
 {
 	if (a > 0 && a <= b) return a;	// a positive and smaller than b
 	else if (b > 0)
@@ -118,19 +118,19 @@ T lowestPositive(const T a, const T b)
 }
 
 template <typename T>
-inline const T min3(const T& A, const T& B, const T& C)
+[[nodiscard]] inline const T min3(const T& A, const T& B, const T& C)
 {
 	return std::min<T>(A, std::min<T>(B, C));
 }
 template <typename T>
-inline const T max3(const T& A, const T& B, const T& C)
+[[nodiscard]] inline const T max3(const T& A, const T& B, const T& C)
 {
 	return std::max<T>(A, std::max<T>(B, C));
 }
 
 /** Rounds toward zero  */
 template <typename T>
-inline int fix(T x)
+[[nodiscard]] inline int fix(T x)
 {
 	return x > 0 ? static_cast<int>(floor(static_cast<double>(x)))
 				 : static_cast<int>(ceil(static_cast<double>(x)));
@@ -161,7 +161,8 @@ inline void saturate(T& var, const T sat_min, const T sat_max)
 /** Like saturate() but it returns the value instead of modifying the variable
  */
 template <typename T>
-inline T saturate_val(const T& value, const T sat_min, const T sat_max)
+[[nodiscard]] inline T saturate_val(
+	const T& value, const T sat_min, const T sat_max)
 {
 	T var = value;
 	if (var > sat_max) var = sat_max;
@@ -171,7 +172,7 @@ inline T saturate_val(const T& value, const T sat_min, const T sat_max)
 
 /** Round up to the nearest power of two of a given number */
 template <class T>
-T round2up(T val)
+[[nodiscard]] T round2up(T val)
 {
 	T n = 1;
 	while (n < val)
@@ -183,14 +184,17 @@ T round2up(T val)
 }
 
 /** shortcut for static_cast<float>(double) */
-inline float d2f(const double d) { return static_cast<float>(d); }
+[[nodiscard]] inline float d2f(const double d) { return static_cast<float>(d); }
 
 /** converts a float [0,1] into an uint8_t [0,255] (without checking for out of
  * bounds) \sa u8tof */
-inline uint8_t f2u8(const float f) { return static_cast<uint8_t>(f * 255); }
+[[nodiscard]] inline uint8_t f2u8(const float f)
+{
+	return static_cast<uint8_t>(f * 255);
+}
 
 /** converts a uint8_t [0,255] into a float [0,1] \sa f2u8 */
-inline float u8tof(const uint8_t v) { return v / 255.0f; }
+[[nodiscard]] inline float u8tof(const uint8_t v) { return v / 255.0f; }
 
 /** @} */
 

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:
@@ -326,6 +328,13 @@ class COccupancyGridMap2D
 		return static_cast<int>((y - ymin) / m_resolution);
 	}
 
+	mrpt::math::TBoundingBoxf boundingBox() const override
+	{
+		return {
+			{m_xMin, m_yMin, insertionOptions.mapAltitude},
+			{m_xMax, m_yMax, insertionOptions.mapAltitude}};
+	}
+
 	/** Scales an integer representation of the log-odd into a real valued
 	 * probability in [0,1], using p=exp(l)/(1+exp(l))  */
 	static inline float l2p(const cellType l)
@@ -1161,6 +1170,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/COccupancyGridMap3D.h

@@ -12,6 +12,7 @@
 #include <mrpt/maps/CLogOddsGridMap3D.h>
 #include <mrpt/maps/CLogOddsGridMapLUT.h>
 #include <mrpt/maps/CMetricMap.h>
+#include <mrpt/maps/NearestNeighborsCapable.h>
 #include <mrpt/maps/OccupancyGridCellType.h>
 #include <mrpt/opengl/opengl_frwds.h>
 #include <mrpt/serialization/CSerializable.h>
@@ -40,7 +41,8 @@ namespace mrpt::maps
  **/
 class COccupancyGridMap3D
 	: public CMetricMap,
-	  public CLogOddsGridMap3D<OccGridCellTraits::cellType>
+	  public CLogOddsGridMap3D<OccGridCellTraits::cellType>,
+	  public mrpt::maps::NearestNeighborsCapable
 {
 	DEFINE_SERIALIZABLE(COccupancyGridMap3D, mrpt::maps)
    public:
@@ -204,6 +206,15 @@ class COccupancyGridMap3D
 	void getVisualizationInto(
 		mrpt::opengl::CSetOfObjects& outObj) const override;
 
+	mrpt::math::TBoundingBoxf boundingBox() const override
+	{
+		return {
+			mrpt::math::TPoint3Df(
+				m_grid.getXMin(), m_grid.getYMin(), m_grid.getZMin()),
+			mrpt::math::TPoint3Df(
+				m_grid.getXMax(), m_grid.getYMax(), m_grid.getZMax())};
+	}
+
 	/** With this struct options are provided to the observation insertion
 	 * process.
 	 * \sa CObservation::insertIntoGridMap */
@@ -387,6 +398,33 @@ class COccupancyGridMap3D
 			m_grid.getResolutionXY(), m_grid.getResolutionZ());
 	}
 
+	/** @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
@@ -929,7 +931,7 @@ class CPointsMap : public CMetricMap,
 	 *  Results are cached unless the map is somehow modified to avoid repeated
 	 * calculations.
 	 */
-	mrpt::math::TBoundingBoxf boundingBox() const;
+	mrpt::math::TBoundingBoxf boundingBox() const override;
 
 	/** Extracts the points in the map within a cylinder in 3D defined the
 	 * provided radius and zmin/zmax values.
@@ -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;