i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Intermediate node fixes for ProximityTime and detecting more loop closures in the GUI

Browse Source
This commit is contained in:
matlabbe
2016-02-18 12:52:15 -05:00
parent c1c62b0738
commit 6b8ab84ba0
2 changed files with 27 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+5 -2
View File
@@ -1153,14 +1153,17 @@ bool Rtabmap::process(
if(_proximityByTime && if(_proximityByTime &&
rehearsedId == 0 && // don't do it if rehearsal happened rehearsedId == 0 && // don't do it if rehearsal happened
signature->getWords3().size() && signature->getWords3().size() &&
_memory->isIncremental()) // don't do it in localization mode _memory->isIncremental(), // don't do it in localization mode
!signature->isBadSignature() &&
signature->getWeight()>=0)
{ {
const std::set<int> & stm = _memory->getStMem(); const std::set<int> & stm = _memory->getStMem();
for(std::set<int>::const_reverse_iterator iter = stm.rbegin(); iter!=stm.rend(); ++iter) for(std::set<int>::const_reverse_iterator iter = stm.rbegin(); iter!=stm.rend(); ++iter)
{ {
if(*iter != signature->id() && if(*iter != signature->id() &&
signature->getLinks().find(*iter) == signature->getLinks().end() && signature->getLinks().find(*iter) == signature->getLinks().end() &&
_memory->getSignature(*iter)->mapId() == signature->mapId()) _memory->getSignature(*iter)->mapId() == signature->mapId() &&
_memory->getSignature(*iter)->getWeight()>=0)
{ {
std::string rejectedMsg; std::string rejectedMsg;
UDEBUG("Check local transform between %d and %d", signature->id(), *iter); UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
guilib/src/MainWindow.cpp
+22 -18
View File
@@ -3711,27 +3711,31 @@ void MainWindow::postProcessing()
Signature signatureFrom = _cachedSignatures[from]; Signature signatureFrom = _cachedSignatures[from];
Signature signatureTo = _cachedSignatures[to]; Signature signatureTo = _cachedSignatures[to];
if(reextractFeatures) if(signatureFrom.getWeight() >= 0 &&
signatureTo.getWeight() >= 0) // ignore intermediate nodes
{ {
signatureFrom.setWords(std::multimap<int, cv::KeyPoint>()); if(reextractFeatures)
signatureFrom.setWords3(std::multimap<int, cv::Point3f>()); {
signatureTo.setWords(std::multimap<int, cv::KeyPoint>()); signatureFrom.setWords(std::multimap<int, cv::KeyPoint>());
signatureTo.setWords3(std::multimap<int, cv::Point3f>()); signatureFrom.setWords3(std::multimap<int, cv::Point3f>());
} signatureTo.setWords(std::multimap<int, cv::KeyPoint>());
signatureTo.setWords3(std::multimap<int, cv::Point3f>());
}
Transform transform; Transform transform;
RegistrationInfo info; RegistrationInfo info;
RegistrationVis registration(parameters); RegistrationVis registration(parameters);
transform = registration.computeTransformation(signatureFrom, signatureTo, Transform(), &info); transform = registration.computeTransformation(signatureFrom, signatureTo, Transform(), &info);
if(!transform.isNull()) if(!transform.isNull())
{ {
UINFO("Added new loop closure between %d and %d.", from, to); UINFO("Added new loop closure between %d and %d.", from, to);
addedLinks.insert(from); addedLinks.insert(from);
addedLinks.insert(to); addedLinks.insert(to);
_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.variance, info.variance))); _currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.variance, info.variance)));
++loopClosuresAdded; ++loopClosuresAdded;
_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size())); _initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
}
} }
} }
} }