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

Fixed #1439

Browse Source
This commit is contained in:
matlabbe
2025-01-19 13:39:17 -08:00
parent 853f603898
commit 7baf435600
2 changed files with 38 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
corelib/src/DBDriverSqlite3.cpp
View File

@@ -2427,9 +2427,13 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
<< "FROM Node ";
if(ignoreChildren)
{
query << "INNER JOIN Link ";
query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
query << "WHERE from_id != to_id "; // ignore self referring links
// use to_id to ignore all children (which don't have link pointing on them)
// ignore self referring links
// keep nodes without link to other nodes (map has only a single node)
query << "WHERE ";
query << "(EXISTS (select 1 from Link where Node.id=to_id and from_id != to_id) OR ";
query << " NOT EXISTS (select 1 from Link where id=to_id and from_id != to_id)) ";
query << "AND weight>-9 "; //ignore invalid nodes
if(ignoreIntermediateNodes)
{

31
corelib/src/Rtabmap.cpp
View File

@@ -716,6 +716,36 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
if(_memory)
{
bool isMemIncremental = _memory->isIncremental();
if(Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), isMemIncremental) &&
isMemIncremental != _memory->isIncremental())
{
// Mode has changed from Mapping to Localization, cleanup the local graph
if(_memory->isGraphReduced() && _memory->isIncremental())
{
// Force reducing graph, then remove filtered nodes from the optimized poses
std::map<int, int> reducedIds;
_memory->incrementMapId(&reducedIds);
for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
{
_optimizedPoses.erase(iter->first);
}
}
// In both cases, we save the latest optimized graph and latest localization pose
_memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
// Mode changed from Localization to Mapping, clear local graph
if(!_memory->isIncremental()) {
_optimizedPoses.clear();
_lastLocalizationPose.setNull();
_mapCorrection.setIdentity();
_mapCorrectionBackup.setNull();
_localizationCovariance = cv::Mat();
_lastLocalizationNodeId = 0;
}
}
_memory->parseParameters(parameters);
if(_memory->isIncremental() && !_globalScanMap.empty())
{
@@ -1720,6 +1750,7 @@ bool Rtabmap::process(
_constraints.erase(--_constraints.end());
}
}
_constraints.insert(std::make_pair(tmp.from(), tmp));
}
// Localization mode stuff