Skip to content

Commit

Permalink
progress voxelmap RGBD dataset demo
Browse files Browse the repository at this point in the history
  • Loading branch information
@jlblancoc
jlblancoc committed Oct 17, 2023
1 parent b920d2f commit 1ddce70
Show file tree
Hide file tree
Showing 3 changed files with 57 additions and 66 deletions.
13 changes: 10 additions & 3 deletions libs/maps/include/mrpt/maps/CVoxelMap.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,8 @@ class CVoxelMap : public CVoxelMapBase<int8_t>,
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);
Expand All @@ -113,14 +114,18 @@ class CVoxelMap : public CVoxelMapBase<int8_t>,
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);

/// 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;
Expand All @@ -132,9 +137,11 @@ class CVoxelMap : public CVoxelMapBase<int8_t>,
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 */
Expand Down Expand Up @@ -265,4 +272,4 @@ class CVoxelMap : public CVoxelMapBase<int8_t>,
mutable mrpt::maps::CSimplePointsMap::Ptr m_cachedOccupied;
};

} // namespace mrpt::maps
} // namespace mrpt::maps
25 changes: 14 additions & 11 deletions libs/maps/src/maps/CVoxelMap.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -308,6 +307,7 @@ void CVoxelMap::TRenderingOptions::writeToStream(

out << generateOccupiedVoxels << visibleOccupiedVoxels;
out << generateFreeVoxels << visibleFreeVoxels;
out << occupiedThreshold << freeThreshold;
}

void CVoxelMap::TRenderingOptions::readFromStream(
Expand All @@ -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);
}
Expand All @@ -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()
Expand Down
85 changes: 33 additions & 52 deletions samples/maps_voxelmap_from_tum_dataset/test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@

#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/io/lazy_load_path.h>
#include <mrpt/maps/CColouredPointsMap.h>
#include <mrpt/maps/CVoxelMap.h>
#include <mrpt/math/CMatrixDynamic.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
Expand All @@ -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(
Expand Down Expand Up @@ -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));
Expand All @@ -90,19 +98,14 @@ void TestVoxelMapFromTUM(

scene->insert(glCamGroup);

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);
scene->insert(gl_map);
scene->insert(glVoxels);

win.unlockAccess3DScene();
}
Expand Down Expand Up @@ -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)
{
Expand All @@ -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++;
Expand All @@ -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();
Expand All @@ -231,12 +212,12 @@ int main(int argc, char** argv)
{
try
{
if (argc != 3)
if (argc != 4)
throw std::invalid_argument(
"Usage: PROGRAM <PATH_TO_TUM_DATASET.rawlog> "
"<GROUND_TRUTH.txt>");
"<GROUND_TRUTH.txt> <VOXELMAP_RESOLUTION>");

TestVoxelMapFromTUM(argv[1], argv[2]);
TestVoxelMapFromTUM(argv[1], argv[2], std::stod(argv[3]));
return 0;
}
catch (const std::exception& e)
Expand Down

0 comments on commit 1ddce70

Please sign in to comment.