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

Planning update: remove virtual links up to current path index

Browse Source
This commit is contained in:
matlabbe
2015-05-11 15:48:27 -04:00
parent c0386be098
commit 04bd596705
3 changed files with 44 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -84,6 +84,7 @@ public:
bool addLink(int to, int from, const Transform & transform, Link::Type type, float rotVariance, float transVariance);
void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
void removeAllVirtualLinks();
void removeVirtualLinks(int signatureId);
std::map<int, int> getNeighborsId(
int signatureId,
int maxGraphDepth,

52
corelib/src/Memory.cpp
View File

@@ -1625,24 +1625,10 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
}
else
{
//make sure that virtual links are removed
const std::map<int, Link> & links = s->getLinks();
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kVirtualClosure)
{
Signature * sTo = this->_getSignature(iter->first);
if(sTo)
{
sTo->removeLink(s->id());
}
else
{
UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id());
}
}
}
s->removeVirtualLinks();
// Make sure that virtual links are removed.
// It should be called before the signature is
// removed from _signatures below.
removeVirtualLinks(s->id());
}
this->disableWordsRef(s->id());
@@ -2729,6 +2715,36 @@ void Memory::removeAllVirtualLinks()
}
}
void Memory::removeVirtualLinks(int signatureId)
{
UDEBUG("");
Signature * s = this->_getSignature(signatureId);
if(s)
{
const std::map<int, Link> & links = s->getLinks();
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kVirtualClosure)
{
Signature * sTo = this->_getSignature(iter->first);
if(sTo)
{
sTo->removeLink(s->id());
}
else
{
UERROR("Link %d of %d not in WM/STM?!?", iter->first, s->id());
}
}
}
s->removeVirtualLinks();
}
else
{
UERROR("Signature %d not in WM/STM?!?", signatureId);
}
}
void Memory::dumpMemory(std::string directory) const
{
UINFO("Dumping memory to directory \"%s\"", directory.c_str());

9
corelib/src/Rtabmap.cpp
View File

@@ -3131,6 +3131,15 @@ void Rtabmap::updateGoalIndex()
if( _memory && _path.size())
{
// remove all previous virtual links
for(unsigned int i=0; i<_pathCurrentIndex && i<_path.size(); ++i)
{
if(_memory->getSignature(_path[i].first))
{
_memory->removeVirtualLinks(_path[i].first);
}
}
// Make sure the next signatures on the path are linked together
float distanceSoFar = 0.0f;
for(unsigned int i=_pathCurrentIndex;