i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

adjust likelihood using z-score

Browse Source
This commit is contained in:
Borong Yuan
2024-04-28 12:22:26 +08:00
parent 27dea71398
commit f3cfa7831d
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

@@ -5265,15 +5265,14 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
//Adjust likelihood with mean and standard deviation (see Angeli phd) //Adjust likelihood with mean and standard deviation (see Angeli phd)
float epsilon = 0.0001;
float max = 0.0f; float max = 0.0f;
int maxId = 0; int maxId = 0;
for(std::map<int, float>::iterator iter=++likelihood.begin(); iter!= likelihood.end(); ++iter) for(std::map<int, float>::iterator iter=++likelihood.begin(); iter!= likelihood.end(); ++iter)
{ {
float value = iter->second; 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) if(value > max)
{ {
max = value; max = value;
@@ -5295,9 +5294,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 else
{ {