i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Detect more loop closures from/to specific map ID only option (#1653)

Browse Source 
* Detect more loop closures from/to specific map ID only option

* default value -1

* Added elapsed time

* Avoid loading ALL signatures in RAM, only load them when necessary

* Avoid creating flann index when initializing with Kp/NNStrategy>=3

* Setting opt params by default
This commit is contained in:
matlabbe
2026-02-13 10:49:12 -08:00
committed by GitHub
parent d6cf470402
commit 0ef907757b
6 changed files with 234 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
corelib/src/Rtabmap.cpp
View File

@@ -5669,7 +5669,8 @@ int Rtabmap::detectMoreLoopClosures(
bool intraSession,
bool interSession,
const ProgressState * processState,
float clusterRadiusMin)
float clusterRadiusMin,
int toFromMapId)
{
UDEBUG("");
UASSERT(iterations>0);
@@ -5696,17 +5697,23 @@ int Rtabmap::detectMoreLoopClosures(
std::map<int, Transform> posesToCheckLoopClosures;
std::map<int, Transform> poses;
std::multimap<int, Link> links;
std::map<int, Signature> signatures; // some signatures may be in LTM, get them all
this->getGraph(poses, links, true, true, &signatures);
this->getGraph(poses, links, true, true);
std::map<int, int> mapIds;
UDEBUG("remove all invalid or intermediate nodes, fill mapIds");
for(std::map<int, Transform>::iterator iter=poses.upper_bound(0); iter!=poses.end();++iter)
{
if(signatures.at(iter->first).getWeight() >= 0)
Transform odom, gt;
int mapId, weight;
std::string l;
double s;
std::vector<float> v;
GPS gps;
EnvSensors srs;
if(_memory->getNodeInfo(iter->first, odom, mapId, weight, l, s, gt, v, gps, srs, true) && weight >= 0)
{
posesToCheckLoopClosures.insert(*iter);
mapIds.insert(std::make_pair(iter->first, signatures.at(iter->first).mapId()));
mapIds.insert(std::make_pair(iter->first, mapId));
}
}
@@ -5720,7 +5727,28 @@ int Rtabmap::detectMoreLoopClosures(
clusterRadiusMax,
clusterAngle);
UINFO("Looking for more loop closures, clustering poses... found %d clusters.", (int)clusters.size());
UINFO("Looking for more loop closures: clustering poses... found %ld clusters.", clusters.size());
if(toFromMapId >=0)
{
for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end();)
{
int mapId = uValue(mapIds, iter->first, 0);
if(mapId != toFromMapId)
{
iter = clusters.erase(iter);
}
else {
++iter;
}
}
UINFO("Looking for more loop closures: filtered %ld clusters for map session %d.", clusters.size(), toFromMapId);
if(clusters.empty())
{
UERROR("No clusters belong to mapId %d, aborting.", toFromMapId);
break;
}
}
int i=0;
std::set<int> addedLinks;
@@ -5772,8 +5800,10 @@ int Rtabmap::detectMoreLoopClosures(
{
checkedLoopClosures.insert(std::make_pair(from, to));
UASSERT(signatures.find(from) != signatures.end());
UASSERT(signatures.find(to) != signatures.end());
Signature fromS = getSignatureCopy(from, false, true, false, false, true, false);
Signature toS = getSignatureCopy(to, false, true, false, false, true, false);
UASSERT(fromS.getWeight()>=0);
UASSERT(toS.getWeight()>=0);
Transform guess;
if(_proximityBySpace && uContains(poses, from) && uContains(poses, to))
@@ -5783,7 +5813,7 @@ int Rtabmap::detectMoreLoopClosures(
RegistrationInfo info;
// use signatures instead of IDs because some signatures may not be in WM
Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), guess, &info);
Transform t = _memory->computeTransform(fromS, toS, guess, &info);
if(!t.isNull())
{
@@ -5792,11 +5822,11 @@ int Rtabmap::detectMoreLoopClosures(
//optimize the graph to see if the new constraint is globally valid
int fromId = from;
int mapId = signatures.at(from).mapId();
int mapId = fromS.mapId();
// use first node of the map containing from
for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster)
for(std::map<int, Transform>::iterator ster=posesToCheckLoopClosures.begin(); ster!=posesToCheckLoopClosures.end(); ++ster)
{
if(ster->second.mapId() == mapId)
if(uValue(mapIds, ster->first, 0) == mapId)
{
fromId = ster->first;
break;
@@ -5811,22 +5841,22 @@ int Rtabmap::detectMoreLoopClosures(
float maxLinearErrorRatio = 0.0f;
float maxAngularErrorRatio = 0.0f;
std::map<int, Transform> optimizedPoses;
std::multimap<int, Link> links;
std::multimap<int, Link> linksOut;
UASSERT(poses.find(fromId) != poses.end());
UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, linksOut);
UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
UASSERT(graph::findLink(links, from, to) != links.end());
optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)linksOut.size()).c_str());
UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)linksOut.size()).c_str());
UASSERT(graph::findLink(linksOut, from, to) != linksOut.end());
optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, linksOut);
std::string msg;
if(optimizedPoses.size())
{
graph::computeMaxGraphErrors(
optimizedPoses,
links,
linksOut,
maxLinearErrorRatio,
maxAngularErrorRatio,
maxLinearError,

20
corelib/src/VWDictionary.cpp
View File

@@ -571,19 +571,27 @@ void VWDictionary::update()
else if(_strategy >= kNNBruteForce &&
_notIndexedWords.size() &&
_removedIndexedWords.size() == 0 &&
_visualWords.size() &&
_dataTree.rows)
_visualWords.size())
{
//just add not indexed words
int i = _dataTree.rows;
_dataTree.reserve(_dataTree.rows + _notIndexedWords.size());
if(!_dataTree.empty()) {
_dataTree.reserve(_dataTree.rows + _notIndexedWords.size());
}
for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
{
VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
UASSERT(w);
UASSERT(w->getDescriptor().cols == _dataTree.cols);
UASSERT(w->getDescriptor().type() == _dataTree.type());
_dataTree.push_back(w->getDescriptor());
if(_dataTree.empty())
{
_dataTree = w->getDescriptor().clone();
}
else
{
UASSERT(w->getDescriptor().cols == _dataTree.cols);
UASSERT(w->getDescriptor().type() == _dataTree.type());
_dataTree.push_back(w->getDescriptor());
}
_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, w->id()));
std::pair<std::map<int, int>::iterator, bool> inserted = _mapIdIndex.insert(std::pair<int, int>(w->id(), i));
UASSERT(inserted.second);