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

[chenxianbo]

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]

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 #15

[topin89]

Oh, there is a PR to fix that: #10