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

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 ";
if(ignoreChildren)
{
query << "INNER JOIN Link "
<< "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
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
}
if(ignoreBadSignatures)
{
if(uStrNumCmp(_version, "0.13.0") >= 0)
if(ignoreChildren)
{
query << "WHERE id in (select node_id from Feature) ";
query << "AND ";
}
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";

8
corelib/src/Rtabmap.cpp
View File

@@ -2556,7 +2556,8 @@ bool Rtabmap::process(
double optimizationError = 0.0;
int optimizationIterations = 0;
cv::Mat localizationCovariance;
if(_rgbdSlamMode &&
if(_rgbdSlamMode
&&
(_loopClosureHypothesis.first>0 ||
lastProximitySpaceClosureId>0 || // can be different map of the current one
statistics_.reducedIds().size() ||
@@ -2564,8 +2565,9 @@ bool Rtabmap::process(
(signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f) || // gravity edge
proximityDetectionsInTimeFound>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()));