selfloclization.h

InnovLabSimu在vc++下实现

#pragma once
/********************************************************************
	created:	2008/07/16
	created:	16:7:2008   18:07
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\SelfLoclization.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2
	file base:	SelfLoclization
	file ext:	h
	author:		GaoYang
	
	purpose:	基于粒子滤波器的自定位程序，将本类实例化将可以实现自定位
*********************************************************************/
#include "CoderBasePosePredict.h"
#include "../PathPlan/LocalMap.h"
#include "../particleFlt/ParticleFilt.h"
#include "../Localization.h"
#include "../particleFlt/GetMeasureProbability.h"
#define PARTICLE_NUM 2000

//typedef boost::mt19937 base_generator_type;
//typedef boost::uniform_real<> distribution_type;
typedef boost::variate_generator<base_generator_type1&, distribution_type1 > uniform_random_type1;

class CSelfLoclization
{
public:
	CSelfLoclization(unsigned MapWidth=1300,unsigned MapHeight=325);
public:
	~CSelfLoclization(void);
	/**
	*@brief 初始化粒子滤波器等，仅需运行一次
	*
	*/
	bool InitSelfLoc();
	/**
	*@brief 接口函数，进行基于粒子滤波器的自定位,所有用到的信息从外部获取
	*/
	RobotLoc LoclizeSelf(CCoderBasePosePredict& PosePredictMod,NoisyWheelDis<uniform_random_type1> WheelDisVal,CGlobalMap &GlobalBoolMap);
	
	/**
	*@brief 接口函数，进行基于粒子滤波器的自定位,仅从外部获取测量信息、编码器信息
	*/
	RobotLoc LoclizeSelf(NoisyWheelDis<uniform_random_type1> WheelDisVal,float* pLaserData);
	const PF_SPA::StateRange LocRange;//各维状态的空间范围
protected:
	CGlobalMap m_cGlobalMap;
	CCoderBasePosePredict m_cPosePredictMod;///<基于编码器推测自身位置的数学模型类，给定位用
	CParticleFilt<CParticle,NoisyWheelDis<uniform_random_type1>,CCoderBasePosePredict,CGetMeasureProbability> m_cParticleFlt;///<粒子滤波器实例
	CResampler m_cResamplerForPF;///<粒子滤波器用到的重采样类
	RobotLoc m_SelfLoc;///<自己最后定位的结果
	float m_LaserData[NumOfData];///<雷达数据的存放地,注意，这里的下标角度单位为度，而粒子滤波器中用到的角度一律为弧度！！
	bool SaveLaserData(float* pLaserData)///<保存雷达数据
	{
		for (unsigned i=0;i<NumOfData;i++)
		{
			m_LaserData[i]=*(pLaserData+i);
		}
	}
};