multihypo.h

基于多假设的地图匹配算法。程序能够根据车辆在行驶过程中收集到的GPS/DR数据正确得到当前车辆所在的道路位置。

/******************************************************************************

    File Name:   MH/MultiHypo.h
    Description: Hypothesis tree structure,
                 for multi-hypothesis map matching algorithms

******************************************************************************/

/******************************************************************************

    Author: [e-mail]	
    Upadate:
        2008/09/18  File founded

    Copyright 2008-2009 Robot Lab., Dept.of Automation, Tsinghua University

******************************************************************************/

#ifndef __MULTIHYPO_H__
#define __MULTIHYPO_H__

#include "../map/SubMap.h"
#include "../common/Interface.h"
#include "../common/Integration.h"

/******************************                   Hypothesis tree node                      ***************************************************/
/*Tree size limit*/
const UINT MaxTreeDeg = 4;                                              /*Maximum degree of tree nodes*/

/*Tree node structure*/
typedef struct HypoNode : public PERIOD
{
    /*Data fields*/
    DATATYPE        finalBear;                                          /*Final bearing of the period*/
    DATATYPE        disOnLink;                                          /*Distance run on the link*/
    DATATYPE        weight;                                             /*Weight of the hypothesis*/

    /*Flag fields*/
    bool            inUse;                                              /*Label whether the node is in use*/
    bool            active;                                             /*Active label of the node: whether allowed to generate children nodes*/

    /*Pointer fields*/
    HypoNode*       parent;                                             /*Link to parent node*/

    HypoNode():weight(0),inUse(0),active(0),parent(NULL){};             /*Default constructor*/
}HYPO,*HYPOPTR;


/******************************                   Operations with the hypothesis tree       ***************************************************/
/*Hypothesis parameters*/
const UINT MaxHypoLength = 50;                                          /*Maximum length of one hypothesis*/
const UINT MaxActive = 1000;                                             /*Maximum active hypothesis count*/
const UINT MaxObserved = 200;                                            /*Maximum active hypothesis count after an observation*/
const UINT HypoSpace = MaxActive * MaxHypoLength * (MaxTreeDeg+1);      /*Maximum hypothesis node count*/
const DATATYPE WeightSame = DATATYPE(1e-20);                            /*Computational threshould to determine weights almost the same*/
//const DATATYPE WeightGain = DATATYPE(0.12);                             /*Parameter in belief calculation*/

/*Multi-hypothesis class*/
typedef class MultiHypo : public Interface
{
protected:
    UINT            accumCount;                                         /*Step count of weight accumulation*/
    HypoNode        hypos[HypoSpace];                                   /*Array of hypothesis nodes*/
    int             flags[HypoSpace];                                   /*Array of assistant flags*/
    UINT            activeCount;                                        /*Count of active hypotheses*/
    HypoNode*       bestHypo;                                           /*Best hypothesis*/

    void GetBestHypo(void);                                             /*Find the best hypothesis*/
    void SetActiveHypos(UINT number);                                   /*Set active hypotheses according to a number*/
    void CutOldNodes(void);                                             /*Cut off old nodes not in use*/

public:
    MultiHypo();                                                        /*Default constructor*/
    ~MultiHypo()=0;                                                     /*Default destructor*/

    RETCHECK GetResult(TIME time, RESULT& result);                      /*Matching result at given time*/
    UINT GetRoute(UINT count, ROUTE route[]);                           /*Recent route, including several links according to count*/
}MultiHypo;

#endif /*__MULTIHYPO_H__*/