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

Added parameter Rtabmap/VirtualPlaceLikelihoodRatio to select how likelihood is normalized/adjusted (0=default old approach used in paper).

Browse Source
This commit is contained in:
matlabbe
2024-05-19 17:46:00 -07:00
parent c034a9631c
commit 995f65b2ec
5 changed files with 103 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
corelib/src/Rtabmap.cpp
View File

@@ -103,6 +103,7 @@ Rtabmap::Rtabmap() :
_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
_loopThr(Parameters::defaultRtabmapLoopThr()),
_loopRatio(Parameters::defaultRtabmapLoopRatio()),
_virtualPlaceLikelihoodRatio(Parameters::defaultRtabmapVirtualPlaceLikelihoodRatio()),
_maxLoopClosureDistance(Parameters::defaultRGBDMaxLoopClosureDistance()),
_verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()),
_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
@@ -567,6 +568,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
Parameters::parse(parameters, Parameters::kRtabmapVirtualPlaceLikelihoodRatio(), _virtualPlaceLikelihoodRatio);
Parameters::parse(parameters, Parameters::kRGBDMaxLoopClosureDistance(), _maxLoopClosureDistance);
Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis);
Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
@@ -5275,36 +5278,37 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
//Adjust likelihood with mean and standard deviation (see Angeli phd)
float epsilon = 0.0001;
float max = 0.0f;
int maxId = 0;
for(std::map<int, float>::iterator iter=++likelihood.begin(); iter!= likelihood.end(); ++iter)
{
float value = iter->second;
if(value > mean+stdDev && stdDev)
iter->second = 1.0f;
if(value > mean+stdDev)
{
iter->second = (value-mean)/stdDev;
if(value > max)
if(_virtualPlaceLikelihoodRatio==0 && mean)
{
max = value;
maxId = iter->first;
iter->second = (value-(stdDev-epsilon))/mean;
}
else if(_virtualPlaceLikelihoodRatio!=0 && stdDev)
{
iter->second = (value-mean)/stdDev;
}
}
else if(value == 1.0f && stdDev == 0)
if(value > max)
{
iter->second = 1.0f;
if(value > max)
{
max = value;
maxId = iter->first;
}
}
else
{
iter->second = 1.0f;
max = value;
maxId = iter->first;
}
}
if(max > mean)
if(_virtualPlaceLikelihoodRatio==0 && stdDev > epsilon && max)
{
likelihood.begin()->second = mean/stdDev + 1.0f;
}
else if(_virtualPlaceLikelihoodRatio!=0 && max > mean)
{
likelihood.begin()->second = stdDev/(max-mean) + 1.0f;
}