getmeasureprobability.cpp

来自「InnovLabSimu在vc++下实现」· C++ 代码 · 共 46 行

#include "GetMeasureProbability.h"


PROBABILITY_TYPE CGetMeasureProbability::GetProbOnLikelihoodFieldMod(float* ScannerData,size_t ScanDataSize,RobotLoc& SelfLoc,CGlobalMap& GlobalMap)
{
	//double Probability=1;
	ScanDataSize=ScanDataSize-1;//改为从0开始数起
	PROBABILITY_TYPE PSingleDirec;//单一方向测量值的概率
	PROBABILITY_TYPE PAllMeasure=1;//所有测量值，总的概率
	for (unsigned i=0;i<ScanDataSize;i++)
	{
		if (*(ScannerData+i)<MAX_DETECT_RANGE)//检测到物体，反求该检测的概率，但是当没有检测到物体，即方向距离值为MAX_DETECT_RANGE时，还没有考虑到该怎么办。
		{
			
			STATE_STYLE PointWorldCoord=LocalDegreePoleCoord2WorldCoord(i*ANGLE_RESOLVING, *(ScannerData+i),SelfLoc);//将测量值转化为世界坐标
			PointWorldCoord(0)=SelfLoc.TransferRealCoord2MapCoord(PointWorldCoord(0));//将测量点的实际坐标转化为像素单位去查地图
			PointWorldCoord(1)=SelfLoc.TransferRealCoord2MapCoord(PointWorldCoord(1));//将测量点的实际坐标转化为像素单位去查地图,调用的函数是robotloc的成员函数
			float TargetValueOnMap=GlobalMap.QueryGridValOnMap(PointWorldCoord(0),PointWorldCoord(1));//返回的是距离值，距离的单位是毫米
			if (TargetValueOnMap<1e-9)//若查到该目标点是黑色，即认为该点不靠近任何边界，
			{
				if (GlobalMap.IsEdgePoint(PointWorldCoord(0),PointWorldCoord(1)))//看看是否一个障碍物的边界点
				{
					PSingleDirec=1;
				}
				else//属障碍物内部，不靠边界的不确定点，统一设定概率
				{
					PSingleDirec=NORMALIZE_FACTOR_IN_SINGLE_DIRECT*m_AverageProb;
				}
				
			}
			else if (TargetValueOnMap==GlobalMap.TransferMapVal2RealDis(NON_OBJECT_VAL))//确定为非障碍物点
			{
				PSingleDirec=0.00001;
			}
			else//计算概率
			{
				PROBABILITY_TYPE Phit=Prob_StandardNormalDistribution(TargetValueOnMap,MESURE_NOISE_DEVIATION);
				PSingleDirec=NORMALIZE_FACTOR_IN_SINGLE_DIRECT*(Weighted_RandomNoise+NormalizerOfNormalDistribution*Phit*MESURE_NOISE_WEIGHT);
			}
			PAllMeasure=PAllMeasure*PSingleDirec;
		}	
		
	}
	return PAllMeasure;
}