i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Reduced local bundle on loop closure iterations to 5. Updated graph::filterLinks() to handle selfRef link type. DbViewer: removed gravity constraints from constraints view.

Browse Source
This commit is contained in:
matlabbe
2019-06-01 21:43:13 -04:00
parent ec943198f0
commit 8cb923b332
4 changed files with 33 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
corelib/src/Graph.cpp
View File

@@ -1117,7 +1117,7 @@ std::multimap<int, Link> filterDuplicateLinks(
std::multimap<int, Link> output; std::multimap<int, Link> output;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter) for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{ {
if(graph::findLink(output, iter->second.from(), iter->second.to(), true) == output.end()) if(graph::findLink(output, iter->second.from(), iter->second.to(), true, iter->second.type()) == output.end())
{ {
output.insert(*iter); output.insert(*iter);
} }
@@ -1132,7 +1132,14 @@ std::multimap<int, Link> filterLinks(
std::multimap<int, Link> output; std::multimap<int, Link> output;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter) for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{ {
if(iter->second.type() != filteredType) if(filteredType == Link::kSelfRefLink)
{
if(iter->second.from() != iter->second.to())
{
output.insert(*iter);
}
}
else if(iter->second.type() != filteredType)
{ {
output.insert(*iter); output.insert(*iter);
} }

18
corelib/src/Memory.cpp
View File

@@ -2838,8 +2838,9 @@ Transform Memory::computeTransform(
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter) for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{ {
int id = iter->first; int id = iter->first;
if(id != fromS.id()) if(id != fromS.id() || iter->second.transform().isNull())
{ {
UDEBUG("%d", id);
const Signature * s; const Signature * s;
if(id == tmpTo.id()) if(id == tmpTo.id())
{ {
@@ -2873,15 +2874,19 @@ Transform Memory::computeTransform(
} }
bundleModels.insert(std::make_pair(id, model)); bundleModels.insert(std::make_pair(id, model));
Transform invLocalTransform = model.localTransform().inverse(); Transform invLocalTransform = model.localTransform().inverse();
if(iter->second.isValid()) UASSERT(iter->second.isValid() || iter->first == fromS.id());
if(iter->second.transform().isNull())
{
// fromId pose
bundlePoses.insert(std::make_pair(id, Transform::getIdentity()));
}
else
{ {
bundleLinks.insert(std::make_pair(iter->second.from(), iter->second)); bundleLinks.insert(std::make_pair(iter->second.from(), iter->second));
bundlePoses.insert(std::make_pair(id, iter->second.transform())); bundlePoses.insert(std::make_pair(id, iter->second.transform()));
} }
else
{
bundlePoses.insert(std::make_pair(id, Transform::getIdentity()));
}
const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords()); const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords());
for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter) for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
{ {
@@ -2903,6 +2908,7 @@ Transform Memory::computeTransform(
std::set<int> sbaOutliers; std::set<int> sbaOutliers;
UTimer bundleTimer; UTimer bundleTimer;
OptimizerG2O sba; OptimizerG2O sba;
sba.setIterations(5);
UTimer bundleTime; UTimer bundleTime;
bundlePoses = sba.optimizeBA(-toS.id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers); bundlePoses = sba.optimizeBA(-toS.id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
UDEBUG("sba...end"); UDEBUG("sba...end");

16
corelib/src/Rtabmap.cpp
View File

@@ -2533,8 +2533,8 @@ bool Rtabmap::process(
(_loopClosureHypothesis.first>0 || (_loopClosureHypothesis.first>0 ||
lastProximitySpaceClosureId>0 || // can be different map of the current one lastProximitySpaceClosureId>0 || // can be different map of the current one
statistics_.reducedIds().size() || statistics_.reducedIds().size() ||
(signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored()) || // prior edge (signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored() && !_memory->isIncremental()) || // prior edge
(signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f) || // gravity edge (signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f && !_memory->isIncremental()) || // gravity edge
proximityDetectionsInTimeFound>0 || proximityDetectionsInTimeFound>0 ||
landmarkDetected!=0 || landmarkDetected!=0 ||
((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size()) && // In localization mode, the new node should be linked ((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size()) && // In localization mode, the new node should be linked
@@ -3251,9 +3251,15 @@ bool Rtabmap::process(
} }
else else
{ {
UASSERT_MSG(uContains(_optimizedPoses, _lastLocalizationNodeId), uFormat("id=%d isInWM?=%d", _lastLocalizationNodeId, _memory->isInWM(_lastLocalizationNodeId)?1:0).c_str()); if(uContains(_optimizedPoses, _lastLocalizationNodeId))
id = _lastLocalizationNodeId; {
UDEBUG("Refresh local map from %d", id); id = _lastLocalizationNodeId;
UDEBUG("Refresh local map from %d", id);
}
else
{
_lastLocalizationNodeId = 0;
}
} }
} }
else if(_memory->isIncremental() && else if(_memory->isIncremental() &&

2
guilib/src/DatabaseViewer.cpp
View File

@@ -7191,7 +7191,7 @@ void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
{ {
linksSortedByParents.insert(std::make_pair(iter->second.to(), iter->second.inverse())); linksSortedByParents.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
} }
else else if(iter->second.to() != iter->second.from())
{ {
linksSortedByParents.insert(*iter); linksSortedByParents.insert(*iter);
} }