i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-07 02:27:47 +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
corelib/src/DBReader.cpp
+21 -10
View File
@@ -433,20 +433,31 @@ SensorData DBReader::getNextData(CameraInfo * info)
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())
// 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();
}