Integrate serialization

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

@@ -119,10 +119,14 @@ class CVoxelMapBase : public mrpt::maps::CMetricMap
 			  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
+}  // namespace mrpt::maps

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

@@ -373,7 +373,9 @@ void CVoxelMapOccupancyBase<voxel_node_t, occupancy_t>::getAsOctoMapVoxels(
 				case COctoMapVoxels::COLOR_FROM_RGB_DATA:
 					if constexpr (internal::has_color<voxel_node_t>::value)
 					{
-						vx_color = data.color;
+						vx_color.R = data.color.R;
+						vx_color.G = data.color.G;
+						vx_color.B = data.color.B;
 					}
 					else
 					{

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

@@ -19,7 +19,10 @@ namespace mrpt::maps
 struct VoxelNodeOccRGB
 {
 	int8_t occupancy = 0;
-	mrpt::img::TColor color;
+	struct TColor
+	{
+		uint8_t R = 0, G = 0, B = 0;
+	} color;
 	uint32_t numColObs = 0;
 
 	// ---- API expected by CVoxelMapOccupancyBase ----

libs/maps/include/mrpt/maps/bonxai/serialization.hpp

@@ -7,7 +7,7 @@
 #include <exception>
 #include <type_traits>
 #include <fstream>
-#include "bonxai/bonxai.hpp"
+#include "bonxai.hpp"
 
 #ifdef __GNUG__
 #include <cstdlib>
@@ -82,7 +82,7 @@ template <typename DataT>
 inline void Serialize(std::ostream& out, const VoxelGrid<DataT>& grid)
 {
   static_assert(std::is_trivially_copyable_v<DataT>,
-                "DataT must ne trivially copyable");
+                "DataT must be trivially copyable");
 
   char header[256];
   std::string type_name = details::demangle(typeid(DataT).name());
@@ -209,18 +209,18 @@ inline VoxelGrid<DataT> Deserialize(std::istream& input, HeaderInfo info)
     {
       const uint32_t inner_index = *inner;
       using LeafGridT = typename VoxelGrid<DataT>::LeafGrid;
-      inner_grid.data[inner_index] = std::make_shared<LeafGridT>(info.leaf_bits);
-      auto& leaf_grid = inner_grid.data[inner_index];
+      inner_grid.cell(inner_index) = std::make_shared<LeafGridT>(info.leaf_bits);
+      auto& leaf_grid = inner_grid.cell(inner_index);
 
-      for (size_t w = 0; w < leaf_grid->mask.wordCount(); w++)
+      for (size_t w = 0; w < leaf_grid->mask().wordCount(); w++)
       {
         uint64_t word = Read<uint64_t>(input);
-        leaf_grid->mask.setWord(w, word);
+        leaf_grid->mask().setWord(w, word);
       }
-      for (auto leaf = leaf_grid->mask.beginOn(); leaf; ++leaf)
+      for (auto leaf = leaf_grid->mask().beginOn(); leaf; ++leaf)
       {
         const uint32_t leaf_index = *leaf;
-        leaf_grid->data[leaf_index] = Read<DataT>(input);
+        leaf_grid->cell(leaf_index) = Read<DataT>(input);
       }
     }
   }

libs/maps/src/maps/CVoxelMap.cpp

@@ -12,6 +12,8 @@
 #include <mrpt/maps/CSimplePointsMap.h>
 #include <mrpt/maps/CVoxelMap.h>
 
+#include <mrpt/maps/bonxai/serialization.hpp>
+
 using namespace mrpt::maps;
 using namespace std::string_literals;  // "..."s
 
@@ -69,8 +71,10 @@ void CVoxelMap::serializeTo(mrpt::serialization::CArchive& out) const
 	renderingOptions.writeToStream(out);  // Added in v1
 	out << genericMapParams;
 
-	THROW_EXCEPTION("TODO");
-	// const_cast<octomap::OcTree*>(&m_impl->m_octomap)->writeBinary(ss);
+	// grid data:
+	std::stringstream ss;
+	Bonxai::Serialize(ss, grid());
+	out << ss.str();
 }
 
 void CVoxelMap::serializeFrom(
@@ -87,8 +91,17 @@ void CVoxelMap::serializeFrom(
 
 			this->clear();
 
-			THROW_EXCEPTION("TODO");
-			// m_impl->m_octomap.readBinary(ss);
+			// grid data:
+			std::string msg;
+			in >> msg;
+			std::istringstream ifile(msg, std::ios::binary);
+
+			char header[256];
+			ifile.getline(header, 256);
+			Bonxai::HeaderInfo info = Bonxai::GetHeaderInfo(header);
+
+			m_impl = std::make_unique<Impl>(
+				std::move(Bonxai::Deserialize<voxel_node_t>(ifile, info)));
 		}
 		break;
 		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);

libs/maps/src/maps/CVoxelMapRGB.cpp

@@ -14,6 +14,8 @@
 #include <mrpt/maps/CVoxelMapRGB.h>
 #include <mrpt/obs/CObservation3DRangeScan.h>
 
+#include <mrpt/maps/bonxai/serialization.hpp>
+
 using namespace mrpt::maps;
 using namespace std::string_literals;  // "..."s
 
@@ -72,8 +74,10 @@ void CVoxelMapRGB::serializeTo(mrpt::serialization::CArchive& out) const
 	renderingOptions.writeToStream(out);  // Added in v1
 	out << genericMapParams;
 
-	THROW_EXCEPTION("TODO");
-	// const_cast<octomap::OcTree*>(&m_impl->m_octomap)->writeBinary(ss);
+	// grid data:
+	std::stringstream ss;
+	Bonxai::Serialize(ss, grid());
+	out << ss.str();
 }
 
 void CVoxelMapRGB::serializeFrom(
@@ -90,8 +94,17 @@ void CVoxelMapRGB::serializeFrom(
 
 			this->clear();
 
-			THROW_EXCEPTION("TODO");
-			// m_impl->m_octomap.readBinary(ss);
+			// grid data:
+			std::string msg;
+			in >> msg;
+			std::istringstream ifile(msg, std::ios::binary);
+
+			char header[256];
+			ifile.getline(header, 256);
+			Bonxai::HeaderInfo info = Bonxai::GetHeaderInfo(header);
+
+			m_impl = std::make_unique<Impl>(
+				Bonxai::Deserialize<voxel_node_t>(ifile, info));
 		}
 		break;
 		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
@@ -161,7 +174,8 @@ bool CVoxelMapRGB::internal_insertObservation_3DScan(
 		mrpt::img::TColorf colF;
 		colPts.getPointColor(i, colF.R, colF.G, colF.B);
 #if 1  // fuse colors:
-		mrpt::img::TColorf oldCol(cell->color);
+		mrpt::img::TColorf oldCol(
+			mrpt::img::TColor(cell->color.R, cell->color.G, cell->color.B));
 
 		mrpt::img::TColorf newF;
 		const float N_1 = 1.0f / (cell->numColObs + 1);
@@ -171,7 +185,10 @@ bool CVoxelMapRGB::internal_insertObservation_3DScan(
 		newF.B = N_1 * (oldCol.B * cell->numColObs + colF.B);
 
 		cell->numColObs++;
-		cell->color = newF.asTColor();
+		const auto nCol = newF.asTColor();
+		cell->color.R = nCol.R;
+		cell->color.G = nCol.G;
+		cell->color.B = nCol.B;
 #else
 		// just copy latest color:
 		cell->color = colF.asTColor();