i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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, bool ignoreLoopIds = false,
double * dbAccessTime = 0) const; double * dbAccessTime = 0) const;
void deleteLastLocation(); void deleteLastLocation();
void deleteLocation(int locationId);
void rejectLastLoopClosure(); void rejectLastLoopClosure();
//getters //getters

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

@@ -87,6 +87,7 @@ public:
void parseParameters(const ParametersMap & parameters); void parseParameters(const ParametersMap & parameters);
void setWorkingDirectory(std::string path); void setWorkingDirectory(std::string path);
void deleteLastLocation(); void deleteLastLocation();
void deleteLocation(int locationId); // Only nodes in STM can be deleted
void rejectLastLoopClosure(); void rejectLastLoopClosure();
void adjustLikelihood(std::map<int, float> & likelihood) const; 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; Signature * lastSignature = _lastSignature;
if(lastSignature) if(lastSignature)
{ {
UDEBUG("deleting last location %d", lastSignature->id()); this->deleteLocation(lastSignature->id());
const std::set<int> & neighbors = lastSignature->getNeighbors(); }
}
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) for(std::set<int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{ {
Signature * s = _getSignature(*iter); Signature * s = _getSignature(*iter);
if(s) if(s)
{ {
s->removeNeighbor(lastSignature->id()); s->removeNeighbor(location->id());
_lastSignature = s; 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) for(std::set<int>::const_iterator iter=child.begin(); iter!=child.end(); ++iter)
{ {
Signature * s = _getSignature(*iter); Signature * s = _getSignature(*iter);
if(s) if(s)
{ {
s->removeLoopClosureId(lastSignature->id()); s->removeLoopClosureId(location->id());
s->setWeight(s->getWeight() + lastSignature->getWeight()); 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() void Rtabmap::deleteLastLocation()
{ {
if(_memory) if(_memory)
{ {
_memory->deleteLastLocation(); _memory->deleteLastLocation();
} }
} }
void Rtabmap::deleteLocation(int locationId)
{
if(_memory)
{
_memory->deleteLocation(locationId);
}
}
void Rtabmap::rejectLastLoopClosure() void Rtabmap::rejectLastLoopClosure()
{ {
UDEBUG("_lcHypothesisId=%d", _lcHypothesisId); 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) void Rtabmap::process(const cv::Mat & image, int id, std::multimap<int, cv::KeyPoint> * words)
{ {
this->process(Image(image, id), words); this->process(Image(image, id), words);
} }