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

core: added rtabmap::deleteLocation(id)

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@920 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2013-08-20 20:37:24 +00:00
parent 21bd27586f
commit 97c70d394e
4 changed files with 32 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+22 -9
View File
@@ -1413,31 +1413,44 @@ void Memory::deleteLastLocation()
Signature * lastSignature = _lastSignature;
if(lastSignature)
{
UDEBUG("deleting last location %d", lastSignature->id());
const std::set<int> & neighbors = lastSignature->getNeighbors();
this->deleteLocation(lastSignature->id());
}
}
void Memory::deleteLocation(int locationId)
{
UASSERT_MSG(this->isInSTM(locationId), "Deleting location outside the STM is not implemented!");
Signature * location = _getSignature(locationId);
if(location)
{
UDEBUG("deleting location %d", location->id());
const std::set<int> & neighbors = location->getNeighbors();
for(std::set<int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
Signature * s = _getSignature(*iter);
if(s)
{
s->removeNeighbor(lastSignature->id());
_lastSignature = s;
s->removeNeighbor(location->id());
if(location == _lastSignature)
{
_lastSignature = s;
}
}
}
const std::set<int> & child = lastSignature->getChildLoopClosureIds();
const std::set<int> & child = location->getChildLoopClosureIds();
for(std::set<int>::const_iterator iter=child.begin(); iter!=child.end(); ++iter)
{
Signature * s = _getSignature(*iter);
if(s)
{
s->removeLoopClosureId(lastSignature->id());
s->setWeight(s->getWeight() + lastSignature->getWeight());
s->removeLoopClosureId(location->id());
s->setWeight(s->getWeight() + location->getWeight());
}
}
lastSignature->setWeight(0);
location->setWeight(0);
this->moveToTrash(lastSignature, false);
this->moveToTrash(location, false);
}
}