From 6387fcfe60bde17ecb4926275f6d02bfdacde55d Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 18 Oct 2023 02:24:43 +0200 Subject: [PATCH] smooth color fusion --- libs/maps/include/mrpt/maps/CVoxelMapRGB.h | 1 + libs/maps/src/maps/CVoxelMapRGB.cpp | 17 ++++++++++++++++- samples/maps_voxelmap_from_tum_dataset/test.cpp | 3 ++- 3 files changed, 19 insertions(+), 2 deletions(-) diff --git a/libs/maps/include/mrpt/maps/CVoxelMapRGB.h b/libs/maps/include/mrpt/maps/CVoxelMapRGB.h index 7aa8e021a2..597c7819e3 100644 --- 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 index 2afd64e9ff..75a0e26e16 100644 --- 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 index 53158d793a..8a70f9a8f4 100644 --- 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(); }