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/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,117 @@
+/* +------------------------------------------------------------------------+
+   |                     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/CVoxelMapBase.h>
+#include <mrpt/obs/obs_frwds.h>
+
+namespace mrpt::maps
+{
+/**
+ * Log-odds occupancy sparse voxel map.
+ *
+ * \ingroup mrpt_maps_grp
+ */
+class CVoxelMap : public CVoxelMapBase<uint8_t>
+{
+	// This must be added to any CSerializable derived class:
+	DEFINE_SERIALIZABLE(CVoxelMap, mrpt::maps)
+
+   public:
+	CVoxelMap(double resolution, uint8_t inner_bits = 2, uint8_t leaf_bits = 3)
+		: CVoxelMapBase(resolution, inner_bits, leaf_bits)
+	{
+	}
+
+	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;
+
+		// See base docs
+		void loadFromConfigFile(
+			const mrpt::config::CConfigFileBase& source,
+			const std::string& section) override;
+		void dumpToTextStream(std::ostream& out) const override;
+	};
+
+	/// 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 dumpToTextStream(std::ostream& out) const 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 generateGridLines = false;
+
+		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)
+
+   protected:
+	void internal_clear() override;
+	bool internal_insertObservation(
+		const mrpt::obs::CObservation& obs,
+		const std::optional<const mrpt::poses::CPose3D>& robotPose =
+			std::nullopt) override;
+};
+
+}  // namespace mrpt::maps

libs/maps/include/mrpt/maps/CVoxelMapBase.h

@@ -0,0 +1,83 @@
+/* +------------------------------------------------------------------------+
+   |                     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/maps/CMetricMap.h>
+#include <mrpt/obs/obs_frwds.h>
+#include <mrpt/opengl/COctoMapVoxels.h>
+#include <mrpt/opengl/CSetOfObjects.h>
+
+#include <cstdint>
+
+#include "bonxai/bonxai.hpp"
+
+namespace mrpt::maps
+{
+/** An mrpt CMetricMap wrapper for Bonxai's VoxelMap container.
+ *
+ * Refer to Davide Faconti's [Bonxai
+ * repository](https://github.com/facontidavide/Bonxai) for publication and
+ * algorithm details, but in short, this is a sparse generic container for
+ * voxels, not needing redimensioning when the map grows, and with efficient
+ * insertion and update operations.
+ *
+ * Users normally use the derived classes, not this generic base template.
+ * This base class implements all common aspects to CMetricMap that do not
+ * depend on the specific contents to be stored at each voxel.
+ *
+ * No multi-threading protection is applied at all in the API.
+ *
+ * \sa CMetricMap, the example in "MRPT/samples/maps_voxelmap_simple",
+ * \ingroup mrpt_maps_grp
+ */
+template <typename node_t>
+class CVoxelMapBase : public mrpt::maps::CMetricMap
+{
+   public:
+	using myself_t = CVoxelMapBase<node_t>;
+
+	/** Constructor, defines the resolution of the voxelmap
+	 *  (length of each voxel side, in meters).
+	 */
+	CVoxelMapBase(
+		double resolution, uint8_t inner_bits = 2, uint8_t leaf_bits = 3)
+		: m_grid(resolution, inner_bits, leaf_bits)
+	{
+	}
+	virtual ~CVoxelMapBase() override = default;
+
+	const Bonxai::VoxelGrid<node_t>& grid() const { return m_grid; }
+
+	/** Returns a short description of the map. */
+	std::string asString() const override { return "Voxelmap"; }
+
+	/** Returns a 3D object representing the map.
+	 * \sa renderingOptions
+	 */
+	void getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const override
+	{
+		auto gl_obj = mrpt::opengl::COctoMapVoxels::Create();
+		this->getAsOctoMapVoxels(*gl_obj);
+		o.insert(gl_obj);
+	}
+
+	/** Builds a renderizable representation of the octomap as a
+	 * mrpt::opengl::COctoMapVoxels object.
+	 * Implementation defined for each children class.
+	 * \sa renderingOptions
+	 */
+	virtual void getAsOctoMapVoxels(
+		mrpt::opengl::COctoMapVoxels& gl_obj) const = 0;
+
+   private:
+	Bonxai::VoxelGrid<node_t> m_grid;
+};
+
+}  // namespace mrpt::maps

libs/maps/include/mrpt/maps/CVoxelMapRGB.h

@@ -0,0 +1,29 @@
+/* +------------------------------------------------------------------------+
+   |                     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/core/safe_pointers.h>
+#include <mrpt/maps/CMetricMap.h>
+#include <mrpt/maps/COctoMapBase.h>
+#include <mrpt/obs/obs_frwds.h>
+
+namespace octomap
+{
+class OcTree;
+}
+namespace octomap
+{
+class OcTreeNode;
+}
+
+namespace mrpt::maps
+{
+}