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

GUI: Support Optimizer/Robust with RGBD/OptimizeMaxError at the same time. DBReader: fixed repeated odom covariance when Mem/ReduceGraph is used. Memory: avoid error msg when receiving frames without features while we expect odometry features. See also #1573

Browse Source
This commit is contained in:
matlabbe
2025-09-24 23:24:32 -07:00
parent c774ef641a
commit a552bdfb2f
7 changed files with 71 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/DBReader.cpp
+33 -14
View File
@@ -510,22 +510,41 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
}
else
{
// if localization data saved in database, covariance will be set in a prior link
_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
if(links.size())
{
// assume the first is the backward neighbor, take its variance
infMatrix = links.begin()->second.infMatrix();
_previousInfMatrix = infMatrix;
}
else
{
if(_previousInfMatrix.empty())
// In case the graph was reduced, look for forward neighbor link from previous id
bool covAdded = false;
if(_currentId != _ids.begin()) {
std::set<int>::iterator previousId = _currentId;
--previousId;
std::multimap<int, Link> previousLinks;
_dbDriver->loadLinks(*previousId, previousLinks, Link::kNeighbor);
if(previousLinks.size() && previousLinks.rbegin()->first == *_currentId)
{
_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
// assume the last is the forward neighbor pointing to current ID, take its covariance
infMatrix = previousLinks.rbegin()->second.infMatrix();
_previousInfMatrix = infMatrix;
covAdded = true;
}
}
if(!covAdded) {
// if localization data saved in database, covariance will be set in a prior link
_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
if(links.size())
{
// assume the first is the backward neighbor, take its variance
infMatrix = links.begin()->second.infMatrix();
_previousInfMatrix = infMatrix;
}
else
{
if(_previousInfMatrix.empty())
{
_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
}
// we have a node not linked to map, use last variance
UWARN("The node loaded (%d) doesn't have neighbor, re-using the covariance of the previous link for odometry.", s->id());
infMatrix = _previousInfMatrix;
}
// we have a node not linked to map, use last variance
infMatrix = _previousInfMatrix;
}
}
}