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

Fix an issue with local BA and improve on-demand signature loading (#1753)

Browse Source 
* Fix local feature map assembly condition

* Allow local BA on merged neighbor links

* On-demand loading local visual features for neighbor signatures

---------

Co-authored-by: matlabbe <[email protected]>
This commit is contained in:
Borong Yuan
2026-08-26 20:48:35 -07:00
committed by GitHub
co-authored by matlabbe
parent 94c93ac9b1
commit 2d4a3391bb
1 changed files with 52 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+38 -12
View File
@@ -3507,7 +3507,8 @@ Transform Memory::computeTransform(
tmpTo.setWordsDescriptors(cv::Mat()); tmpTo.setWordsDescriptors(cv::Mat());
} }
bool isNeighborRefining = fromS.getLinks().find(toS.id()) != fromS.getLinks().end() && fromS.getLinks().find(toS.id())->second.type() == Link::kNeighbor; std::multimap<int, Link> links = getNeighborLinks(fromS.id(), false); // assemble only neighbors for the local feature map
links.erase(toS.id());
if(guess.isNull() && !_registrationPipeline->isImageRequired()) if(guess.isNull() && !_registrationPipeline->isImageRequired())
{ {
@@ -3520,7 +3521,7 @@ Transform Memory::computeTransform(
transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info); transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
} }
} }
else if(!isNeighborRefining && else if(!fromS.hasLink(toS.id(), Link::kNeighbor) &&
_localBundleOnLoopClosure && _localBundleOnLoopClosure &&
_registrationPipeline->isImageRequired() && _registrationPipeline->isImageRequired() &&
!_registrationPipeline->isScanRequired() && !_registrationPipeline->isScanRequired() &&
@@ -3533,14 +3534,45 @@ Transform Memory::computeTransform(
!tmpFrom.getWords().empty() && !tmpFrom.getWords().empty() &&
!tmpFrom.getWordsKpts().empty() && !tmpFrom.getWordsKpts().empty() &&
!tmpFrom.getWords3().empty() && !tmpFrom.getWords3().empty() &&
fromS.hasLink(0, Link::kNeighbor)) // If doesn't have neighbors, skip bundle !links.empty() && // If doesn't have neighbors, skip bundle
_dbDriver)
{ {
std::multimap<int, int> words; std::multimap<int, int> words;
std::vector<cv::Point3f> words3DMap; std::vector<cv::Point3f> words3DMap;
std::vector<cv::KeyPoint> wordsMap; std::vector<cv::KeyPoint> wordsMap;
cv::Mat wordsDescriptorsMap; cv::Mat wordsDescriptorsMap;
const std::multimap<int, Link> & links = fromS.getLinks(); for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
Signature * s = this->_getSignature(iter->first);
if(s && !s->getWords().empty() && s->getWordsKpts().empty())
{
UDEBUG("Loading local visual features for neighbor signature %d", s->id());
std::multimap<int, int> words;
std::vector<cv::KeyPoint> keypoints;
std::vector<cv::Point3f> points;
cv::Mat descriptors;
UTimer timer;
_dbDriver->getLocalFeatures(s->id(), words, keypoints, points, descriptors);
if(!words.empty() && !keypoints.empty())
{
UASSERT(words.size() == s->getWords().size());
std::map<int, int> wordsChanged = s->getWordsChanged();
bool wasEnabled = s->isEnabled();
s->setWords(words, keypoints, points, descriptors);
for(const auto & iter : wordsChanged) {
s->changeWordsRef(iter.first, iter.second);
}
s->setEnabled(wasEnabled);
UDEBUG("Loaded %ld local visual features for neighbor signature %d! (in %f s)", words.size(), s->id(), timer.ticks());
}
else
{
UDEBUG("Failed to load local visual features for neighbor signature %d.", s->id());
}
}
}
if(!fromS.getWords3().empty()) if(!fromS.getWords3().empty())
{ {
const std::map<int, int> & wordsFrom = uMultimapToMapUnique(fromS.getWords()); const std::map<int, int> & wordsFrom = uMultimapToMapUnique(fromS.getWords());
@@ -3561,13 +3593,10 @@ Transform Memory::computeTransform(
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)
{ {
int id = iter->first; const Signature * s = this->getSignature(iter->first);
if(id != fromS.id() && iter->second.type() == Link::kNeighbor) // assemble only neighbors for the local feature map
{
const Signature * s = this->getSignature(id);
if(s) if(s)
{ {
if(s->getWordsKpts().empty() && s->getWords3().empty() && s->getWordsDescriptors().empty()) { if(s->getWordsKpts().empty() || s->getWords3().empty() || s->getWordsDescriptors().empty()) {
UDEBUG("Signature %d doesn't have features set. Cannot be added in the local feature map.", s->id()); UDEBUG("Signature %d doesn't have features set. Cannot be added in the local feature map.", s->id());
continue; continue;
} }
@@ -3587,7 +3616,6 @@ Transform Memory::computeTransform(
} }
} }
} }
}
UDEBUG("words3DMap=%d", (int)words3DMap.size()); UDEBUG("words3DMap=%d", (int)words3DMap.size());
Signature tmpFrom2(fromS.id()); Signature tmpFrom2(fromS.id());
tmpFrom2.setWords(words, wordsMap, words3DMap, wordsDescriptorsMap); tmpFrom2.setWords(words, wordsMap, words3DMap, wordsDescriptorsMap);
@@ -3607,8 +3635,6 @@ Transform Memory::computeTransform(
std::map<int, std::vector<CameraModel> > bundleModels; std::map<int, std::vector<CameraModel> > bundleModels;
std::map<int, std::map<int, FeatureBA> > wordReferences; std::map<int, std::map<int, FeatureBA> > wordReferences;
std::multimap<int, Link> links = fromS.getLinks();
links = graph::filterLinks(links, Link::kNeighbor, true); // assemble only neighbors for the local feature map
links.insert(std::make_pair(toS.id(), Link(fromS.id(), toS.id(), Link::kGlobalClosure, transform, info->covariance.inv()))); links.insert(std::make_pair(toS.id(), Link(fromS.id(), toS.id(), Link::kGlobalClosure, transform, info->covariance.inv())));
links.insert(std::make_pair(fromS.id(), Link())); links.insert(std::make_pair(fromS.id(), Link()));