i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 10:00:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

MainWindow: added timing stats on global map creation. Updated util3d::occupancy2DFromLaserScan() and util3d::create2DMap() interfaces (hit/noHit scans in opencv matrix format). Parameter default: Grid/ProjRayTracing=true, Grid/NormalK=true

Browse Source
This commit is contained in:
matlabbe
2017-04-06 12:27:12 -04:00
parent e415dd8504
commit 1d8952867d
13 changed files with 386 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

110
guilib/src/MainWindow.cpp
View File

@@ -586,6 +586,13 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->statsToolBox->updateStat("GUI/Refresh odom/ms", false);
_ui->statsToolBox->updateStat("GUI/RGB-D cloud/ms", false);
_ui->statsToolBox->updateStat("GUI/Graph Update/ms", false);
#ifdef RTABMAP_OCTOMAP
_ui->statsToolBox->updateStat("GUI/Octomap Update/ms", false);
_ui->statsToolBox->updateStat("GUI/Octomap Rendering/ms", false);
#endif
_ui->statsToolBox->updateStat("GUI/Grid Update/ms", false);
_ui->statsToolBox->updateStat("GUI/Grid Rendering/ms", false);
_ui->statsToolBox->updateStat("GUI/Refresh stats/ms", false);
_ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", false);
_ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", false);
@@ -1429,12 +1436,18 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
signature = stat.getSignatures().at(stat.refImageId());
signature.sensorData().uncompressData(); // make sure data are uncompressed
if(!smallMovement &&
uStr2Bool(_preferencesDialog->getParameter(Parameters::kMemIncrementalMemory())) &&
if( uStr2Bool(_preferencesDialog->getParameter(Parameters::kMemIncrementalMemory())) &&
signature.getWeight()>=0) // ignore intermediate nodes for the cache
{
_cachedSignatures.insert(signature.id(), signature);
_cachedMemoryUsage += signature.sensorData().getMemoryUsed();
if(smallMovement)
{
_cachedSignatures.insert(-1, signature); // negative means temporary
}
else
{
_cachedSignatures.insert(signature.id(), signature);
_cachedMemoryUsage += signature.sensorData().getMemoryUsed();
}
}
}
@@ -1675,8 +1688,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
//======================
// RGB-D Mapping stuff
//======================
UTimer timerVis;
// update clouds
if(stat.poses().size())
{
@@ -1725,19 +1736,40 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
}
if(_cachedSignatures.contains(-1))
{
if(poses.find(stat.refImageId())!=poses.end())
{
poses.insert(std::make_pair(-1, poses.at(stat.refImageId())));
poses.erase(stat.refImageId());
}
if(groundTruth.find(stat.refImageId())!=groundTruth.end())
{
groundTruth.insert(std::make_pair(-1, groundTruth.at(stat.refImageId())));
groundTruth.erase(stat.refImageId());
}
}
std::map<std::string, float> updateCloudSats;
updateMapCloud(
poses,
stat.constraints(),
mapIds,
labels,
groundTruth);
groundTruth,
false,
&updateCloudSats);
_odometryReceived = false;
_odometryCorrection = groundTruthOffset * stat.mapCorrection();
UDEBUG("time= %d ms", time.restart());
_ui->statsToolBox->updateStat("GUI/RGB-D cloud/ms", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(timerVis.elapsed()*1000.0f), _preferencesDialog->isCacheSavedInFigures());
for(std::map<std::string, float>::iterator iter=updateCloudSats.begin(); iter!=updateCloudSats.end(); ++iter)
{
_ui->statsToolBox->updateStat(iter->first.c_str(), _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), int(iter->second), _preferencesDialog->isCacheSavedInFigures());
}
// loop closure view
if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0) &&
@@ -1783,6 +1815,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
UDEBUG("");
_cachedSignatures.remove(-1); // remove tmp negative ids
// keep only compressed data in cache
if(_cachedSignatures.contains(stat.refImageId()))
{
@@ -1796,7 +1830,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_cachedMemoryUsage += s.sensorData().getMemoryUsed();
}
UDEBUG("");
}
else if(!stat.extended() && stat.loopClosureId()>0)
@@ -1843,17 +1876,21 @@ void MainWindow::updateMapCloud(
const std::map<int, int> & mapIdsIn,
const std::map<int, std::string> & labels,
const std::map<int, Transform> & groundTruths, // ground truth should contain only valid transforms
bool verboseProgress)
bool verboseProgress,
std::map<std::string, float> * stats)
{
UTimer timer;
UDEBUG("posesIn=%d constraints=%d mapIdsIn=%d labelsIn=%d",
(int)posesIn.size(), (int)constraints.size(), (int)mapIdsIn.size(), (int)labels.size());
if(posesIn.size())
{
_currentPosesMap = posesIn;
_currentPosesMap.erase(-1); // don't keep -1 if it is there
_currentLinksMap = constraints;
_currentMapIds = mapIdsIn;
_currentLabels = labels;
_currentGTPosesMap = groundTruths;
_currentGTPosesMap.erase(-1);
if(_state != kMonitoring && _state != kDetecting)
{
_ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
@@ -1902,12 +1939,12 @@ void MainWindow::updateMapCloud(
}
_ui->widget_mapVisibility->setMap(posesIn, posesMask);
if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
if(groundTruths.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
{
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
if(gtIter!=_currentGTPosesMap.end())
std::map<int, Transform>::const_iterator gtIter = groundTruths.find(iter->first);
if(gtIter!=groundTruths.end())
{
iter->second = gtIter->second;
}
@@ -1932,6 +1969,12 @@ void MainWindow::updateMapCloud(
{
std::string cloudName = uFormat("cloud%d", iter->first);
if(iter->first < 0)
{
viewerClouds.remove(cloudName);
_cloudViewer->removeCloud(cloudName);
}
// 3d point cloud
bool update3dCloud = _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0);
if(update3dCloud)
@@ -1971,6 +2014,11 @@ void MainWindow::updateMapCloud(
// 2d point cloud
std::string scanName = uFormat("scan%d", iter->first);
if(iter->first < 0)
{
viewerClouds.remove(scanName);
_cloudViewer->removeCloud(scanName);
}
if(_cloudViewer->isVisible() && _preferencesDialog->isScansShown(0))
{
if(viewerClouds.contains(scanName))
@@ -2004,6 +2052,12 @@ void MainWindow::updateMapCloud(
}
// occupancy grids
if(iter->first < 0)
{
_gridLocalMaps.erase(iter->first);
_gridViewPoints.erase(iter->first);
}
bool updateGridMap =
((_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) ||
(_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())) &&
@@ -2061,6 +2115,11 @@ void MainWindow::updateMapCloud(
// 3d features
std::string featuresName = uFormat("features%d", iter->first);
if(iter->first < 0)
{
viewerClouds.remove(featuresName);
_cloudViewer->removeCloud(featuresName);
}
if(_cloudViewer->isVisible() && _preferencesDialog->isFeaturesShown(0))
{
if(viewerClouds.contains(featuresName))
@@ -2129,6 +2188,10 @@ void MainWindow::updateMapCloud(
}
UDEBUG("");
if(stats)
{
stats->insert(std::make_pair("GUI/RGB-D cloud/ms", (float)timer.restart()*1000.0f));
}
// update 3D graphes (show all poses)
_cloudViewer->removeAllGraphs();
@@ -2268,6 +2331,10 @@ void MainWindow::updateMapCloud(
}
UDEBUG("");
if(stats)
{
stats->insert(std::make_pair("GUI/Graph Update/ms", (float)timer.restart()*1000.0f));
}
#ifdef RTABMAP_OCTOMAP
_cloudViewer->removeOctomap();
@@ -2279,6 +2346,10 @@ void MainWindow::updateMapCloud(
_octomap->update(poses);
UINFO("Octomap update time = %fs", time.ticks());
}
if(stats)
{
stats->insert(std::make_pair("GUI/Octomap Update/ms", (float)timer.restart()*1000.0f));
}
if(_preferencesDialog->isOctomapShown())
{
UDEBUG("");
@@ -2294,11 +2365,16 @@ void MainWindow::updateMapCloud(
if(obstacles->size())
{
_cloudViewer->addCloud("octomap_cloud", cloud);
_cloudViewer->setCloudPointSize("octomap_cloud", _preferencesDialog->getOctomapPointSize());
}
}
UINFO("Octomap show 3d map time = %fs", time.ticks());
}
UDEBUG("");
if(stats)
{
stats->insert(std::make_pair("GUI/Octomap Rendering/ms", (float)timer.restart()*1000.0f));
}
#endif
// Update occupancy grid map in 3D map view and graph view
@@ -2326,6 +2402,10 @@ void MainWindow::updateMapCloud(
if(_preferencesDialog->isGridMapIncremental())
{
_occupancyGrid->update(poses, 0, _preferencesDialog->getGridMapFootprintRadius());
if(stats)
{
stats->insert(std::make_pair("GUI/Grid Update/ms", (float)timer.restart()*1000.0f));
}
map8S = _occupancyGrid->getMap(xMin, yMin);
}
else
@@ -2359,6 +2439,10 @@ void MainWindow::updateMapCloud(
_ui->graphicsView_graphView->update();
UDEBUG("");
if(stats)
{
stats->insert(std::make_pair("GUI/Grid Rendering/ms", (float)timer.restart()*1000.0f));
}
if(!_preferencesDialog->getGridMapShown())
{