?? selfloclization.cpp
字號:
#include "SelfLoclization.h"
CSelfLoclization::CSelfLoclization(unsigned MapWidth,unsigned MapHeight):LocRange(400,580,0,70,2.0000001,2*PI),//狀態(tài)的范圍
m_cParticleFlt(PARTICLE_NUM,CSelfLoclization::LocRange),m_cGlobalMap(MapHeight,MapWidth)
{
}
CSelfLoclization::~CSelfLoclization(void)
{
}
bool CSelfLoclization::InitSelfLoc()
{
m_cParticleFlt.InitParticleFlt(m_cGlobalMap);
//CParticle InitPart;
//InitPart.XCoord=26740;
//InitPart.YCoord=2949.5;
//InitPart.Orientation=1.5*3.14159-(3.14159-2.912);
//InitPart.Weight=1;
////m_cParticleFlt.InitParticleFlt(InitPart);
//m_cParticleFlt.InitParticleFltWithOneInitLoc(InitPart,m_cGlobalMap);
return true;
}
RobotLoc CSelfLoclization::LoclizeSelf(CCoderBasePosePredict& PosePredictMod,NoisyWheelDis<uniform_random_type1> WheelDisVal,CGlobalMap &GlobalMap)
{
RobotLoc SelfRobotLoc;
if (m_cParticleFlt.RunFlt(PosePredictMod,WheelDisVal,GlobalMap,m_LaserData,NumOfData,m_cResamplerForPF))//定位成功運(yùn)行
{
SelfRobotLoc=m_cParticleFlt.GetStateExpect();
}
else//定位不成功
{
//沒定義
}
return SelfRobotLoc;
}
RobotLoc CSelfLoclization::LoclizeSelf(NoisyWheelDis<uniform_random_type1> WheelDisVal,float* pLaserData)
{
RobotLoc SelfRobotLoc;
if (m_cParticleFlt.RunFlt(m_cPosePredictMod,WheelDisVal,m_cGlobalMap,pLaserData,NumOfData,m_cResamplerForPF))//定位成功運(yùn)行
{
SelfRobotLoc=m_cParticleFlt.GetStateExpect();
}
else//定位不成功
{
//沒定義
}
return SelfRobotLoc;
}
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -