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

Fixed labels not correctly shown. Fixed planner not sending next goals when memory management is disabled.

Browse Source
This commit is contained in:
matlabbe
2018-11-19 18:00:24 -05:00
parent bf2a9db5e4
commit 283c1df00c
5 changed files with 55 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
corelib/src/Memory.cpp
View File

@@ -249,6 +249,10 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
}
}
// Get labels
UDEBUG("Get labels");
_dbDriver->getAllLabels(_labels);
UDEBUG("Check if all nodes are in Working Memory");
for(std::map<int, Signature*>::iterator iter=_signatures.begin(); iter!=_signatures.end() && _allNodesInWM; ++iter)
{
@@ -923,6 +927,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
{
UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
signature->setLabel(tag);
_labels.insert(std::make_pair(signature->id(), tag));
}
}
}
@@ -935,6 +940,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
{
UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
signature->setLabel(tag);
_labels.insert(std::make_pair(signature->id(), tag));
}
}
@@ -1593,6 +1599,7 @@ void Memory::clear()
_rectStereoCameraModel = StereoCameraModel();
_odomMaxInf.clear();
_groundTruths.clear();
_labels.clear();
_allNodesInWM = true;
if(_dbDriver)
@@ -2286,6 +2293,7 @@ bool Memory::labelSignature(int id, const std::string & label)
Signature * s = this->_getSignature(id);
if(s)
{
uInsert(_labels, std::make_pair(s->id(), label));
s->setLabel(label);
_linksChanged = s->isSaved(); // HACK to get label updated in Localization mode
UWARN("Label \"%s\" set to node %d", label.c_str(), id);
@@ -2299,6 +2307,7 @@ bool Memory::labelSignature(int id, const std::string & label)
_dbDriver->loadSignatures(ids,signatures);
if(signatures.size())
{
uInsert(_labels, std::make_pair(signatures.front()->id(), label));
signatures.front()->setLabel(label);
UWARN("Label \"%s\" set to node %d", label.c_str(), id);
_dbDriver->asyncSave(signatures.front()); // move it again to trash
@@ -2317,23 +2326,6 @@ bool Memory::labelSignature(int id, const std::string & label)
return false;
}
std::map<int, std::string> Memory::getAllLabels() const
{
std::map<int, std::string> labels;
for(std::map<int, Signature*>::const_iterator iter = _signatures.begin(); iter!=_signatures.end(); ++iter)
{
if(!iter->second->getLabel().empty())
{
labels.insert(std::make_pair(iter->first, iter->second->getLabel()));
}
}
if(_dbDriver)
{
_dbDriver->getAllLabels(labels);
}
return labels;
}
bool Memory::setUserData(int id, const cv::Mat & data)
{
Signature * s = this->_getSignature(id);

73
corelib/src/Rtabmap.cpp
View File

@@ -1807,54 +1807,57 @@ bool Rtabmap::process(
timeGetNeighborsSpaceDb);
}
}
//============================================================
// RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve
//============================================================
std::list<int> retrievalLocalIds;
if(_rgbdSlamMode)
//============================================================
// RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve
//============================================================
std::list<int> retrievalLocalIds;
if(_rgbdSlamMode)
{
// Priority on locations on the planned path
if(_path.size())
{
// Priority on locations on the planned path
if(_path.size())
updateGoalIndex();
float distanceSoFar = 0.0f;
// immunize all nodes after current node and
// retrieve nodes after current node in the maximum radius from the current node
for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
{
updateGoalIndex();
float distanceSoFar = 0.0f;
// immunize all nodes after current node and
// retrieve nodes after current node in the maximum radius from the current node
for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i)
if(_localRadius > 0.0f && i != _pathCurrentIndex)
{
if(_localRadius > 0.0f && i != _pathCurrentIndex)
{
distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
}
distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
}
if(distanceSoFar <= _localRadius)
if(distanceSoFar <= _localRadius)
{
if(_memory->getSignature(_path[i].first) != 0)
{
if(_memory->getSignature(_path[i].first) != 0)
if(immunizedLocations.insert(_path[i].first).second)
{
if(immunizedLocations.insert(_path[i].first).second)
{
++immunizedLocally;
}
UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
}
else if(retrievalLocalIds.size() < _maxLocalRetrieved)
{
UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
retrievalLocalIds.push_back(_path[i].first);
// retrieved locations are automatically immunized
++immunizedLocally;
}
UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
}
else
else if(retrievalLocalIds.size() < _maxLocalRetrieved)
{
UDEBUG("Stop on node %d (dist=%fm > %fm)",
_path[i].first, distanceSoFar, _localRadius);
break;
UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar);
retrievalLocalIds.push_back(_path[i].first);
// retrieved locations are automatically immunized
}
}
else
{
UDEBUG("Stop on node %d (dist=%fm > %fm)",
_path[i].first, distanceSoFar, _localRadius);
break;
}
}
}
if(!(_memory->allNodesInWM() && maxLocalLocationsImmunized == 0))
{
// immunize the path from the nearest local location to the current location
if(immunizedLocally < maxLocalLocationsImmunized &&
_memory->isIncremental()) // Can only work in mapping mode
@@ -2739,6 +2742,8 @@ bool Rtabmap::process(
}
}
statistics_.setLabels(_memory->getAllLabels());
// Path
if(_path.size())
{