getmeasureprobability.h

InnovLabSimu在vc++下实现

#pragma once
/********************************************************************
	created:	2008/07/13
	created:	13:7:2008   11:55
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\GetMeasureProbability.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2
	file base:	GetMeasureProbability
	file ext:	h
	author:		GaoYang
	
	purpose:	专门用来实现坐标变换的类，包括输入一个局部坐标，获得该坐标的全局坐标
*********************************************************************/
#include "../Localization.h"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include "../PathPlan/ShareObject.h"
#include "../PathPlan/ObjectAngleCheck.h"

typedef boost::numeric::ublas::vector<double> STATE_STYLE;
typedef boost::numeric::ublas::matrix<double> MatrixStyle;


class CCoordinateTrans:public ObjectAngleCheck
{
public:
	
	/**
	*@输入局部坐标系中的极坐标,角度单位为弧度，获得世界坐标系中的x,y坐标，局部坐标系定义为，正前方90度，右方为0度
	*返回计算得到的世界坐标系中的x,y坐标STATE_STYLE类型，第一个元素是x
	*/
	STATE_STYLE LocalRadPoleCoord2WorldCoord(double AngleInRad,float Dis,RobotLoc&SelfLoc)
	{
		return LocalVerticalCoord2WorldCoord(Dis*cos(AngleInRad),Dis*sin(AngleInRad),SelfLoc);		
	}
	/**
	*@输入局部坐标系中的极坐标,角度单位为度，获得世界坐标系中的x,y坐标，局部坐标系定义为，正前方90度，右方为0度
	*返回计算得到的世界坐标系中的x,y坐标STATE_STYLE类型，第一个元素是x,单位毫米
	*/
	STATE_STYLE LocalDegreePoleCoord2WorldCoord(float AngleInDeg,float Dis,RobotLoc&SelfLoc)
	{
		double AngleInRad=TransAngleToArc(AngleInDeg);
		return LocalVerticalCoord2WorldCoord(Dis*cos(AngleInRad),Dis*sin(AngleInRad),SelfLoc);		
	}
	/**
	*@输入局部坐标系中的直角坐标，获得世界坐标系中的x,y坐标，局部坐标系定义为，正前方90度，右方为0度
	*返回计算得到的世界坐标系中的x,y坐标STATE_STYLE类型，第一个元素是x，单位毫米，弧度
	*/
	STATE_STYLE LocalVerticalCoord2WorldCoord(float X_Local,float Y_Local,RobotLoc&SelfLoc)
	{
		//置旋转矩阵
		double SinTheta=sin(SelfLoc.Orientation-HALF_PI);
		double CosTheta=cos(SelfLoc.Orientation-HALF_PI);
		MatrixStyle TranslateMatrix(2,2);
		TranslateMatrix(0,0)=CosTheta;
		TranslateMatrix(0,1)=-SinTheta;
		TranslateMatrix(1,0)=SinTheta;
		TranslateMatrix(1,1)=CosTheta;
		STATE_STYLE TargetPointCoord(2);
		////////////////////置目标点的直角局部坐标//////////////////////////////////////////////////////
		TargetPointCoord(0)=X_Local;
		TargetPointCoord(1)=Y_Local;
		STATE_STYLE TargetPointWordCoord(2);
		STATE_STYLE SelfCoord(2);//自身世界坐标
		SelfCoord(0)=SelfLoc.XCoord;
		SelfCoord(1)=SelfLoc.YCoord;
		TargetPointWordCoord=SelfCoord+prod(TranslateMatrix,TargetPointCoord);
		return TargetPointWordCoord;
	}

protected:
private:
};
/********************************************************************
	created:	2008/07/13
	created:	13:7:2008   22:43
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\GetMeasureProbability.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2
	file base:	GetMeasureProbability
	file ext:	h
	author:		GaoYang
	
	purpose:	从分布中获得可能性
*********************************************************************/
typedef double PROBABILITY_TYPE;
#define MESURE_NOISE_DEVIATION 1500//观测噪声的方差 单位毫米

class CProbOfDistribution
{
public:
#define NORMALIZE_FACTOR_IN_SINGLE_DIRECT  5///<对每一角度上得到的概率乘此系数，以防每个角度的概率太小，导致中的测量概率太小。
	CProbOfDistribution()
	{
		SqrtTwoPi=sqrt(2*PI);
		NormalizerOfNormalDistribution=1.0/Prob_StandardNormalDistribution(0,MESURE_NOISE_DEVIATION);
	}
	~CProbOfDistribution()
	{

	}
	/**
	*依据输入的x值，基于标准正态分布的pdf计算该x值的概率，StandardDeviation为均方差，或者标准差，
	*/
	PROBABILITY_TYPE Prob_StandardNormalDistribution(double x,double StandardDeviation)
	{
		return 1/(SqrtTwoPi*StandardDeviation)*exp(-pow(x,2)/(2*pow(StandardDeviation,2)));
	}
protected:
	double SqrtTwoPi;//2pi开方
	double NormalizerOfNormalDistribution;///<由于Prob_StandardNormalDistribution在所得结果最大值远小于1，因此用此系数乘以Prob_StandardNormalDistribution结果获得归一化，使最大值为1
private:
};
/********************************************************************
	created:	2008/07/12
	created:	12:7:2008   22:46
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\GetMeasureProbability.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2
	file base:	GetMeasureProbability
	file ext:	h
	author:		GaoYang
	
	purpose:	粒子滤波器用的，通过输入的机器人位置、扫描数据，查地图，利用likelyhood_field_range_finder_model
	            模型计算该位置时，获得该测量值的可能性
*********************************************************************/
#include "../SelfLoc/GlobalMap.h"
#include "../PathPlan/LocalMap.h"
typedef boost::numeric::ublas::vector<double> STATE_STYLE;
typedef boost::numeric::ublas::matrix<double> MatrixStyle;

#define MESURE_NOISE_WEIGHT 0.8//正常的观测噪声在最后模型的权重
#define RANDOM_NOISE_WEIGHT 0.2//平均分布的随即噪声在最后模型的权重

class CGetMeasureProbability:public CCoordinateTrans,CProbOfDistribution
{
public:
	CGetMeasureProbability(void)
	{
		m_AverageProb=1.0/MAX_DETECT_RANGE;
		Weighted_RandomNoise=m_AverageProb*RANDOM_NOISE_WEIGHT;
	}
public:
	~CGetMeasureProbability(void)
	{

	}
	/**
	*@brief 基于likelyhood_field_range_finder_model模型计算在SelfLoc处，获得ScannerData作为观测值的可能性，返回该可能性
	*
	*@parm ScannerData 扫描仪所的数据的指针
	*@parm ScanDataSize 扫描仪所的数据的个数，从1开始数起
	*@parm SelfLoc 机器人自身位姿
	*@parm GlobalMap 全局地图
	*/
	PROBABILITY_TYPE GetProbOnLikelihoodFieldMod(float* ScannerData,size_t ScanDataSize,RobotLoc& SelfLoc,CGlobalMap& GlobalMap);
	PROBABILITY_TYPE m_AverageProb;///<针对不确定点的平均概率，在构造函数中赋值
	PROBABILITY_TYPE Weighted_RandomNoise;///<包含了权重RANDOM_NOISE_WEIGHT 的平均分布随即噪声带来的可能值，GetProbOnLikelihoodFieldMod中用到
	//void TransferLocal
		
};