ros-perception / openslam_gmapping

218 stars 206 forks source link

question about ScanMatcher::score #15

Open asmashine opened 7 years ago

asmashine commented 7 years ago

in this function ,to caculate pfree,there are double freeDelta=map.getDelta()*m_freeCellRatio;and pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle); pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);; obviously r - delta^2sqrt(2) (freeDelta = sqrt(2))
i think r - delta
sqart(2) can show the range of pfree ,but the equation above i don't know its mean.

lmy19880626 commented 7 years ago

me too! I need the explanation.

KOTOKORURU commented 6 years ago

I have the same question ,have you sloved it?

1344618323 commented 5 years ago

me too! I think there's a bug!

topin89 commented 4 years ago

I think this is a bug that was shadowed by another bug, https://github.com/ros-perception/openslam_gmapping/issues/25 making everything about pfree, ipfree, pf and fcell irrelevant.

And yes,

pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);

should be replaced with

pfree.x+=(*r-*freeDelta)*cos(lp.theta+*angle);
pfree.y+=(*r-*freeDelta)*sin(lp.theta+*angle);

With these two bugs solved, gmapping stops looking for best score inside a wall.

topin89 commented 4 years ago

Oh, there is a PR to fix that: https://github.com/ros-perception/openslam_gmapping/pull/10