Feature/bonxai

appveyor.yml

@@ -31,7 +31,7 @@ build_script:
   - cd c:\projects\mrpt
   - cmd: IF NOT DEFINED APPVEYOR_REPO_TAG_NAME SET PKG_NAME=branch-%APPVEYOR_REPO_BRANCH%
   - cmd: IF DEFINED APPVEYOR_REPO_TAG_NAME SET PKG_NAME=release-%APPVEYOR_REPO_TAG_NAME%
-  - cmd: move c:\projects\mrpt\build\MRPT*.exe c:\projects\mrpt\mrpt-%PKG_NAME%.exe
+  - cmd: IF EXIST c:\projects\mrpt\build\MRPT*.exe move c:\projects\mrpt\build\MRPT*.exe c:\projects\mrpt\mrpt-%PKG_NAME%.exe
   - dir
 
 install:

doc/source/bibliography.bib

@@ -170,4 +170,13 @@ @article{horn1987closed
   pages={629--642},
   year={1987},
   publisher={Optica Publishing Group}
+}
+
+@inproceedings{wurm2010octomap,
+  title={OctoMap: A probabilistic, flexible, and compact 3D map representation for robotic systems},
+  author={Wurm, Kai M and Hornung, Armin and Bennewitz, Maren and Stachniss, Cyrill and Burgard, Wolfram},
+  booktitle={Proc. of the ICRA 2010 workshop on best practice in 3D perception and modeling for mobile manipulation},
+  volume={2},
+  pages={3},
+  year={2010}
 }

doc/source/doxygen-docs/changelog.md

@@ -1,6 +1,13 @@
 \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
+      - Example: \ref maps_voxelmap_from_tum_dataset
+      - Example: \ref maps_voxelmap_simple
 - 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-hwdrivers_taobotics_imu.md

@@ -4,5 +4,7 @@ This example demonstrates the C++ API to read from Taobotics IMU sensors (e.g. R
 
 ![Screenshot](https://mrpt.github.io/imgs/screenshot_hwdrivers_taobotics_imu.jpg)
 
+Short video: [https://www.youtube.com/shorts/qaaP9BmZYmo](https://www.youtube.com/shorts/qaaP9BmZYmo)
+
 C++ example source code:
 \include hwdrivers_taobotics_imu/test.cpp

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

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

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

@@ -0,0 +1,5 @@
+\page maps_voxelmap_simple Example: maps_voxelmap_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,8 @@ 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_maps_voxelmap_from_tum_dataset.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/CColouredOctoMap.h

@@ -30,6 +30,8 @@ namespace maps
  *  This version stores both, occupancy information and RGB colour data at
  * each octree node. See the base class mrpt::maps::COctoMapBase.
  *
+ * The octomap library was presented in \cite wurm2010octomap
+ *
  * \sa CMetricMap, the example in "MRPT/samples/octomap_simple"
  * \ingroup mrpt_maps_grp
  */

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

@@ -33,6 +33,8 @@ namespace maps
  *  This version only stores occupancy information at each octree node. See the
  * base class mrpt::maps::COctoMapBase.
  *
+ * The octomap library was presented in \cite wurm2010octomap
+ *
  * \sa CMetricMap, the example in "MRPT/samples/octomap_simple"
  * \ingroup mrpt_maps_grp
  */
@@ -49,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/COctoMapBase.h

@@ -30,13 +30,7 @@ namespace mrpt::maps
  * To use octomap's iterators to go through the voxels, use
  * COctoMap::getOctomap()
  *
- * The octomap library was presented in:
- *  - K. M. Wurm, A. Hornung, M. Bennewitz, C. Stachniss, and W. Burgard,
- *     <i>"OctoMap: A Probabilistic, Flexible, and Compact 3D Map Representation
- * for Robotic Systems"</i>
- *     in Proc. of the ICRA 2010 Workshop on Best Practice in 3D Perception and
- * Modeling for Mobile Manipulation, 2010. Software available at
- * http://octomap.sf.net/.
+ * The octomap library was presented in \cite wurm2010octomap
  *
  * \sa CMetricMap, the example in "MRPT/samples/octomap_simple"
  * \ingroup mrpt_maps_grp

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

@@ -0,0 +1,64 @@
+/* +------------------------------------------------------------------------+
+   |                     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/CVoxelMapOccupancyBase.h>
+
+namespace mrpt::maps
+{
+/** Voxel contents for CVoxelMap
+ */
+struct VoxelNodeOccupancy
+{
+	int8_t occupancy = 0;
+
+	// ---- API expected by CVoxelMapOccupancyBase ----
+	int8_t& occupancyRef() { return occupancy; }
+	const int8_t& occupancyRef() const { return occupancy; }
+};
+
+/**
+ * Log-odds sparse voxel map for cells containing only the *occupancy* of each
+ * voxel.
+ *
+ * \ingroup mrpt_maps_grp
+ */
+class CVoxelMap : public CVoxelMapOccupancyBase<VoxelNodeOccupancy>
+{
+	// This must be added to any CSerializable derived class:
+	DEFINE_SERIALIZABLE(CVoxelMap, mrpt::maps)
+
+   public:
+	CVoxelMap(
+		double resolution = 0.05, uint8_t inner_bits = 2, uint8_t leaf_bits = 3)
+		: CVoxelMapOccupancyBase(resolution, inner_bits, leaf_bits)
+	{
+	}
+	~CVoxelMap();
+
+	MAP_DEFINITION_START(CVoxelMap)
+	double resolution = 0.10;
+	uint8_t inner_bits = 2;
+	uint8_t leaf_bits = 3;
+	mrpt::maps::TVoxelMap_InsertionOptions insertionOpts;
+	mrpt::maps::TVoxelMap_LikelihoodOptions likelihoodOpts;
+	MAP_DEFINITION_END(CVoxelMap)
+
+   protected:
+	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

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

@@ -0,0 +1,132 @@
+/* +------------------------------------------------------------------------+
+   |                     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 <mrpt/opengl/Scene.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>;
+	using voxel_node_t = 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_impl(std::make_unique<Impl>(resolution, inner_bits, leaf_bits))
+	{
+	}
+	virtual ~CVoxelMapBase() override = default;
+
+	CVoxelMapBase(const CVoxelMapBase& o) : CVoxelMapBase(o.grid().resolution)
+	{
+		*this = o;
+	}
+	CVoxelMapBase& operator=(const CVoxelMapBase& o)
+	{
+		// grid() = o.grid();
+		THROW_EXCEPTION("Bonxai voxel grid copy not implemented");
+		return *this;
+	}
+
+	CVoxelMapBase(CVoxelMapBase&& o) : m_impl(std::move(o.m_impl)) {}
+	CVoxelMapBase& operator=(CVoxelMapBase&& o)
+	{
+		m_impl = std::move(o.m_impl);
+		return *this;
+	}
+
+	const Bonxai::VoxelGrid<node_t>& grid() const { return m_impl->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;
+
+	virtual void saveMetricMapRepresentationToFile(
+		const std::string& filNamePrefix) const override
+	{
+		MRPT_START
+		// Save as 3D Scene:
+		{
+			mrpt::opengl::Scene scene;
+			scene.insert(this->getVisualization());
+			const std::string fil = filNamePrefix + std::string("_3D.3Dscene");
+			scene.saveToFile(fil);
+		}
+		// Save binary data file?
+		MRPT_END
+	}
+
+   protected:
+	struct Impl
+	{
+		Impl(double resolution, uint8_t inner_bits, uint8_t leaf_bits)
+			: grid(resolution, inner_bits, leaf_bits),
+			  accessor(grid.createAccessor())
+		{
+		}
+		Impl(Bonxai::VoxelGrid<node_t>&& g)
+			: grid(std::move(g)), accessor(grid.createAccessor())
+		{
+		}
+		Bonxai::VoxelGrid<node_t> grid;
+		mutable typename Bonxai::VoxelGrid<node_t>::Accessor accessor;
+	};
+	std::unique_ptr<Impl> m_impl;
+};
+
+}  // namespace mrpt::maps