diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index ea681700..4d43b650 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -76,6 +76,7 @@ public: bool ignoreLoopIds = false, double * dbAccessTime = 0) const; void deleteLastLocation(); + void deleteLocation(int locationId); void rejectLastLoopClosure(); //getters diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index b0413d51..0a7cc607 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -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 & likelihood) const; diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index b9faaf28..9658b920 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -1413,31 +1413,44 @@ void Memory::deleteLastLocation() Signature * lastSignature = _lastSignature; if(lastSignature) { - UDEBUG("deleting last location %d", lastSignature->id()); - const std::set & 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 & neighbors = location->getNeighbors(); for(std::set::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 & child = lastSignature->getChildLoopClosureIds(); + const std::set & child = location->getChildLoopClosureIds(); for(std::set::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); } } diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 1b632a22..2da4177b 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -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 * words) { - this->process(Image(image, id), words); }