WIP: Add new Voxelmap classes

doc/source/doxygen-docs/changelog.md

@@ -1,6 +1,11 @@
 \page changelog Change Log
 
-# Version 2.10.3: UNRELEASED
+# Version 2.11.0: UNRELEASED
+- Changes in libraries:
+  - \ref mrpt_maps_grp
+    - New voxel map containers, based on Faconti's [Bonxai](https://github.com/facontidavide/Bonxai) header-only libray (MPL-2.0 license):
+      - mrpt::maps::CVoxelMap
+      - mrpt::maps::CVoxelMapRGB
 - BUG FIXES:
   - Fix python wrapper FTBFS in armhf and other architectures.
   - Fix matrices removeColumns() and removeRows() won't throw if user specified a non-existing index.

doc/source/doxygen-docs/example-maps_voxelmap_simple.md

@@ -0,0 +1,5 @@
+\page maps_octomap_simple Example: maps_octomap_simple
+
+![maps_voxelmap_simple screenshot](doc/source/images/maps_voxelmap_simple_screenshot.png)
+C++ example source code:
+\include maps_voxelmap_simple/test.cpp

doc/source/examples.rst

@@ -78,6 +78,7 @@ Python examples are `here <python_examples.html>`_.
   page_maps_observer_pattern_example.rst
   page_maps_octomap_simple.rst
   page_maps_ransac_data_association.rst
+  page_maps_voxelmap_simple.rst
   page_math_csparse_example.rst
   page_math_kmeans_example.rst
   page_math_leastsquares_example.rst

libs/containers/include/mrpt/containers/NonCopiableData.h

@@ -31,10 +31,10 @@ class NonCopiableData
 	T data;
 
 	NonCopiableData(const NonCopiableData&) {}
-	NonCopiableData& operator=(const NonCopiableData& o) { return *this; }
+	NonCopiableData& operator=(const NonCopiableData&) { return *this; }
 
 	NonCopiableData(NonCopiableData&&) {}
-	NonCopiableData& operator=(NonCopiableData&& o) { return *this; }
+	NonCopiableData& operator=(NonCopiableData&&) { return *this; }
 };
 
 }  // namespace mrpt::containers

libs/maps/include/mrpt/maps.h

@@ -32,6 +32,8 @@ MRPT_WARNING(
 #include <mrpt/maps/CRandomFieldGridMap3D.h>
 #include <mrpt/maps/CReflectivityGridMap2D.h>
 #include <mrpt/maps/CSimplePointsMap.h>
+#include <mrpt/maps/CVoxelMap.h>
+#include <mrpt/maps/CVoxelMapRGB.h>
 #include <mrpt/maps/CWeightedPointsMap.h>
 #include <mrpt/maps/CWirelessPowerGridMap2D.h>
 

libs/maps/include/mrpt/maps/CMultiMetricMap.h

@@ -34,6 +34,7 @@ class TSetOfMetricMapInitializers;
  *	- mrpt::maps::COccupancyGridMap3D: 3D occupancy voxel map.
  *	- mrpt::maps::COctoMap: For 3D occupancy grids of variable resolution,
  *    with octrees (based on the library `octomap`).
+ *  - mrpt::maps::CVoxelMap or mrpt::maps::CVoxelMapRGB: 3D sparse voxel maps.
  *	- mrpt::maps::CColouredOctoMap: The same than above, but nodes can store
  *    RGB data appart from occupancy.
  *	- mrpt::maps::CLandmarksMap: For visual landmarks,etc...

libs/maps/include/mrpt/maps/COccupancyGridMap3D.h

@@ -28,6 +28,9 @@ namespace mrpt::maps
  *certainly occupied, 1 means a certainly empty voxel. Initially 0.5 means
  *uncertainty.
  *
+ * An alternative, sparse representation of a 3D map is provided
+ * via mrpt::maps::CVoxelMap and mrpt::maps::CVoxelMapRGB
+ *
  * \ingroup mrpt_maps_grp
  **/
 class COccupancyGridMap3D

libs/maps/include/mrpt/maps/COctoMap.h

@@ -51,13 +51,11 @@ class COctoMap : public COctoMapBase<octomap::OcTree, octomap::OcTreeNode>
 		mrpt::opengl::COctoMapVoxels& gl_obj) const override;
 
 	MAP_DEFINITION_START(COctoMap)
-	double resolution{
-		0.10};	//!< The finest resolution of the octomap (default: 0.10
-	//! meters)
-	mrpt::maps::COctoMap::TInsertionOptions
-		insertionOpts;	//!< Observations insertion options
-	mrpt::maps::COctoMap::TLikelihoodOptions
-		likelihoodOpts;	 //!< Probabilistic observation likelihood options
+	/// The finest resolution of the octomap (default: 0.10 meters)
+	double resolution = 0.10;
+	mrpt::maps::COctoMap::TInsertionOptions insertionOpts;
+	/// Probabilistic observation likelihood options
+	mrpt::maps::COctoMap::TLikelihoodOptions likelihoodOpts;
 	MAP_DEFINITION_END(COctoMap)
 
 	/** Returns true if the map is empty/no observation has been inserted */

libs/maps/include/mrpt/maps/CVoxelMap.h

@@ -0,0 +1,249 @@
+/* +------------------------------------------------------------------------+
+   |                     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/config/CLoadableOptions.h>
+#include <mrpt/maps/CLogOddsGridMapLUT.h>
+#include <mrpt/maps/CVoxelMapBase.h>
+#include <mrpt/maps/OccupancyGridCellType.h>
+#include <mrpt/maps/logoddscell_traits.h>
+#include <mrpt/obs/obs_frwds.h>
+
+namespace mrpt::maps
+{
+/**
+ * Log-odds occupancy sparse voxel map.
+ *
+ * \ingroup mrpt_maps_grp
+ */
+class CVoxelMap : public CVoxelMapBase<int8_t>,
+				  public detail::logoddscell_traits<int8_t>
+{
+	// This must be added to any CSerializable derived class:
+	DEFINE_SERIALIZABLE(CVoxelMap, mrpt::maps)
+
+   protected:
+	using traits_t = detail::logoddscell_traits<voxel_node_t>;
+
+   public:
+	CVoxelMap(
+		double resolution = 0.05, uint8_t inner_bits = 2, uint8_t leaf_bits = 3)
+		: CVoxelMapBase(resolution, inner_bits, leaf_bits)
+	{
+	}
+	~CVoxelMap();
+
+	bool isEmpty() const override;
+	void getAsOctoMapVoxels(
+		mrpt::opengl::COctoMapVoxels& gl_obj) const override;
+
+	/** Manually updates the occupancy of the voxel at (x,y,z) as being occupied
+	 * (true) or free (false), using the log-odds parameters in \a
+	 * insertionOptions */
+	void updateVoxel(
+		const double x, const double y, const double z, bool occupied);
+
+	/** Get the occupancy probability [0,1] of a point
+	 * \return false if the point is not mapped, in which case the returned
+	 * "prob" is undefined. */
+	bool getPointOccupancy(
+		const double x, const double y, const double z,
+		double& prob_occupancy) const;
+
+	void insertPointCloudAsRays(
+		const mrpt::maps::CPointsMap& pts,
+		const mrpt::math::TPoint3D& sensorPt);
+
+	void insertPointCloudAsEndPoints(const mrpt::maps::CPointsMap& pts);
+
+	struct TInsertionOptions : public mrpt::config::CLoadableOptions
+	{
+		TInsertionOptions() = default;
+
+		double max_range = -1;	//!< Maximum insertion ray range (<0: none)
+
+		double prob_miss = 0.45;
+		double prob_hit = 0.65;
+		double clamp_min = 0.10;
+		double clamp_max = 0.95;
+
+		bool ray_trace_free_space = false;
+
+		// See base docs
+		void loadFromConfigFile(
+			const mrpt::config::CConfigFileBase& source,
+			const std::string& section) override;
+
+		void writeToStream(mrpt::serialization::CArchive& out) const;
+		void readFromStream(mrpt::serialization::CArchive& in);
+	};
+
+	/// The options used when inserting observations in the map:
+	TInsertionOptions insertionOptions;
+
+	/** Options used when evaluating "computeObservationLikelihood"
+	 * \sa CObservation::computeObservationLikelihood
+	 */
+	struct TLikelihoodOptions : public mrpt::config::CLoadableOptions
+	{
+		TLikelihoodOptions() = default;
+		~TLikelihoodOptions() override = default;
+
+		// See base docs
+		void loadFromConfigFile(
+			const mrpt::config::CConfigFileBase& source,
+			const std::string& section) override;
+
+		void writeToStream(mrpt::serialization::CArchive& out) const;
+		void readFromStream(mrpt::serialization::CArchive& in);
+
+		/// Speed up the likelihood computation by considering only one out of N
+		/// rays (default=1)
+		uint32_t decimation = 1;
+	};
+
+	TLikelihoodOptions likelihoodOptions;
+
+	/** Options for the conversion of a mrpt::maps::COctoMap into a
+	 * mrpt::opengl::COctoMapVoxels */
+	struct TRenderingOptions
+	{
+		TRenderingOptions() = default;
+
+		bool generateOccupiedVoxels = true;
+		bool visibleOccupiedVoxels = true;
+
+		bool generateFreeVoxels = true;
+		bool visibleFreeVoxels = true;
+
+		/** Binary dump to stream */
+		void writeToStream(mrpt::serialization::CArchive& out) const;
+		/** Binary dump to stream */
+		void readFromStream(mrpt::serialization::CArchive& in);
+	};
+
+	TRenderingOptions renderingOptions;
+
+	MAP_DEFINITION_START(CVoxelMap)
+	double resolution = 0.10;
+	uint8_t inner_bits = 2;
+	uint8_t leaf_bits = 3;
+	mrpt::maps::CVoxelMap::TInsertionOptions insertionOpts;
+	mrpt::maps::CVoxelMap::TLikelihoodOptions likelihoodOpts;
+	MAP_DEFINITION_END(CVoxelMap)
+
+	/** Performs Bayesian fusion of a new observation of a cell.
+	 * This method increases the "occupancy-ness" of a cell, managing possible
+	 * saturation.
+	 *  \param theCell The cell to modify
+	 *  \param logodd_obs Observation of the cell, in log-odd form as
+	 * transformed by p2l.
+	 *  \param thres  This must be CELLTYPE_MIN+logodd_obs
+	 * \sa updateCell, updateCell_fast_free
+	 */
+	inline void updateCell_fast_occupied(
+		voxel_node_t* theCell, const voxel_node_t logodd_obs,
+		const voxel_node_t thres)
+	{
+		if (theCell == nullptr) return;
+		if (*theCell > thres) *theCell -= logodd_obs;
+		else
+			*theCell = traits_t::CELLTYPE_MIN;
+	}
+
+	/** Performs Bayesian fusion of a new observation of a cell.
+	 * This method increases the "occupancy-ness" of a cell, managing possible
+	 * saturation.
+	 *  \param coord Cell indexes.
+	 *  \param logodd_obs Observation of the cell, in log-odd form as
+	 * transformed by p2l.
+	 *  \param thres  This must be CELLTYPE_MIN+logodd_obs
+	 * \sa updateCell, updateCell_fast_free
+	 */
+	inline void updateCell_fast_occupied(
+		const Bonxai::CoordT& coord, const voxel_node_t logodd_obs,
+		const voxel_node_t thres)
+	{
+		if (voxel_node_t* cell = m_impl->accessor.value(coord, true /*create*/);
+			cell)
+			updateCell_fast_occupied(cell, logodd_obs, thres);
+	}
+
+	/** Performs Bayesian fusion of a new observation of a cell.
+	 * This method increases the "free-ness" of a cell, managing possible
+	 * saturation.
+	 *  \param logodd_obs Observation of the cell, in log-odd form as
+	 * transformed by p2l.
+	 *  \param thres  This must be CELLTYPE_MAX-logodd_obs
+	 * \sa updateCell_fast_occupied
+	 */
+	inline void updateCell_fast_free(
+		voxel_node_t* theCell, const voxel_node_t logodd_obs,
+		const voxel_node_t thres)
+	{
+		if (theCell == nullptr) return;
+		if (*theCell < thres) *theCell += logodd_obs;
+		else
+			*theCell = traits_t::CELLTYPE_MAX;
+	}
+
+	/** Performs the Bayesian fusion of a new observation of a cell.
+	 * This method increases the "free-ness" of a cell, managing possible
+	 * saturation.
+	 *  \param coord Cell indexes.
+	 *  \param logodd_obs Observation of the cell, in log-odd form as
+	 * transformed by p2l.
+	 *  \param thres  This must be CELLTYPE_MAX-logodd_obs
+	 * \sa updateCell_fast_occupied
+	 */
+	inline void updateCell_fast_free(
+		const Bonxai::CoordT& coord, const voxel_node_t logodd_obs,
+		const voxel_node_t thres)
+	{
+		if (voxel_node_t* cell = m_impl->accessor.value(coord, true /*create*/);
+			cell)
+			updateCell_fast_free(cell, logodd_obs, thres);
+	}
+
+	/** Lookup tables for log-odds */
+	static CLogOddsGridMapLUT<voxel_node_t>& get_logodd_lut();
+
+	/** 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 voxel_node_t l)
+	{
+		return get_logodd_lut().l2p(l);
+	}
+
+	/** Scales an integer representation of the log-odd into a linear scale
+	 * [0,255], using p=exp(l)/(1+exp(l)) */
+	static inline uint8_t l2p_255(const voxel_node_t l)
+	{
+		return get_logodd_lut().l2p_255(l);
+	}
+	/** Scales a real valued probability in [0,1] to an integer representation
+	 * of: log(p)-log(1-p)  in the valid range of voxel_node_t */
+	static inline voxel_node_t p2l(const float p)
+	{
+		return get_logodd_lut().p2l(p);
+	}
+
+   protected:
+	void internal_clear() override;
+	bool internal_insertObservation(
+		const mrpt::obs::CObservation& obs,
+		const std::optional<const mrpt::poses::CPose3D>& robotPose =
+			std::nullopt) override;
+	double internal_computeObservationLikelihood(
+		const mrpt::obs::CObservation& obs,
+		const mrpt::poses::CPose3D& takenFrom) const override;
+};
+
+}  // namespace mrpt::maps