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

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

3
corelib/include/rtabmap/core/Memory.h
View File

@@ -160,7 +160,7 @@ public:
const Signature * getLastWorkingSignature() const;
int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
bool labelSignature(int id, const std::string & label);
std::map<int, std::string> getAllLabels() const;
const std::map<int, std::string> & getAllLabels() const {return _labels;}
bool allNodesInWM() const {return _allNodesInWM;}
/**
@@ -328,6 +328,7 @@ private:
std::set<int> _stMem; // id
std::map<int, double> _workingMem; // id,age
std::map<int, Transform> _groundTruths;
std::map<int, std::string> _labels;
//Keypoint stuff
VWDictionary * _vwd;

3
corelib/include/rtabmap/core/Statistics.h
View File

@@ -192,6 +192,7 @@ public:
void setMapCorrection(const Transform & mapCorrection) {_mapCorrection = mapCorrection;}
void setLoopClosureTransform(const Transform & loopClosureTransform) {_loopClosureTransform = loopClosureTransform;}
void setLocalizationCovariance(const cv::Mat & covariance) {_localizationCovariance = covariance;}
void setLabels(const std::map<int, std::string> & labels) {_labels = labels;}
void setWeights(const std::map<int, int> & weights) {_weights = weights;}
void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
@@ -218,6 +219,7 @@ public:
const Transform & mapCorrection() const {return _mapCorrection;}
const Transform & loopClosureTransform() const {return _loopClosureTransform;}
const cv::Mat & localizationCovariance() const {return _localizationCovariance;}
const std::map<int, std::string> & labels() const {return _labels;}
const std::map<int, int> & weights() const {return _weights;}
const std::map<int, float> & posterior() const {return _posterior;}
const std::map<int, float> & likelihood() const {return _likelihood;}
@@ -248,6 +250,7 @@ private:
Transform _loopClosureTransform;
cv::Mat _localizationCovariance;
std::map<int, std::string> _labels;
std::map<int, int> _weights;
std::map<int, float> _posterior;
std::map<int, float> _likelihood;

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())
{