From 2d4a3391bbdd1a26f048a4a530b435cec3e1df4a Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Thu, 27 Aug 2026 11:48:35 +0800 Subject: [PATCH] Fix an issue with local BA and improve on-demand signature loading (#1753) * 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 --- corelib/src/Memory.cpp | 78 ++++++++++++++++++++++++++++-------------- 1 file changed, 52 insertions(+), 26 deletions(-) diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 23553a11..c1ee7ad4 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -3507,7 +3507,8 @@ Transform Memory::computeTransform( 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 links = getNeighborLinks(fromS.id(), false); // assemble only neighbors for the local feature map + links.erase(toS.id()); if(guess.isNull() && !_registrationPipeline->isImageRequired()) { @@ -3520,7 +3521,7 @@ Transform Memory::computeTransform( transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info); } } - else if(!isNeighborRefining && + else if(!fromS.hasLink(toS.id(), Link::kNeighbor) && _localBundleOnLoopClosure && _registrationPipeline->isImageRequired() && !_registrationPipeline->isScanRequired() && @@ -3533,14 +3534,45 @@ Transform Memory::computeTransform( !tmpFrom.getWords().empty() && !tmpFrom.getWordsKpts().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 words; std::vector words3DMap; std::vector wordsMap; cv::Mat wordsDescriptorsMap; - const std::multimap & links = fromS.getLinks(); + for(std::multimap::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 words; + std::vector keypoints; + std::vector 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 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()) { const std::map & wordsFrom = uMultimapToMapUnique(fromS.getWords()); @@ -3561,29 +3593,25 @@ Transform Memory::computeTransform( for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) { - int id = iter->first; - if(id != fromS.id() && iter->second.type() == Link::kNeighbor) // assemble only neighbors for the local feature map + const Signature * s = this->getSignature(iter->first); + if(s) { - const Signature * s = this->getSignature(id); - if(s) + 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()); + continue; + } + const std::map & wordsTo = uMultimapToMapUnique(s->getWords()); + for(std::map::const_iterator jter=wordsTo.begin(); jter!=wordsTo.end(); ++jter) { - 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()); - continue; - } - const std::map & wordsTo = uMultimapToMapUnique(s->getWords()); - for(std::map::const_iterator jter=wordsTo.begin(); jter!=wordsTo.end(); ++jter) + const cv::Point3f & pt = s->getWords3()[jter->second]; + if( jter->first > 0 && + util3d::isFinite(pt) && + words.find(jter->first) == words.end()) { - const cv::Point3f & pt = s->getWords3()[jter->second]; - if( jter->first > 0 && - util3d::isFinite(pt) && - words.find(jter->first) == words.end()) - { - words.insert(words.end(), std::make_pair(jter->first, words.size())); - words3DMap.push_back(util3d::transformPoint(pt, iter->second.transform())); - wordsMap.push_back(s->getWordsKpts()[jter->second]); - wordsDescriptorsMap.push_back(s->getWordsDescriptors().row(jter->second)); - } + words.insert(words.end(), std::make_pair(jter->first, words.size())); + words3DMap.push_back(util3d::transformPoint(pt, iter->second.transform())); + wordsMap.push_back(s->getWordsKpts()[jter->second]); + wordsDescriptorsMap.push_back(s->getWordsDescriptors().row(jter->second)); } } } @@ -3607,8 +3635,6 @@ Transform Memory::computeTransform( std::map > bundleModels; std::map > wordReferences; - std::multimap 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(fromS.id(), Link()));