From 1ddce707046d55516dc6a0d2e5a057e17da3c7b0 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 18 Oct 2023 00:24:18 +0200 Subject: [PATCH] progress voxelmap RGBD dataset demo --- libs/maps/include/mrpt/maps/CVoxelMap.h | 13 ++- libs/maps/src/maps/CVoxelMap.cpp | 25 +++--- .../maps_voxelmap_from_tum_dataset/test.cpp | 85 +++++++------------ 3 files changed, 57 insertions(+), 66 deletions(-) diff --git a/libs/maps/include/mrpt/maps/CVoxelMap.h b/libs/maps/include/mrpt/maps/CVoxelMap.h index 7543dccf88..fdd5aa4697 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMap.h +++ b/libs/maps/include/mrpt/maps/CVoxelMap.h @@ -91,7 +91,8 @@ class CVoxelMap : public CVoxelMapBase, const mrpt::config::CConfigFileBase& source, const std::string& section) override; void saveToConfigFile( - mrpt::config::CConfigFileBase& c, const std::string& s) const; + mrpt::config::CConfigFileBase& c, + const std::string& s) const override; void writeToStream(mrpt::serialization::CArchive& out) const; void readFromStream(mrpt::serialization::CArchive& in); @@ -113,7 +114,8 @@ class CVoxelMap : public CVoxelMapBase, const mrpt::config::CConfigFileBase& source, const std::string& section) override; void saveToConfigFile( - mrpt::config::CConfigFileBase& c, const std::string& s) const; + mrpt::config::CConfigFileBase& c, + const std::string& s) const override; void writeToStream(mrpt::serialization::CArchive& out) const; void readFromStream(mrpt::serialization::CArchive& in); @@ -121,6 +123,9 @@ class CVoxelMap : public CVoxelMapBase, /// Speed up the likelihood computation by considering only one out of N /// rays (default=1) uint32_t decimation = 1; + + /// Minimum occupancy (0,1) for a voxel to be considered occupied. + double occupiedThreshold = 0.60; }; TLikelihoodOptions likelihoodOptions; @@ -132,9 +137,11 @@ class CVoxelMap : public CVoxelMapBase, TRenderingOptions() = default; bool generateOccupiedVoxels = true; + double occupiedThreshold = 0.60; bool visibleOccupiedVoxels = true; bool generateFreeVoxels = true; + double freeThreshold = 0.40; bool visibleFreeVoxels = true; /** Binary dump to stream */ @@ -265,4 +272,4 @@ class CVoxelMap : public CVoxelMapBase, mutable mrpt::maps::CSimplePointsMap::Ptr m_cachedOccupied; }; -} // namespace mrpt::maps \ No newline at end of file +} // namespace mrpt::maps diff --git a/libs/maps/src/maps/CVoxelMap.cpp b/libs/maps/src/maps/CVoxelMap.cpp index 9d62ce28f6..1fed856f5d 100644 --- a/libs/maps/src/maps/CVoxelMap.cpp +++ b/libs/maps/src/maps/CVoxelMap.cpp @@ -144,11 +144,7 @@ double CVoxelMap::internal_computeObservationLikelihood( return 0; } -bool CVoxelMap::isEmpty() const -{ - THROW_EXCEPTION("TODO"); - return false; -} +bool CVoxelMap::isEmpty() const { return m_impl->grid.activeCellsCount() == 0; } void CVoxelMap::getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels& gl_obj) const { @@ -188,8 +184,10 @@ void CVoxelMap::getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels& gl_obj) const const auto pt = Bonxai::CoordToPos(coord, grid.resolution); bbox.updateWithPoint({pt.x, pt.y, pt.z}); - if ((occ >= 0.5 && renderingOptions.generateOccupiedVoxels) || - (occ < 0.5 && renderingOptions.generateFreeVoxels)) + if ((occ >= renderingOptions.occupiedThreshold && + renderingOptions.generateOccupiedVoxels) || + (occ < renderingOptions.freeThreshold && + renderingOptions.generateFreeVoxels)) { mrpt::img::TColor vx_color; double coefc, coeft; @@ -232,8 +230,9 @@ void CVoxelMap::getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels& gl_obj) const default: THROW_EXCEPTION("Unknown coloring scheme!"); } - const size_t vx_set = - (occ > 0.5) ? VOXEL_SET_OCCUPIED : VOXEL_SET_FREESPACE; + const size_t vx_set = (occ > renderingOptions.occupiedThreshold) + ? VOXEL_SET_OCCUPIED + : VOXEL_SET_FREESPACE; gl_obj.push_back_Voxel( vx_set, @@ -308,6 +307,7 @@ void CVoxelMap::TRenderingOptions::writeToStream( out << generateOccupiedVoxels << visibleOccupiedVoxels; out << generateFreeVoxels << visibleFreeVoxels; + out << occupiedThreshold << freeThreshold; } void CVoxelMap::TRenderingOptions::readFromStream( @@ -319,6 +319,7 @@ void CVoxelMap::TRenderingOptions::readFromStream( case 0: in >> generateOccupiedVoxels >> visibleOccupiedVoxels; in >> generateFreeVoxels >> visibleFreeVoxels; + in >> occupiedThreshold >> freeThreshold; break; default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); } @@ -328,23 +329,25 @@ void CVoxelMap::TLikelihoodOptions::loadFromConfigFile( const mrpt::config::CConfigFileBase& c, const std::string& s) { MRPT_LOAD_CONFIG_VAR(decimation, int, c, s); + MRPT_LOAD_CONFIG_VAR(occupiedThreshold, double, c, s); } void CVoxelMap::TLikelihoodOptions::saveToConfigFile( mrpt::config::CConfigFileBase& c, const std::string& s) const { MRPT_SAVE_CONFIG_VAR(decimation, c, s); + MRPT_SAVE_CONFIG_VAR(occupiedThreshold, c, s); } void CVoxelMap::TLikelihoodOptions::writeToStream( mrpt::serialization::CArchive& out) const { - out << decimation; + out << decimation << occupiedThreshold; } void CVoxelMap::TLikelihoodOptions::readFromStream( mrpt::serialization::CArchive& in) { - in >> decimation; + in >> decimation >> occupiedThreshold; } void CVoxelMap::internal_clear() diff --git a/samples/maps_voxelmap_from_tum_dataset/test.cpp b/samples/maps_voxelmap_from_tum_dataset/test.cpp index a7cc43afb4..459e760d46 100644 --- a/samples/maps_voxelmap_from_tum_dataset/test.cpp +++ b/samples/maps_voxelmap_from_tum_dataset/test.cpp @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -29,7 +30,8 @@ // TestVoxelMapFromTUM // ------------------------------------------------------ void TestVoxelMapFromTUM( - const std::string& datasetRawlogFile, const std::string& groundTruthFile) + const std::string& datasetRawlogFile, const std::string& groundTruthFile, + double VOXELMAP_RESOLUTION) { // To find out external image files: mrpt::io::setLazyLoadPathBase( @@ -63,13 +65,19 @@ void TestVoxelMapFromTUM( gtData(i, 1), gtData(i, 2), gtData(i, 3)))); } - mrpt::maps::CVoxelMap map(0.02); + // ---------------------- + // Voxel map + // ---------------------- - // map.insertionOptions.max_range = 5.0; // [m] + mrpt::maps::CVoxelMap map(VOXELMAP_RESOLUTION); + map.insertionOptions.max_range = 5.0; // [m] + map.insertionOptions.ray_trace_free_space = false; // only occupied + + // gui and demo app: mrpt::gui::CDisplayWindow3D win("VoxelMap demo", 640, 480); - auto gl_map = mrpt::opengl::COctoMapVoxels::Create(); + auto glVoxels = mrpt::opengl::COctoMapVoxels::Create(); auto glCamGroup = mrpt::opengl::CSetOfObjects::Create(); glCamGroup->insert(mrpt::opengl::stock_objects::CornerXYZSimple(0.3)); @@ -90,8 +98,6 @@ void TestVoxelMapFromTUM( scene->insert(glCamGroup); - map.getAsOctoMapVoxels(*gl_map); - // View occupied points: { auto mapPts = map.getOccupiedVoxels(); @@ -99,10 +105,7 @@ void TestVoxelMapFromTUM( 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); - scene->insert(gl_map); + scene->insert(glVoxels); win.unlockAccess3DScene(); } @@ -143,9 +146,13 @@ void TestVoxelMapFromTUM( 0.0_deg, -90.0_deg, 90.0_deg); // draw observation raw data: + mrpt::maps::CColouredPointsMap colPts; + mrpt::obs::T3DPointsProjectionParams pp; pp.takeIntoAccountSensorPoseOnRobot = true; - obs->unprojectInto(*glObsPts, pp); + obs->unprojectInto(colPts, pp); + + glObsPts->loadFromPointsMap(&colPts); if (!glCamFrustrumDone) { @@ -158,8 +165,17 @@ void TestVoxelMapFromTUM( } // update the voxel map: + colPts.changeCoordinatesReference(camPose); + map.insertPointCloudAsEndPoints(colPts); // Update the voxel map visualization: + static int decimUpdateViz = 0; + if (decimUpdateViz++ > 10) + { + decimUpdateViz = 0; + map.renderingOptions.generateFreeVoxels = false; + map.getAsOctoMapVoxels(*glVoxels); + } } } rawlogIndex++; @@ -173,51 +189,16 @@ void TestVoxelMapFromTUM( switch (k) { - case 'g': - case 'G': - gl_map->showGridLines(!gl_map->areGridLinesVisible()); - break; - case 'f': - case 'F': - gl_map->showVoxels( - mrpt::opengl::VOXEL_SET_FREESPACE, - !gl_map->areVoxelsVisible( - mrpt::opengl::VOXEL_SET_FREESPACE)); - break; - case 'o': - case 'O': - gl_map->showVoxels( - mrpt::opengl::VOXEL_SET_OCCUPIED, - !gl_map->areVoxelsVisible( - mrpt::opengl::VOXEL_SET_OCCUPIED)); - break; - case 'l': - case 'L': - gl_map->enableLights(!gl_map->areLightsEnabled()); - break; + // }; } win.addTextMessage( 5, 5, mrpt::format( - "Commands: 'f' (freespace=%s) | 'o' (occupied=%s) | 'l' " - "(lights=%s)", - gl_map->areVoxelsVisible(mrpt::opengl::VOXEL_SET_FREESPACE) - ? "YES" - : "NO", - gl_map->areVoxelsVisible(mrpt::opengl::VOXEL_SET_OCCUPIED) - ? "YES" - : "NO", - gl_map->areLightsEnabled() ? "YES" : "NO"), - 0 /*id*/); - - win.addTextMessage( - 5, 20, - mrpt::format( - "Timestamp: %s RawlogIndex: %zu", + "Timestamp: %s RawlogIndex: %zu ActiveVoxelCells: %zu", mrpt::system::dateTimeLocalToString(lastObsTim).c_str(), - rawlogIndex), + rawlogIndex, map.grid().activeCellsCount()), 1 /*id*/); win.repaint(); @@ -231,12 +212,12 @@ int main(int argc, char** argv) { try { - if (argc != 3) + if (argc != 4) throw std::invalid_argument( "Usage: PROGRAM " - ""); + " "); - TestVoxelMapFromTUM(argv[1], argv[2]); + TestVoxelMapFromTUM(argv[1], argv[2], std::stod(argv[3])); return 0; } catch (const std::exception& e)