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

Merge branch 'likelihood_experiment' of https://github.com/borongyuan/rtabmap into borongyuan-likelihood_experiment

Browse Source
This commit is contained in:
matlabbe
2024-05-19 14:57:58 -07:00
parent 256dcbded4 f3cfa7831d
commit c034a9631c
1 changed files with 4 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
corelib/src/Rtabmap.cpp
View File

@@ -5275,15 +5275,14 @@ 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 && mean)
if(value > mean+stdDev && stdDev)
{
iter->second = (value-(stdDev-epsilon))/mean;
iter->second = (value-mean)/stdDev;
if(value > max)
{
max = value;
@@ -5305,9 +5304,9 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
}
}
if(stdDev > epsilon && max)
if(max > mean)
{
likelihood.begin()->second = mean/stdDev + 1.0f;
likelihood.begin()->second = stdDev/(max-mean) + 1.0f;
}
else
{