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

Only remove compressed raw data when publish data is false (keep features/occupancy grid)

Browse Source
This commit is contained in:
matlabbe
2023-08-11 16:31:26 -07:00
parent 6395487b58
commit ba5579f063
1 changed files with 52 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

126
corelib/src/Rtabmap.cpp
View File

@@ -4081,15 +4081,16 @@ bool Rtabmap::process(
ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation); ULOGGER_INFO("Time creating stats = %f...", timeStatsCreation);
} }
Signature lastSignatureData(signature->id()); Signature lastSignatureData = *signature;
Transform lastSignatureLocalizedPose; Transform lastSignatureLocalizedPose;
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end()) if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end())
{ {
lastSignatureLocalizedPose = _optimizedPoses.at(signature->id()); lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
} }
if(_publishLastSignatureData) if(!_publishLastSignatureData)
{ {
lastSignatureData = *signature; lastSignatureData.sensorData().clearCompressedData();
lastSignatureData.sensorData().clearRawData();
} }
if(!_rawDataKept) if(!_rawDataKept)
{ {
@@ -4372,96 +4373,73 @@ bool Rtabmap::process(
poses = _optimizedPoses; poses = _optimizedPoses;
constraints = _constraints; constraints = _constraints;
} }
UDEBUG(""); UINFO("Adding data %d [%d] (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.mapId(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
if(_publishLastSignatureData)
{
UINFO("Adding data %d [%d] (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.mapId(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
statistics_.addSignatureData(lastSignatureData); statistics_.addSignatureData(lastSignatureData);
if(_nodesToRepublish.size()) if(_nodesToRepublish.size())
{
std::multimap<int, int> missingIds;
// priority to loopId
int tmpId = loopId>0?loopId:_highestHypothesis.first;
if(tmpId>0 && _nodesToRepublish.find(tmpId) != _nodesToRepublish.end())
{ {
std::multimap<int, int> missingIds; missingIds.insert(std::make_pair(-1, tmpId));
}
// priority to loopId if(!_lastLocalizationPose.isNull())
int tmpId = loopId>0?loopId:_highestHypothesis.first; {
if(tmpId>0 && _nodesToRepublish.find(tmpId) != _nodesToRepublish.end()) // Republish data from closest nodes of the current localization
std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
int id = rtabmap::graph::findNearestNode(nodesOnly, _lastLocalizationPose);
if(id>0)
{ {
missingIds.insert(std::make_pair(-1, tmpId)); std::map<int, int> ids = _memory->getNeighborsId(id, 0, 0, true, false, true);
} for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
if(!_lastLocalizationPose.isNull())
{
// Republish data from closest nodes of the current localization
std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
int id = rtabmap::graph::findNearestNode(nodesOnly, _lastLocalizationPose);
if(id>0)
{ {
std::map<int, int> ids = _memory->getNeighborsId(id, 0, 0, true, false, true); if(iter->first != loopId &&
for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter) _nodesToRepublish.find(iter->first) != _nodesToRepublish.end())
{ {
if(iter->first != loopId && missingIds.insert(std::make_pair(iter->second, iter->first));
_nodesToRepublish.find(iter->first) != _nodesToRepublish.end())
{
missingIds.insert(std::make_pair(iter->second, iter->first));
}
} }
}
if(_nodesToRepublish.size() != missingIds.size()) if(_nodesToRepublish.size() != missingIds.size())
{
// remove requested nodes not anymore in the graph
for(std::set<int>::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();)
{ {
// remove requested nodes not anymore in the graph if(ids.find(*iter) == ids.end())
for(std::set<int>::iterator iter=_nodesToRepublish.begin(); iter!=_nodesToRepublish.end();)
{ {
if(ids.find(*iter) == ids.end()) iter = _nodesToRepublish.erase(iter);
{ }
iter = _nodesToRepublish.erase(iter); else
} {
else ++iter;
{
++iter;
}
} }
} }
} }
} }
}
int loaded = 0; int loaded = 0;
std::stringstream stream; std::stringstream stream;
for(std::multimap<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<(int)_maxRepublished; ++iter) for(std::multimap<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<(int)_maxRepublished; ++iter)
{
statistics_.addSignatureData(getSignatureCopy(iter->second, true, true, true, true, true, true));
_nodesToRepublish.erase(iter->second);
++loaded;
stream << iter->second << " ";
}
if(loaded)
{
UWARN("Republishing data of requested node(s) %s(%s=%d)",
stream.str().c_str(),
Parameters::kRtabmapMaxRepublished().c_str(),
_maxRepublished);
}
}
}
else
{
// only copy node info
Signature nodeInfo(
lastSignatureData.id(),
lastSignatureData.mapId(),
lastSignatureData.getWeight(),
lastSignatureData.getStamp(),
lastSignatureData.getLabel(),
lastSignatureData.getPose(),
lastSignatureData.getGroundTruthPose());
const std::vector<float> & v = lastSignatureData.getVelocity();
if(v.size() == 6)
{ {
nodeInfo.setVelocity(v[0], v[1], v[2], v[3], v[4], v[5]); statistics_.addSignatureData(getSignatureCopy(iter->second, true, true, true, true, true, true));
_nodesToRepublish.erase(iter->second);
++loaded;
stream << iter->second << " ";
}
if(loaded)
{
UWARN("Republishing data of requested node(s) %s(%s=%d)",
stream.str().c_str(),
Parameters::kRtabmapMaxRepublished().c_str(),
_maxRepublished);
} }
nodeInfo.sensorData().setGPS(lastSignatureData.sensorData().gps());
nodeInfo.sensorData().setEnvSensors(lastSignatureData.sensorData().envSensors());
statistics_.addSignatureData(nodeInfo);
} }
UDEBUG(""); UDEBUG("");
localGraphSize = (int)poses.size(); localGraphSize = (int)poses.size();
if(!lastSignatureLocalizedPose.isNull()) if(!lastSignatureLocalizedPose.isNull())