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

rtabmap/dbreader: save odometry covariance in pose prior link when in localization mode and saving localization data.

Browse Source
This commit is contained in:
matlabbe
2022-02-06 20:17:46 -05:00
parent b44b212218
commit f584f42ea4
2 changed files with 28 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
corelib/src/DBReader.cpp
View File

@@ -433,20 +433,31 @@ SensorData DBReader::getNextData(CameraInfo * info)
infMatrix = links.begin()->second.infMatrix(); infMatrix = links.begin()->second.infMatrix();
_previousInfMatrix = infMatrix; _previousInfMatrix = infMatrix;
} }
else if(_previousMapId != s->mapId())
{
// first node, set high variance to make rtabmap trigger a new map
infMatrix /= 9999.0;
UDEBUG("First node of map %d, variance set to 9999", s->mapId());
}
else else
{ {
if(_previousInfMatrix.empty()) // if localization data saved in database, covariance will be set in a prior link
_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
if(links.size())
{ {
_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1); // assume the first is the backward neighbor, take its variance
infMatrix = links.begin()->second.infMatrix();
_previousInfMatrix = infMatrix;
}
else if(_previousMapId != s->mapId())
{
// first node, set high variance to make rtabmap trigger a new map
infMatrix /= 9999.0;
UDEBUG("First node of map %d, variance set to 9999", s->mapId());
}
else
{
if(_previousInfMatrix.empty())
{
_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
}
// we have a node not linked to map, use last variance
infMatrix = _previousInfMatrix;
} }
// we have a node not linked to map, use last variance
infMatrix = _previousInfMatrix;
} }
_previousMapId = s->mapId(); _previousMapId = s->mapId();
} }

7
corelib/src/Rtabmap.cpp
View File

@@ -3798,6 +3798,13 @@ bool Rtabmap::process(
_memory->removeRawData(signature->id(), true, !_neighborLinkRefining && !_proximityBySpace, true); _memory->removeRawData(signature->id(), true, !_neighborLinkRefining && !_proximityBySpace, true);
} }
// Localization mode and saving localization data: save odometry covariance in a prior link
// so that DBReader can republish the covariance of localization data
if(!_memory->isIncremental() && _memory->isLocalizationDataSaved() && !odomCovariance.empty())
{
_memory->addLink(Link(signature->id(), signature->id(), Link::kPosePrior, odomPose, odomCovariance.inv()));
}
// remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored) // remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored)
int signatureRemoved = _memory->cleanup(); int signatureRemoved = _memory->cleanup();
if(signatureRemoved) if(signatureRemoved)