i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Localization: Fixed local map cleared when gravity links are added and no localization happened. Fixed "Word not found" error on initialization when Mem/InitWMWithAllNodes is true.

Browse Source
This commit is contained in:
matlabbe
2019-09-19 15:23:18 -04:00
parent 955660ba2a
commit c437375f08
2 changed files with 20 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
corelib/src/DBDriverSqlite3.cpp
View File

@@ -2373,18 +2373,28 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
<< "FROM Node "; << "FROM Node ";
if(ignoreChildren) if(ignoreChildren)
{ {
query << "INNER JOIN Link " query << "INNER JOIN Link ";
<< "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them) 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
} }
if(ignoreBadSignatures) if(ignoreBadSignatures)
{ {
if(uStrNumCmp(_version, "0.13.0") >= 0) if(ignoreChildren)
{ {
query << "WHERE id in (select node_id from Feature) "; query << "AND ";
} }
else else
{ {
query << "WHERE id in (select node_id from Map_Node_Word) "; query << "WHERE ";
}
if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << " id in (select node_id from Feature) ";
}
else
{
query << " id in (select node_id from Map_Node_Word) ";
} }
} }
query << "ORDER BY id"; query << "ORDER BY id";

8
corelib/src/Rtabmap.cpp
View File

@@ -2556,7 +2556,8 @@ bool Rtabmap::process(
double optimizationError = 0.0; double optimizationError = 0.0;
int optimizationIterations = 0; int optimizationIterations = 0;
cv::Mat localizationCovariance; cv::Mat localizationCovariance;
if(_rgbdSlamMode && if(_rgbdSlamMode
&&
(_loopClosureHypothesis.first>0 || (_loopClosureHypothesis.first>0 ||
lastProximitySpaceClosureId>0 || // can be different map of the current one lastProximitySpaceClosureId>0 || // can be different map of the current one
statistics_.reducedIds().size() || statistics_.reducedIds().size() ||
@@ -2564,8 +2565,9 @@ bool Rtabmap::process(
(signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f) || // gravity edge (signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f) || // gravity edge
proximityDetectionsInTimeFound>0 || proximityDetectionsInTimeFound>0 ||
landmarkDetected!=0 || landmarkDetected!=0 ||
((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size()) && // In localization mode, the new node should be linked signaturesRetrieved.size()) // can be different map of the current one
signaturesRetrieved.size()))) // can be different map of the current one &&
(_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size())) // In localization mode, the new node should be linked
{ {
UASSERT(uContains(_optimizedPoses, signature->id())); UASSERT(uContains(_optimizedPoses, signature->id()));