globalmap.cpp

InnovLabSimu在vc++下实现

#include "GlobalMap.h"

CGlobalMap::CGlobalMap(void)//:m_pfCheckPossibleParticle(&IsAPossibleLocate)
{
//	m_pfCheckPossibleParticle=&CGlobalMap::IsAPossibleLocate;
}
CGlobalMap::CGlobalMap(unsigned Height,unsigned Width)//:m_pfCheckPossibleParticle(IsAPossibleLocate)
{
	if (!ReadMapFromFile(Height,Width))//读地图
	{
		
	}
	ReadBoolMapFromFile(Height,Width);//读二值地图
	//ExpandObstacleOnboolMap(Height,Width,0.5*RADIOUS*1000/ONE_PIXEL_LENGTH);//扩充障碍物点，获得新的二值地图,0.5*RADIOUS*100/ONE_PIXEL_LENGTH为机器人半径的像素大小
	X_DisOfWordCord2MapCoord=0.5*Width;

	Y_DisOfWordCord2MapCoord=0.5*Height;
	
}
CGlobalMap::~CGlobalMap(void)
{
}
bool CGlobalMap::ReadMapFromFile(unsigned Height,unsigned Width)
{
	std::ifstream in("Mapdata.txt");
	for(unsigned row=0; row < Height; row++)
	{
		if (row>=m_mGolbalMap.size())//输入的行数超过了m_mGolbalMap中的行数则添加1行
		{
			std::vector<float>RowVector;//定义一个行的空vector，供m_mGolbalMap压入
			m_mGolbalMap.push_back(RowVector);
		}
		for(unsigned column=0; column < Width; column++)
		{
			float CurrentPixelTmp;
			in >> CurrentPixelTmp;
			m_mGolbalMap[row].push_back(CurrentPixelTmp);//将文件中保存的row行column列的元素压入地图
		//	DCCompatible.SetPixel(column,row,rgb);
		}
	}
	in.close();
	return true;
}
bool CGlobalMap::ReadBoolMapFromFile(unsigned Height,unsigned Width)
{
	std::ifstream in("BoolMap.txt");
//	in.open();
	for(unsigned row=0; row < Height; row++)
	{
		if (row>=m_BoolGolbalMap.size())//输入的行数超过了m_mGolbalMap中的行数则添加1行
		{
			std::vector<bool>RowVector;//定义一个行的空vector，供m_mGolbalMap压入
			m_BoolGolbalMap.push_back(RowVector);
#ifdef JUDGE_EDGE_POINT_IN_MAP//需要考虑边界点的化
			m_bvEdgePointMap.push_back(RowVector);
#endif
			
		}
		for(unsigned column=0; column < Width; column++)
		{
			
			unsigned CurrentPixelTmp;
			in >> CurrentPixelTmp;
#ifdef JUDGE_EDGE_POINT_IN_MAP//需要考虑边界点的化
			if (CurrentPixelTmp==EDGE_POINT_VAL_IN_BOOL_MAP)//找到一个边界点，首先由于该点是障碍物点，所以先把CurrentPixelTmp改为0
			{
				CurrentPixelTmp=0;
				m_bvEdgePointMap[row].push_back(true);
			}
			else
			{
				m_bvEdgePointMap[row].push_back(false);
			}
#endif		
			m_BoolGolbalMap[row].push_back(CurrentPixelTmp);//将文件中保存的row行column列的元素压入地图
			
			//	DCCompatible.SetPixel(column,row,rgb);
		}
	}
	in.close();
	//__asm int 3;
	return true;

}
void CGlobalMap::ExpandObstacleOnboolMap(unsigned Height,unsigned Width,unsigned ExpandWidthInPixel)
{
	BoolMapType bmapAfterExpand(m_BoolGolbalMap);
	for(unsigned row=0; row < Height; row++)
	{
	
		for(unsigned column=0; column < Width; column++)
		{
			if (!m_BoolGolbalMap[row][column])//是障碍物就扩充周边
			{
				//扩充
				for (int i=row-ExpandWidthInPixel;i<row+ExpandWidthInPixel;i++)//行
				{
					for (int j=column-ExpandWidthInPixel;j<column+ExpandWidthInPixel;j++)//列
					{
						if (i>0&&i<Height&&j>0&&j<Width)//没超出范围的话
						{
							bmapAfterExpand[i][j]=0;
						}
					}
				}
			}			//	DCCompatible.SetPixel(column,row,rgb);
		}
	}
	m_BoolGolbalMap=bmapAfterExpand;//用扩充后的图代替扩充前的图
}