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

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

47
corelib/src/DBReader.cpp
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;
}
}
}

29
corelib/src/Memory.cpp
View File

@@ -131,6 +131,7 @@ Memory::Memory(const ParametersMap & parameters) :
_linksChanged(false),
_signaturesAdded(0),
_allNodesInWM(true),
_receivingOdometryFeatures(false),
_badSignRatio(Parameters::defaultKpBadSignRatio()),
_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
_parallelized(Parameters::defaultKpParallelized()),
@@ -1262,16 +1263,17 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
std::multimap<int, Link> linksCopy = links;
for(std::multimap<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged)
if(iter->second.type() == Link::kNeighborMerged)
{
// Removing only merged neighbor links, we keep original neighbor
// links to be able to reprocess databases with correct odometry covariance.
s->removeLink(iter->first);
if(iter->second.type() == Link::kNeighbor)
}
if(iter->second.type() == Link::kNeighbor)
{
if(_lastGlobalLoopClosureId == s->id())
{
if(_lastGlobalLoopClosureId == s->id())
{
_lastGlobalLoopClosureId = iter->first;
}
_lastGlobalLoopClosureId = iter->first;
}
}
}
@@ -1929,6 +1931,7 @@ void Memory::clear()
_landmarksIndex.clear();
_landmarksSize.clear();
_allNodesInWM = true;
_receivingOdometryFeatures = false;
if(_dbDriver)
{
@@ -4849,6 +4852,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
meanWordsPerLocation = _vwd->getTotalActiveReferences() / (treeSize-1); // ignore virtual signature
}
else if(_useOdometryFeatures) {
// To not detect first image as bad signature if odometry
// is using less features than feature2D->getMaxFeatures()
meanWordsPerLocation = 0;
}
if(_parallelized && !isIntermediateNode)
{
@@ -4952,10 +4960,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
SensorData decimatedData;
UDEBUG("Received kpts=%d kpts3D=%d, descriptors=%d _useOdometryFeatures=%s",
(int)data.keypoints().size(), (int)data.keypoints3D().size(), data.descriptors().rows, _useOdometryFeatures?"true":"false");
// TODO: do we still need the third and fouth comparisons?
// TODO: there is significant repetitive code between the if and the else, could we combine them?!
if(!_useOdometryFeatures ||
data.keypoints().empty() ||
(!_receivingOdometryFeatures && data.keypoints().empty()) ||
(int)data.keypoints().size() != data.descriptors().rows ||
(_feature2D->getType() == Feature2D::kFeatureOrbOctree && data.descriptors().empty()))
(!_receivingOdometryFeatures && _feature2D->getType() == Feature2D::kFeatureOrbOctree && data.descriptors().empty()))
{
if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
{
@@ -5292,6 +5302,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
else if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
{
_receivingOdometryFeatures = true;
UINFO("Use odometry features: kpts=%d 3d=%d desc=%d (dim=%d, type=%d)",
(int)data.keypoints().size(),
(int)data.keypoints3D().size(),

11
corelib/src/VWDictionary.cpp
View File

@@ -919,14 +919,21 @@ std::list<int> VWDictionary::addNewWords(
type = _visualWords.begin()->second->getDescriptor().type();
UASSERT(type == CV_32F || type == CV_8U);
}
static std::string moreInfo = uFormat(
"This could happen if the computer doesn't have access to same "
"feature detectors than when the database was created. This could "
"also happen if we enabled \"%s\" but the first frame received "
"was empty, thus features were re-extracted with a different detector "
"than the one used by the odometry.",
Parameters::kMemUseOdomFeatures().c_str());
if(dim && dim != descriptorsIn.cols)
{
UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", descriptorsIn.cols, dim);
UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary (size=%d). %s", descriptorsIn.cols, dim, moreInfo.c_str());
return wordIds;
}
if(type>=0 && type != descriptorsIn.type())
{
UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", descriptorsIn.type(), type);
UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary (type=%d). %s", descriptorsIn.type(), type, moreInfo.c_str());
return wordIds;
}