Add cached pointcloud

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

@@ -11,6 +11,7 @@
 
 #include <mrpt/config/CLoadableOptions.h>
 #include <mrpt/maps/CLogOddsGridMapLUT.h>
+#include <mrpt/maps/CSimplePointsMap.h>
 #include <mrpt/maps/CVoxelMapBase.h>
 #include <mrpt/maps/OccupancyGridCellType.h>
 #include <mrpt/maps/logoddscell_traits.h>
@@ -63,6 +64,14 @@ class CVoxelMap : public CVoxelMapBase<int8_t>,
 
 	void insertPointCloudAsEndPoints(const mrpt::maps::CPointsMap& pts);
 
+	/** Returns all occupied voxels as a point cloud. The shared_ptr is
+	 *  also hold and updated internally, so it is not safe to read it
+	 *  while also updating the voxel map in another thread.
+	 *
+	 *  The point cloud is cached, and invalidated upon map updates.
+	 */
+	mrpt::maps::CSimplePointsMap::Ptr getOccupiedVoxels() const;
+
 	struct TInsertionOptions : public mrpt::config::CLoadableOptions
 	{
 		TInsertionOptions() = default;
@@ -249,6 +258,11 @@ class CVoxelMap : public CVoxelMapBase<int8_t>,
 	double internal_computeObservationLikelihood(
 		const mrpt::obs::CObservation& obs,
 		const mrpt::poses::CPose3D& takenFrom) const override;
+
+	void invalidateOccupiedCache() { m_cachedOccupied.reset(); }
+
+	void updateOccupiedPointsCache() const;
+	mutable mrpt::maps::CSimplePointsMap::Ptr m_cachedOccupied;
 };
 
 }  // namespace mrpt::maps

libs/maps/src/maps/CVoxelMap.cpp

@@ -356,6 +356,8 @@ void CVoxelMap::internal_clear()
 void CVoxelMap::updateVoxel(
 	const double x, const double y, const double z, bool occupied)
 {
+	invalidateOccupiedCache();
+
 	voxel_node_t* cell = m_impl->accessor.value(
 		Bonxai::PosToCoord({x, y, z}, m_impl->grid.inv_resolution),
 		true /*create*/);
@@ -399,6 +401,8 @@ bool CVoxelMap::getPointOccupancy(
 void CVoxelMap::insertPointCloudAsRays(
 	const mrpt::maps::CPointsMap& pts, const mrpt::math::TPoint3D& sensorPt)
 {
+	invalidateOccupiedCache();
+
 	const voxel_node_t logodd_observation_occupied =
 		std::max<voxel_node_t>(1, p2l(insertionOptions.prob_hit));
 	const voxel_node_t logodd_thres_occupied =
@@ -486,6 +490,8 @@ void CVoxelMap::insertPointCloudAsRays(
 
 void CVoxelMap::insertPointCloudAsEndPoints(const mrpt::maps::CPointsMap& pts)
 {
+	invalidateOccupiedCache();
+
 	const voxel_node_t logodd_observation_occupied =
 		std::max<voxel_node_t>(1, p2l(insertionOptions.prob_hit));
 	const voxel_node_t logodd_thres_occupied =
@@ -513,3 +519,36 @@ void CVoxelMap::insertPointCloudAsEndPoints(const mrpt::maps::CPointsMap& pts)
 			cell, logodd_observation_occupied, logodd_thres_occupied);
 	}
 }
+
+void CVoxelMap::updateOccupiedPointsCache() const
+{
+	if (m_cachedOccupied) return;  // done
+
+	m_cachedOccupied = mrpt::maps::CSimplePointsMap::Create();
+
+	// forEachCell() has no const version
+	auto& grid = const_cast<Bonxai::VoxelGrid<voxel_node_t>&>(m_impl->grid);
+
+	// Go thru all voxels:
+	auto lmbdPerVoxel = [this, &grid](
+							voxel_node_t& data, const Bonxai::CoordT& coord) {
+		using mrpt::img::TColor;
+
+		// log-odds to probability:
+		const double occFreeness = this->l2p(data);
+		const auto pt = Bonxai::CoordToPos(coord, grid.resolution);
+
+		if (occFreeness < 0.5)
+		{
+			m_cachedOccupied->insertPointFast(pt.x, pt.y, pt.z);
+		}
+	};	// end lambda for each voxel
+
+	grid.forEachCell(lmbdPerVoxel);
+}
+
+mrpt::maps::CSimplePointsMap::Ptr CVoxelMap::getOccupiedVoxels() const
+{
+	updateOccupiedPointsCache();
+	return m_cachedOccupied;
+}

samples/maps_voxelmap_simple/test.cpp

@@ -82,6 +82,13 @@ void TestVoxelMap()
 
 		map.getAsOctoMapVoxels(*gl_map);
 
+		// View occupied points:
+		{
+			auto mapPts = map.getOccupiedVoxels();
+			mapPts->renderOptions.point_size = 5.0;
+			scene->insert(mapPts->getVisualization());
+		}
+
 		gl_map->showGridLines(false);
 		gl_map->showVoxels(mrpt::opengl::VOXEL_SET_OCCUPIED, true);
 		gl_map->showVoxels(mrpt::opengl::VOXEL_SET_FREESPACE, true);