Open chenxianbo opened 5 years ago
Yeah, you're right. Good thing this can be fixed by changing lines
pfree=pfree-phit;
...
IntPoint pf=pr+ipfree;
to
//pfree=pfree-phit;
...
IntPoint pf=ipfree+IntPoint(xx,yy);
Looks like the whole point of ipfree is to make sure there is empty space right before the wall, so we don't score anything inside of it.
So another way to fix it is to drop fcell
from the condition:
if (((double)cell )> m_fullnessThreshold /*&& ((double)fcell )<m_fullnessThreshold*/)
Also, see https://github.com/ros-perception/openslam_gmapping/issues/15
Oh, there is a PR to fix that: https://github.com/ros-perception/openslam_gmapping/pull/10
the line "IntPoint ipfree=map.world2map(pfree);" exist bug, it cannot calculate relative grid position.
assume pfree = (0.1, 0.1), m_center=(0,0),m_delta = 0.05, m_sizeX2 = 200,m_sizeY2=200,then calculate ipfree=(202,202) so in function "socre", the distance between pr and pf is about ipfree=(202,202),it isn't right,so do in the function "likelihoodAndScore".