mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-07 02:27:47 +08:00
New rtabmap-reduceGraph CLI tool (#1655)
* New rtabmap-reduceGraph CLI tool * fixed some edge cases * Regenerating optimized map if there was one before reducing the graph * addMoreLoopClosures: refactored how ctrl-c is handled to stop faster when no loop closures are added * Added kilted status * Make offline tool always propagate neighbor merged links * removed a parameter * fixed disconnected graph * fixed --help * Added error log on Kp/NNStrategy not compatible with huge vocabulary. ReduceGraph/DetectMoreLoopClosures: Make sure original parameters are saved back on closing. g2o: fixing optimizer to Levenberg for SBA to avoid [SetJac] infinite jac fatal error. * exposing neighbor merged ratio parameter to the tool * show param in log * refactored detectMoreLoopClosures to ignore too close nodes in terms of neighbor links based on Mem/STMSize parameter. Reduce graph: added direction parameter. * Simplified: removed ratio parameter, removed recursive reduction. Just don't reduce if a NM link is longer than maxDistance. * Removed NNStrategy override, as it was still done on closing when we changed back to original params * DBViewer: show missing links when showing OptimizedPoses in GraphView, fixed clicking on landmark links * DetectMoreLoopClosures: Added support for min graph distance option in MainWindow and DbViewer * slight renaming of ROS jobs * reprocess: added option --params_last
This commit is contained in:
matlabbe
+150
-83
@@ -1288,114 +1288,180 @@ void Memory::addSignatureToWmFromLTM(Signature * signature)
|
||||
}
|
||||
}
|
||||
|
||||
void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
|
||||
bool Memory::canBeReduced(const Link & link, float maxDistance, int direction)
|
||||
{
|
||||
UDEBUG("Inserting node %d from STM in WM...", id);
|
||||
UASSERT(_stMem.find(id) != _stMem.end());
|
||||
return link.to() != link.from() &&
|
||||
link.type() != Link::kNeighbor &&
|
||||
link.type() != Link::kNeighborMerged &&
|
||||
link.userDataCompressed().empty() &&
|
||||
link.type() != Link::kUndef &&
|
||||
link.type() != Link::kVirtualClosure &&
|
||||
(maxDistance == 0.0f || link.transform().getNorm() < maxDistance) &&
|
||||
(direction == 0 || (direction==-1 && link.to() < link.from()) || (direction==1 && link.to() > link.from()));
|
||||
}
|
||||
|
||||
int Memory::reduceNode(int id, float maxDistance, bool keepLinkedInDb, int direction)
|
||||
{
|
||||
UDEBUG("Reducing %d (max distance=%f, keep linked in db=%s, direction=%d)",
|
||||
id, maxDistance, keepLinkedInDb?"true":"false", direction);
|
||||
Signature * s = this->_getSignature(id);
|
||||
UASSERT(s!=0);
|
||||
|
||||
if(_reduceGraph)
|
||||
if(s==0)
|
||||
{
|
||||
bool merge = false;
|
||||
const std::multimap<int, Link> & links = s->getLinks();
|
||||
std::map<int, Link> neighbors;
|
||||
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
|
||||
{
|
||||
if(!merge)
|
||||
{
|
||||
merge = iter->second.to() < s->id() && // should be a parent->child link
|
||||
iter->second.to() != iter->second.from() &&
|
||||
iter->second.type() != Link::kNeighbor &&
|
||||
iter->second.type() != Link::kNeighborMerged &&
|
||||
iter->second.userDataCompressed().empty() &&
|
||||
iter->second.type() != Link::kUndef &&
|
||||
iter->second.type() != Link::kVirtualClosure;
|
||||
if(merge)
|
||||
{
|
||||
UDEBUG("Reduce %d to %d", s->id(), iter->second.to());
|
||||
if(reducedTo)
|
||||
{
|
||||
*reducedTo = iter->second.to();
|
||||
}
|
||||
}
|
||||
UWARN("Node %d is not in WM/STM, cannot reduce it.", id);
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
if(iter->second.type() == Link::kNeighbor)
|
||||
if(!s->getLabel().empty())
|
||||
{
|
||||
// We currently not remove nodes with labels
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::multimap<int, Link> links = s->getLinks();
|
||||
std::map<int, Link> neighbors;
|
||||
int reducedTo = 0;
|
||||
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
|
||||
{
|
||||
if(canBeReduced(iter->second, maxDistance, direction))
|
||||
{
|
||||
float distance = iter->second.transform().getNorm();
|
||||
reducedTo = iter->second.to();
|
||||
UDEBUG("Reduce %d to %d (distance=%f)",
|
||||
s->id(), iter->second.to(), distance);
|
||||
}
|
||||
|
||||
if(iter->second.type() == Link::kNeighbor)
|
||||
{
|
||||
neighbors.insert(*iter);
|
||||
}
|
||||
}
|
||||
if(reducedTo>0)
|
||||
{
|
||||
if(maxDistance > 0.0f)
|
||||
{
|
||||
// Only reduce if all neighbor merged links are also below maxDistance
|
||||
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
|
||||
{
|
||||
neighbors.insert(*iter);
|
||||
if( iter->second.type() == Link::kNeighborMerged &&
|
||||
iter->second.transform().getNorm() > maxDistance)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(merge)
|
||||
|
||||
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
|
||||
{
|
||||
if(s->getLabel().empty())
|
||||
Signature * sTo = this->_getSignature(iter->first);
|
||||
if(sTo->id()!=s->id()) // Not Prior/Gravity links...
|
||||
{
|
||||
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
|
||||
UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str());
|
||||
sTo->removeLink(s->id());
|
||||
if(iter->second.type() != Link::kNeighbor &&
|
||||
iter->second.type() != Link::kUndef)
|
||||
{
|
||||
Signature * sTo = this->_getSignature(iter->first);
|
||||
if(sTo->id()!=s->id()) // Not Prior/Gravity links...
|
||||
if(iter->second.type() == Link::kNeighborMerged)
|
||||
{
|
||||
UASSERT_MSG(sTo!=0, uFormat("id=%d", iter->first).c_str());
|
||||
sTo->removeLink(s->id());
|
||||
if(iter->second.type() != Link::kNeighbor &&
|
||||
iter->second.type() != Link::kNeighborMerged &&
|
||||
iter->second.type() != Link::kUndef)
|
||||
s->removeLink(sTo->id());
|
||||
if(maxDistance == 0.0f)
|
||||
{
|
||||
// link to all neighbors
|
||||
for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
|
||||
// online graph reduction, always skip these links
|
||||
continue;
|
||||
}
|
||||
}
|
||||
// link to all neighbors
|
||||
for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
|
||||
{
|
||||
if(!sTo->hasLink(jter->second.to()))
|
||||
{
|
||||
Link l = iter->second.inverse().merge(
|
||||
jter->second,
|
||||
iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
|
||||
UDEBUG("Merging link %d->%d (type=%d) to with %d->%d (type %d). Adding %d->%d (type %d) to %d and %d",
|
||||
iter->second.to(), iter->second.from(), iter->second.type(),
|
||||
jter->second.from(), jter->second.to(), jter->second.type(),
|
||||
l.from(), l.to(), l.type(), sTo->id(), l.to());
|
||||
sTo->addLink(l);
|
||||
Signature * sB = this->_getSignature(l.to());
|
||||
UASSERT(sB!=0);
|
||||
UASSERT_MSG(!sB->hasLink(l.from()), uFormat("%d->%d type=%d", sB->id(), l.to(), l.type()).c_str());
|
||||
sB->addLink(l.inverse());
|
||||
}
|
||||
}
|
||||
// link to all landmarks
|
||||
for(std::map<int, Link>::const_iterator jter=s->getLandmarks().begin(); jter!=s->getLandmarks().end(); ++jter)
|
||||
{
|
||||
if(!uContains(sTo->getLandmarks(), jter->first))
|
||||
{
|
||||
UDEBUG("Move landmark observation %d from %d to %d",
|
||||
jter->first, s->id(), sTo->id());
|
||||
Link l = iter->second.inverse().merge(
|
||||
jter->second,
|
||||
jter->second.type());
|
||||
sTo->addLandmark(l);
|
||||
// Update landmark index
|
||||
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(jter->first);
|
||||
if(nter!=_landmarksIndex.end())
|
||||
{
|
||||
if(!sTo->hasLink(jter->second.to()))
|
||||
{
|
||||
UDEBUG("Merging link %d->%d (type=%d) to link %d->%d (type %d)",
|
||||
iter->second.from(), iter->second.to(), iter->second.type(),
|
||||
jter->second.from(), jter->second.to(), jter->second.type());
|
||||
Link l = iter->second.inverse().merge(
|
||||
jter->second,
|
||||
iter->second.userDataCompressed().empty() && iter->second.type() != Link::kVirtualClosure?Link::kNeighborMerged:iter->second.type());
|
||||
sTo->addLink(l);
|
||||
Signature * sB = this->_getSignature(l.to());
|
||||
UASSERT(sB!=0);
|
||||
UASSERT_MSG(!sB->hasLink(l.from()), uFormat("%d->%d", sB->id(), l.to()).c_str());
|
||||
sB->addLink(l.inverse());
|
||||
}
|
||||
nter->second.insert(sTo->id());
|
||||
}
|
||||
else
|
||||
{
|
||||
std::set<int> tmp;
|
||||
tmp.insert(sTo->id());
|
||||
_landmarksIndex.insert(std::make_pair(jter->first, tmp));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//remove neighbor links
|
||||
std::multimap<int, Link> linksCopy = links;
|
||||
for(std::multimap<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
|
||||
this->moveToTrash(s, keepLinkedInDb);
|
||||
s = 0;
|
||||
_linksChanged = true;
|
||||
_memoryChanged = true;
|
||||
}
|
||||
return reducedTo;
|
||||
}
|
||||
|
||||
void Memory::moveSignatureToWMFromSTM(int id, int * reducedToOut)
|
||||
{
|
||||
UDEBUG("Inserting node %d from STM in WM...", id);
|
||||
UASSERT(_stMem.find(id) != _stMem.end());
|
||||
int reducedId = 0;
|
||||
if(_reduceGraph)
|
||||
{
|
||||
Signature * s = this->_getSignature(id);
|
||||
UASSERT(s!=0);
|
||||
std::multimap<int, Link> links = s->getLinks();
|
||||
// Setting true to make sure we save all visual
|
||||
// words that could be referenced in a previously
|
||||
// transferred node in LTM (#979)
|
||||
reducedId = reduceNode(s->id(), 0, true);
|
||||
if(reducedToOut) {
|
||||
*reducedToOut = reducedId;
|
||||
}
|
||||
if(reducedId>0)
|
||||
{
|
||||
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
|
||||
{
|
||||
if(iter->second.type() == Link::kNeighbor)
|
||||
{
|
||||
if(iter->second.type() == Link::kNeighborMerged)
|
||||
if(_lastGlobalLoopClosureId == s->id())
|
||||
{
|
||||
// Removing only merged neighbor links, we keep original neighbor
|
||||
// links to be able to reprocess databases with correct odometry covariance.
|
||||
s->removeLink(iter->first);
|
||||
}
|
||||
if(iter->second.type() == Link::kNeighbor)
|
||||
{
|
||||
if(_lastGlobalLoopClosureId == s->id())
|
||||
{
|
||||
_lastGlobalLoopClosureId = iter->first;
|
||||
}
|
||||
_lastGlobalLoopClosureId = iter->first;
|
||||
}
|
||||
}
|
||||
|
||||
// Setting true to make sure we save all visual
|
||||
// words that could be referenced in a previously
|
||||
// transferred node in LTM (#979)
|
||||
this->moveToTrash(s, true);
|
||||
s = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(s != 0)
|
||||
if(reducedId == 0)
|
||||
{
|
||||
_workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
|
||||
_stMem.erase(*_stMem.begin());
|
||||
}
|
||||
// else already removed from STM/WM in moveToTrash()
|
||||
// else already removed from STM/WM in reduceNode()
|
||||
}
|
||||
|
||||
const Signature * Memory::getSignature(int id) const
|
||||
@@ -2610,9 +2676,10 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
|
||||
// If not saved to database
|
||||
if(!keepLinkedToGraph)
|
||||
{
|
||||
UASSERT_MSG(this->isInSTM(s->id()),
|
||||
UASSERT_MSG(this->isInSTM(s->id()) || this->isInWM(s->id()),
|
||||
uFormat("Deleting location (%d) outside the "
|
||||
"STM is not implemented!", s->id()).c_str());
|
||||
"WM/STM is not implemented! STM size=%ld WM size=%ld",
|
||||
s->id(), this->getStMem().size(), this->getWorkingMem().size()).c_str());
|
||||
const std::multimap<int, Link> & links = s->getLinks();
|
||||
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
|
||||
{
|
||||
@@ -2623,7 +2690,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
|
||||
UASSERT_MSG(sTo!=0,
|
||||
uFormat("A neighbor (%d) of the deleted location %d is "
|
||||
"not found in WM/STM! Are you deleting a location "
|
||||
"outside the STM?", iter->first, s->id()).c_str());
|
||||
"outside the WM/STM?", iter->first, s->id()).c_str());
|
||||
|
||||
if(iter->first > s->id() && links.size()>1 && sTo->hasLink(s->id()))
|
||||
{
|
||||
@@ -2633,7 +2700,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
|
||||
}
|
||||
|
||||
// child
|
||||
if(iter->second.type() == Link::kGlobalClosure && s->id() > sTo->id() && s->getWeight()>0)
|
||||
if(iter->second.type() == Link::kGlobalClosure && s->getWeight()>0)
|
||||
{
|
||||
sTo->setWeight(sTo->getWeight() + s->getWeight()); // copy weight
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user