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

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) //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;
@@ -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 else
{ {