?? mhgpsdr.cpp
字號:
/****************************************************************************** File Name: MH/MHGPSDR.cpp Description: Multi-hypothesis map matching Calculate weights with GPS, bearing and odometry distance ******************************************************************************//****************************************************************************** Author: alfred.liushu@gmail.com Upadate: 2008/09/21 File founded Copyright 2008-2009 Robot Lab., Dept.of Automation, Tsinghua University******************************************************************************/#include "../map/SubMap.h"#include "../MH/MHGPSDR.h"/****************************** Implementation of GPS-DR MH matching ***************************************************//*Constructor*/MHGPSDR::MHGPSDR(){ return;}/*Destructor*/MHGPSDR::~MHGPSDR(){ return;}/*Weigh hypotheses (after new hypothesis generated)*/DATATYPE MHGPSDR::Weigh(const HypoNode& hypoOld, const HypoNode& hypoNew){ COORD coordOnLink; DATATYPE finalBear; hypoNew.link->PointOnLink(hypoNew.direction, hypoNew.disOnLink, coordOnLink, finalBear); DATATYPE weight = DATATYPE(0); /*GPS weight*/ if(gpsAvail==1) { weight += - coordOnLink.DisSquareTo(coord)/(2*GPSDev*GPSDev); } /*Bearing weight*/ { DATATYPE angleDif; angleDif = ABS(finalBear - hypoNew.finalBear); if(angleDif>Pi) angleDif = 2*Pi-angleDif; weight += - angleDif*angleDif/(2*BearDev*BearDev); if(angleDif>BearPunishThre) weight += BearPunish; } /*Distance weight*/ if(hypoNew.link != hypoOld.link) { DATATYPE DisDif; DisDif = MAX(1 - hypoNew.disOnLink/hypoOld.link->linkLen,0); weight += - DisDif*DisDif/(2*BearDev*BearDev); } return weight;}?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -