globalmap.h

InnovLabSimu在vc++下实现

#pragma once

#include <vector>
#include <fstream>
#include "../particleFlt/sir_pf_share.h"
#include "../Global.h"
#include "../Localization.h"
//#include <afxwin.h>
/********************************************************************
	created:	2008/07/11
	created:	11:7:2008   9:20
	filename: 	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2\GlobalMap.h
	file path:	c:\RoboCupSimu\RoboCupSimu2\RoboCupSimu\controllers\Robot2005_V2
	file base:	GlobalMap
	file ext:	h
	author:		GaoYang
	
	purpose:	全局地图类，包括读入地图，从地图中查找值，地图按照webots中仿真的坐标系建立，地图中央为原点，坐标有正负，坐标系方向则是向上为Y轴正向，与仿真中的Z轴反向
	            x轴与仿真中的webots重合
	            而地图文件中是以左上角为原点，右、下方向为正
*********************************************************************/
#define JUDGE_EDGE_POINT_IN_MAP//bool地图中是否单独考虑了障碍物的边界点，若是，则定义它，则读入bool地图是，需注意，地图中的边界点，值不是0，而是2000
typedef std::vector<std::vector<float>> MAP_TYPE;
typedef std::vector<std::vector<bool>> BoolMapType;

#define ONE_PIXEL_LENGTH 50///<每一像素的长度，单位豪米，应与DrawMap中对应
#define PRECISION 100///<地图中存的距离值的有效精度,因为存的是整型,所以以这种方法保证小数点后的精度.实际上单位为毫米的距离=地图值/PRECISION*10，
#define NON_OBJECT_VAL 16777215///<无障碍物点的地图值
#define EDGE_POINT_VAL_IN_BOOL_MAP 2000///<存在二值地图的文件中，表示障碍物点的地图值
class CGlobalMap
{
public:
	CGlobalMap(void);
	CGlobalMap(unsigned Height,unsigned Width);
public:
	~CGlobalMap(void);
	/**
	*从已有文件中读入二值地图，1表示空白，0表示障碍物，本函数从默认文件中读
	*
	*/
	bool ReadBoolMapFromFile(unsigned Height,unsigned Width);
#ifdef JUDGE_EDGE_POINT_IN_MAP
	BoolMapType m_bvEdgePointMap;///<bool地图，是边界点则为真，不是则为假
	/**
	*@brief 是否边界点的查询函数，输入世界坐标系坐标，但要求先转化为像素为单位
	*
	*@parm XCoord 世界坐标系X坐标，但要求先转化为像素为单位
	*@parm YCoord 世界坐标系Y坐标，但要求先转化为像素为单位
	*@output 若是边界点则返回真
	*/
	bool IsEdgePoint(float XCoord,float YCoord)
	{
		if (fabs(XCoord)>X_DisOfWordCord2MapCoord||
			fabs(YCoord)>Y_DisOfWordCord2MapCoord)
		{
			return false;//产生的点超出整个地图范围时，返回不可能
		}
		float MemoryCoordX=X_DisOfWordCord2MapCoord+XCoord;//转化为像素数的图像坐标单位
		float MemoryCoordY=Y_DisOfWordCord2MapCoord-YCoord;//转化为像素数的图像坐标单位
		return  m_bvEdgePointMap[Floor4_Ceil5(MemoryCoordY)][Floor4_Ceil5(MemoryCoordX)];
	}

#endif
	BoolMapType m_BoolGolbalMap;
	MAP_TYPE m_mGolbalMap;///<全局地图，由两个vector嵌套而成，最外层的每一个元素为一行，即m_mGolbalMap[4][5]为下标4行，5列的元素，下标从0开始数起
	size_t X_DisOfWordCord2MapCoord;///<世界地图原点相对图像原点沿地图的图像坐标系的X轴方向偏移
	size_t Y_DisOfWordCord2MapCoord;///<世界地图原点相对图像原点,沿地图的图像坐标系的Y轴方向偏移
	/**
	*从已有文件中读入地图，本函数从默认文件中读,指定图的宽度和高度
	*
	*/
	bool ReadMapFromFile(unsigned Height,unsigned Width);
	/**
	*从已有地图中，依据X,Y坐标查询地图对应点的误差距离值（地图中对应点存的值）
	*
	*/
	//float QueryGridValOnMap(float XCoord,float YCoord);
	/**
	*从已有地图中，依据X,Y坐标查询地图对应点的误差距离值（地图中对应点存的值）,当所查询点为一个靠近边缘的点，则地图中直接存了距离值取出即可
	*
	*
	*/
	float QueryAnFreeGridVal(float& XCoord,float& YCoord)
	{
		return QueryGridValOnMap(XCoord,YCoord);
	}
	/**
	*部分额外的数学运算，目前包括四舍五入运算
	*/
	long Floor4_Ceil5(float& NumTobeOperate)
	{
		long IntPart=floor(NumTobeOperate);//取整数部分
		if (NumTobeOperate-IntPart>=0.5)
		{
			return IntPart+1;
		}
		else
		{
			return IntPart;
		}
	}
	/**
	*@brief 在地图中膨胀障碍物范围，扩充障碍物点，获得新的二值地图结果仍然存放在m_BoolGolbalMap
	*
	*@parm Height地图的高度
	*@parm Height地图的宽度
	*@parm ExpandWidthInPixel对障碍物点，需要向外扩充多少个像素，注意单位是像素数
	*/
	void ExpandObstacleOnboolMap(unsigned Height,unsigned Width,unsigned ExpandWidthInPixel);
	/**
	*@brief 从已有地图中，依据X,Y坐标查询二值地图对应点的值（地图中对应点存的值）
	*返回1表示空白，0表示障碍物,越界则返回0(false)
	*/
	bool QueryboolMap(float& XCoord,float& YCoord)
	{
		if (fabs(XCoord)>X_DisOfWordCord2MapCoord||
			fabs(YCoord)>Y_DisOfWordCord2MapCoord)
		{
			return false;//产生的点超出整个地图范围时，返回不可能
		}
		float MemoryCoordX=X_DisOfWordCord2MapCoord+XCoord;//转化为像素数的图像坐标单位
		float MemoryCoordY=Y_DisOfWordCord2MapCoord-YCoord;//转化为像素数的图像坐标单位
		bool tmp=m_mGolbalMap[Floor4_Ceil5(MemoryCoordY)][Floor4_Ceil5(MemoryCoordX)];//测试用，应去掉
		return tmp;	
		
	}
	bool IsAPossibleLocate(CParticle& LocTobeCheck)
	{
		return QueryboolMap(LocTobeCheck.XCoord,LocTobeCheck.YCoord);
		
	}
	typedef bool (CGlobalMap::*pCheckLocPossibleFun)(CParticle& ParticleTobeCheck);
	bool (CGlobalMap::*m_pfCheckPossibleParticle)(CParticle& ParticleTobeCheck) ;///<指向IsAPossibleLocate函数的指针成员
	/**
	*@brief m_mGolbalMap地图中存进来的数据下标是按照0开始的，而输入进来的机器人世界坐标系是原点在中心，有正负，因此本函数
	*输入世界坐标系坐标，返回查询到的坐标值，不经任何运算，仅进行坐标转化，并返回查到的内容,并未考虑输入的坐标越界的情况！！！
	*
	*注意，本函数假设 m_mGolbalMap地图中存进来的数据下标是按照0开始的，原点或者说坐标系以左上角为原点，右、下方向为正
	*而假设输入的绝对世界坐标系则，以webots中仿真的坐标系,地图中央为原点，坐标有正负，坐标系方向则是向上为Y轴正向,
	*X轴方向两坐标系相同，注意在地图中是以像素为单位存的，而输入的直角坐标单位是毫米，一个像素=？毫米？？所以要进行一次转化
	*
	*@todo 万一地图的大小是奇数，则进行本函数转化会有一个像素的误差问题！！
	*/
	float QueryGridValOnMap(float XCoord,float YCoord)
	{
		if (fabs(XCoord)>X_DisOfWordCord2MapCoord||
			fabs(YCoord)>Y_DisOfWordCord2MapCoord)
		{
			return 3.4e8;//产生的点超出整个地图范围时，随便返回的一个大的数，表示不可能
		}
		float MemoryCoordX=X_DisOfWordCord2MapCoord+XCoord;//转化为像素数的图像坐标单位
		float MemoryCoordY=Y_DisOfWordCord2MapCoord-YCoord;//转化为像素数的图像坐标单位
		int MapDisVal=m_mGolbalMap[Floor4_Ceil5(MemoryCoordY)][Floor4_Ceil5(MemoryCoordX)];
		return TransferMapVal2RealDis(MapDisVal);///<将地图存的距离转化为实际单位为毫米的值
	}
	/**
	*@brief地图中存的距离值的是整型数,当初保证小数点后的精度.定义了一个PRECISION参数，所以实际上单位为毫米的实际距离=地图值/PRECISION*10，
	*
	*@parm MapValue 地图中存的距离值
	*/
	float TransferMapVal2RealDis(int MapValue)
	{
		return MapValue/PRECISION*10;
	}
	
};