search

ros-perception / openslam_gmapping

218 stars 206 forks source link

there exit serious bug in funciton "inline double ScanMatcher::score" and inline unsigned int "ScanMatcher::likelihoodAndScore" #25

Open chenxianbo opened 5 years ago

chenxianbo commented 5 years ago 
inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
        .......
    double freeDelta=map.getDelta()*m_freeCellRatio;
    for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
        IntPoint ipfree=map.world2map(pfree);
        for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
        for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
            IntPoint pr=iphit+IntPoint(xx,yy);
            IntPoint pf=pr+ipfree;
                    .......
            //}
        }
    }
}

the line "IntPoint ipfree=map.world2map(pfree);" exist bug, it cannot calculate relative grid position.

IntPoint Map<Cell,Storage,isClass>::world2map(const Point& p) const
{
    return IntPoint( (int)round((p.x-m_center.x)/m_delta)+m_sizeX2, (int)round((p.y-m_center.y)/m_delta)+m_sizeY2);
}

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".

topin89 commented 4 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

topin89 commented 4 years ago

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