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

Rtabmap: Added deleteLastLocation() and rejectLastLoopClosure() methods

Browse Source
This commit is contained in:
matlabbe
2018-08-29 12:29:11 -04:00
parent 956f07785b
commit 30290c36d7
3 changed files with 102 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -153,7 +153,8 @@ public:
void parseParameters(const ParametersMap & parameters);
const ParametersMap & getParameters() const {return _parameters;}
void setWorkingDirectory(std::string path);
void rejectLoopClosure(int oldId, int newId);
void rejectLastLoopClosure();
void deleteLastLocation();
void setOptimizedPoses(const std::map<int, Transform> & poses);
void get3DMap(std::map<int, Signature> & signatures,
std::map<int, Transform> & poses,

2
corelib/src/Memory.cpp
View File

@@ -2126,7 +2126,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
_lastGlobalLoopClosureId = 0;
}
if( (_notLinkedNodesKeptInDb || keepLinkedToGraph) &&
if( (_notLinkedNodesKeptInDb || keepLinkedToGraph || s->isSaved()) &&
_dbDriver &&
s->id()>0 &&
(_incrementalMemory || s->isSaved()))

108
corelib/src/Rtabmap.cpp
View File

@@ -3017,21 +3017,111 @@ void Rtabmap::setWorkingDirectory(std::string path)
}
}
void Rtabmap::rejectLoopClosure(int oldId, int newId)
void Rtabmap::rejectLastLoopClosure()
{
UDEBUG("_loopClosureHypothesis.first=%d", _loopClosureHypothesis.first);
if(_loopClosureHypothesis.first)
if(_memory && _memory->getStMem().find(getLastLocationId())!=_memory->getStMem().end())
{
_loopClosureHypothesis.first = 0;
if(_memory)
std::map<int, Link> links = _memory->getLinks(getLastLocationId(), false);
bool linksRemoved = false;
for(std::map<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
{
_memory->removeLink(oldId, newId);
if(iter->second.type() == Link::kGlobalClosure ||
iter->second.type() == Link::kLocalSpaceClosure ||
iter->second.type() == Link::kLocalTimeClosure ||
iter->second.type() == Link::kUserClosure)
{
_memory->removeLink(iter->second.from(), iter->second.to());
std::multimap<int, Link>::iterator jter = graph::findLink(_constraints, iter->second.from(), iter->second.to(), true);
if(jter!=_constraints.end())
{
_constraints.erase(jter);
// second time if link is also inverted
jter = graph::findLink(_constraints, iter->second.from(), iter->second.to(), true);
if(jter!=_constraints.end())
{
_constraints.erase(jter);
}
}
linksRemoved = true;
}
}
if(uContains(statistics_.data(), rtabmap::Statistics::kLoopRejectedHypothesis()))
if(linksRemoved)
{
statistics_.addStatistic(rtabmap::Statistics::kLoopRejectedHypothesis(), 1.0f);
_loopClosureHypothesis.first = 0;
// we have to re-optimize the graph without the rejected links
if(_memory->isIncremental() && _optimizedPoses.size())
{
UINFO("Update graph");
std::map<int, Transform> poses = _optimizedPoses;
std::multimap<int, Link> constraints;
cv::Mat covariance;
optimizeCurrentMap(getLastLocationId(), false, poses, covariance, &constraints);
if(poses.empty())
{
UWARN("Graph optimization failed after removing loop closure links from last location!");
}
else
{
UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
_optimizedPoses = poses;
_constraints = constraints;
_mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()) * _memory->getLastWorkingSignature()->getPose().inverse();
}
}
}
}
}
void Rtabmap::deleteLastLocation()
{
if(_memory && _memory->getStMem().size())
{
int lastId = *_memory->getStMem().rbegin();
_memory->deleteLocation(lastId);
// we have to re-optimize the graph without the deleted location
if(_memory->isIncremental() && _optimizedPoses.size())
{
UINFO("Update graph");
_optimizedPoses.erase(lastId);
std::map<int, Transform> poses = _optimizedPoses;
//remove all constraints with last localization id
for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end();)
{
if(iter->second.from() == lastId || iter->second.to() == lastId)
{
_constraints.erase(iter++);
}
else
{
++iter;
}
}
if(poses.empty())
{
_mapCorrection.setIdentity();
}
else
{
std::multimap<int, Link> constraints;
cv::Mat covariance;
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, poses, covariance, &constraints);
if(poses.empty())
{
UWARN("Graph optimization failed after deleting the last location!");
}
else
{
_optimizedPoses = poses;
_constraints = constraints;
_mapCorrection = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()) * _memory->getLastWorkingSignature()->getPose().inverse();
}
}
}
statistics_.setLoopClosureId(0);
}
}