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

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

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

@@ -76,6 +76,7 @@ public:
bool ignoreLoopIds = false,
double * dbAccessTime = 0) const;
void deleteLastLocation();
void deleteLocation(int locationId);
void rejectLastLoopClosure();
//getters

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

@@ -87,6 +87,7 @@ public:
void parseParameters(const ParametersMap & parameters);
void setWorkingDirectory(std::string path);
void deleteLastLocation();
void deleteLocation(int locationId); // Only nodes in STM can be deleted
void rejectLastLoopClosure();
void adjustLikelihood(std::map<int, float> & likelihood) const;

31
corelib/src/Memory.cpp
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);
}
}

10
corelib/src/Rtabmap.cpp
View File

@@ -1209,13 +1209,20 @@ void Rtabmap::setWorkingDirectory(std::string path)
void Rtabmap::deleteLastLocation()
{
if(_memory)
{
_memory->deleteLastLocation();
}
}
void Rtabmap::deleteLocation(int locationId)
{
if(_memory)
{
_memory->deleteLocation(locationId);
}
}
void Rtabmap::rejectLastLoopClosure()
{
UDEBUG("_lcHypothesisId=%d", _lcHypothesisId);
@@ -1236,7 +1243,6 @@ void Rtabmap::rejectLastLoopClosure()
void Rtabmap::process(const cv::Mat & image, int id, std::multimap<int, cv::KeyPoint> * words)
{
this->process(Image(image, id), words);
}