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

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); 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 updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
void removeAllVirtualLinks(); void removeAllVirtualLinks();
void removeVirtualLinks(int signatureId);
std::map<int, int> getNeighborsId( std::map<int, int> getNeighborsId(
int signatureId, int signatureId,
int maxGraphDepth, int maxGraphDepth,

52
corelib/src/Memory.cpp
View File

@@ -1625,24 +1625,10 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
} }
else else
{ {
//make sure that virtual links are removed // Make sure that virtual links are removed.
const std::map<int, Link> & links = s->getLinks(); // It should be called before the signature is
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter) // removed from _signatures below.
{ removeVirtualLinks(s->id());
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();
} }
this->disableWordsRef(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 void Memory::dumpMemory(std::string directory) const
{ {
UINFO("Dumping memory to directory \"%s\"", directory.c_str()); 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()) 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 // Make sure the next signatures on the path are linked together
float distanceSoFar = 0.0f; float distanceSoFar = 0.0f;
for(unsigned int i=_pathCurrentIndex; for(unsigned int i=_pathCurrentIndex;