smooth color fusion

MRPT · Oct 18, 2023 · 6387fcf · 6387fcf
1 parent cfaa33a
commit 6387fcf
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 2 deletions.
diff --git a/libs/maps/include/mrpt/maps/CVoxelMapRGB.h b/libs/maps/include/mrpt/maps/CVoxelMapRGB.h
@@ -20,6 +20,7 @@ struct VoxelNodeOccRGB
 {
 	int8_t occupancy = 0;
 	mrpt::img::TColor color;
+	uint32_t numColObs = 0;
 
 	// ---- API expected by CVoxelMapOccupancyBase ----
 	int8_t& occupancyRef() { return occupancy; }

diff --git a/libs/maps/src/maps/CVoxelMapRGB.cpp b/libs/maps/src/maps/CVoxelMapRGB.cpp
@@ -157,10 +157,25 @@ bool CVoxelMapRGB::internal_insertObservation_3DScan(
 		updateCell_fast_occupied(
 			cell, logodd_observation_occupied, logodd_thres_occupied);
 
-		// and copy color:
+		// and merge color:
 		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 newF;
+		const float N_1 = 1.0f / (cell->numColObs + 1);
+
+		newF.R = N_1 * (oldCol.R * cell->numColObs + colF.R);
+		newF.G = N_1 * (oldCol.G * cell->numColObs + colF.G);
+		newF.B = N_1 * (oldCol.B * cell->numColObs + colF.B);
+
+		cell->numColObs++;
+		cell->color = newF.asTColor();
+#else
+		// just copy latest color:
 		cell->color = colF.asTColor();
+#endif
 	}
 
 	return true;

diff --git a/samples/maps_voxelmap_from_tum_dataset/test.cpp b/samples/maps_voxelmap_from_tum_dataset/test.cpp
@@ -119,7 +119,8 @@ void TestVoxelMapFromTUM(
 		scene->insert(glVoxels);
 
 		glViewRGB = scene->createViewport("rgb_view");
-		glViewRGB->setViewportPosition(0, 0.7, 0.4, 0.3);
+		glViewRGB->setViewportPosition(0, 0.7, 0.3, 0.25);
+		glViewRGB->setTransparent(true);
 
 		win.unlockAccess3DScene();
 	}