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

Bump version 0.20.22. Refactored RtabmapThread commands handling. MainWindow/RtabmapThread/Statistics/UVariant: added new option to republish missing data on GUI side. MainWindow: fixed 1 sec lag when waypoints are used. Parameters: added Rtabmap/MaxRepublished (moved from rtabmap_ros). OccupancyGrid: fixed ground cells ignored if there are empty cells, also don't add pose in addedNodes if corresponding node was not in cache.

Browse Source
This commit is contained in:
matlabbe
2022-10-28 12:25:48 -07:00
parent 8e30c1c812
commit 1297714271
18 changed files with 1021 additions and 610 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
guilib/src/MainWindow.cpp
View File

@@ -1997,6 +1997,33 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
}
// Add data
for(std::map<int, Signature>::const_iterator iter = stat.getSignaturesData().begin();
iter!=stat.getSignaturesData().end();
++iter)
{
if(signature.id() != iter->first &&
(!_cachedSignatures.contains(iter->first) ||
(_cachedSignatures.value(iter->first).sensorData().imageCompressed().empty() && !iter->second.sensorData().imageCompressed().empty())))
{
_cachedSignatures.insert(iter->first, iter->second);
_cachedMemoryUsage += iter->second.sensorData().getMemoryUsed();
unsigned int count = 0;
if(!iter->second.getWords3().empty())
{
for(std::multimap<int, int>::const_iterator jter=iter->second.getWords().upper_bound(-1); jter!=iter->second.getWords().end(); ++jter)
{
if(util3d::isFinite(iter->second.getWords3()[jter->second]))
{
++count;
}
}
}
_cachedWordsCount.insert(std::make_pair(iter->first, (float)count));
UINFO("Added %d node data to cache", iter->first);
}
}
// For intermediate empty nodes, keep latest image shown
if(signature.getWeight() >= 0)
{
@@ -2573,6 +2600,33 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_cachedMemoryUsage += s.sensorData().getMemoryUsed();
}
// Check missing cache
if(stat.getSignaturesData().size() <= 1)
{
if(_preferencesDialog->isMissingCacheRepublished() &&
_preferencesDialog->isImagesKept() &&
atoi(_preferencesDialog->getParameter(Parameters::kRtabmapMaxRepublished()).c_str()) > 0)
{
std::vector<int> missingIds;
for(std::map<int, Transform>::const_iterator iter=stat.poses().begin(); iter!=stat.poses().end(); ++iter)
{
QMap<int, Signature>::iterator ster = _cachedSignatures.find(iter->first);
if(ster == _cachedSignatures.end() ||
(ster.value().getWeight() >=0 && // ignore intermediate nodes
ster.value().sensorData().imageCompressed().empty() &&
ster.value().sensorData().depthOrRightCompressed().empty() &&
ster.value().sensorData().laserScanCompressed().empty()))
{
missingIds.push_back(iter->first);
}
}
if(!missingIds.empty())
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdRepublishData, UVariant(missingIds)));
}
}
}
UDEBUG("time= %d ms (update cache)", time.restart());
}
else if(!stat.extended() && stat.loopClosureId()>0)
@@ -4651,8 +4705,7 @@ void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathE
else if(event.getPoses().empty() && _waypoints.size())
{
// resend the same goal
uSleep(1000);
this->postGoal(_waypoints.at(_waypointsIndex % _waypoints.size()));
QTimer::singleShot(1000, this, SLOT(postGoal()));
}
}
@@ -7045,6 +7098,14 @@ void MainWindow::sendWaypoints()
}
}
void MainWindow::postGoal()
{
if(!_waypoints.isEmpty())
{
postGoal(_waypoints.at(_waypointsIndex % _waypoints.size()));
}
}
void MainWindow::postGoal(const QString & goal)
{
if(!goal.isEmpty())