multihypo.cpp

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

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

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

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

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

    Author: [e-mail]	
    Upadate:
        2008/09/18  File founded
        2008/10/13  Belief calculation determined

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

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

#include <math.h>
#include "../MH/MultiHypo.h"

/******************************                   Multi-hypothesis implementations          ***************************************************/
/*Constructor*/
MultiHypo::MultiHypo()
:accumCount(0),activeCount(0),bestHypo(NULL)
{
    return;
}

/*Destructor*/
MultiHypo::~MultiHypo()
{
    return;
}

/*Find the best hypothesis*/
void MultiHypo::GetBestHypo(void)
{
    HypoNode* p;
    for(p=hypos; p!=hypos+HypoSpace; p++)
    {
        if(p->inUse==1 && p->active==1) break;
    }
    if(p==hypos+HypoSpace)                                              /*No active hypothesis node*/
    {
        bestHypo = NULL;
        return;
    }

    for(bestHypo=p; p!=hypos+HypoSpace; p++)
    {
        if(p->inUse==1 && p->active==1 && p->weight>bestHypo->weight) bestHypo=p;/*Find maximum weight*/
    }

    while(bestHypo->parent && bestHypo->parent->weight > bestHypo->weight - WeightSame)
    {
        bestHypo = bestHypo->parent;                                    /*Generate a simpler hypothesis*/
    }

    return;
}

/*Set active hypotheses*/
void MultiHypo::SetActiveHypos(UINT number)
{
    if(activeCount<=number) return;                                     /*No elimination needed*/

    /*First round*/
    UINT ui,uj,ref,newActive;
    for(ui=0; ui<=HypoSpace-1; ui++) flags[ui] = 0;                     /*Set flags zero*/
    for(ref=0; hypos[ref].active!=1; ref++);                            /*Find a reference hypothesis*/
    for(ui=ref,newActive=0; ui<=HypoSpace-1; ui++)                      /*Set hypotheses for the first round*/
    {
        if(hypos[ui].active==0) continue;                               /*Not active*/
        if(hypos[ui].weight > hypos[ref].weight)
        {
            flags[ui] = 1;
            newActive++;
        }
        else flags[ui] = -1;
    }

    /*Fast search division*/
    while(newActive!=number)
    {
        if(newActive<number)
        {
            for(ref=0; flags[ref]!=-1; ref++);                          /*Find a reference hypothesis*/
            for(ui=ref,uj=0; ui<=HypoSpace-1; ui++)                     /*Set other hypotheses*/
            {
                if(flags[ui]==0 || ABS(flags[ui])==2) continue;         /*Not active or already determined*/
                if(flags[ui]==1)
                {
                    flags[ui] = 2;                                      /*Determination*/
                }
                else
                {
                    if(hypos[ui].weight > hypos[ref].weight)
                    {
                        flags[ui] = 1;
                        uj++;
                    }
                }
            }
            if(uj==0)
            {
                flags[ref] = 2;                                         /*Determination*/
                uj++;
            }
            newActive += uj;
        }
        else
        {
            for(ref=0; flags[ref]!=1; ref++);                           /*Find a reference hypothesis*/
            for(ui=ref,uj=0; ui<=HypoSpace-1; ui++)                     /*Set other hypotheses*/
            {
                if(flags[ui]==0 || ABS(flags[ui])==2) continue;         /*Not active or already determined*/
                if(flags[ui]==-1)
                {
                    flags[ui] = -2;                                     /*Determination*/
                }
                else
                {
                    if(hypos[ui].weight < hypos[ref].weight)
                    {
                        flags[ui] = -1;
                        uj++;
                    }
                }
            }
            if(uj==0)
            {
                flags[ref] = -2;                                        /*Determination*/
                uj++;
            }
            newActive -= uj;
        }
    }

    /*Set active label*/
    for(ui=0; ui<=HypoSpace-1; ui++)
    {
        if(flags[ui]==-2) hypos[ui].active = 0;
    }

    return;
}

/*Set active hypotheses*/
void MultiHypo::CutOldNodes(void)
{
    UINT ui,uj;
    HypoNode* p;

    for(ui=0; ui<=HypoSpace-1; ui++) hypos[ui].inUse = 0;               /*Set in use label zero*/
    for(ui=0; ui<=HypoSpace-1; ui++)
    {
        if(hypos[ui].active==0) continue;
        for(uj=0,p=hypos+ui; p && uj<=MaxHypoLength-1; uj++,p=p->parent) p->inUse = 1;
        if(p) p->parent = NULL;
    }

    return;
}

/******************************                   Interface function implementations        ***************************************************/
/*Matching result at given time*/
RETCHECK MultiHypo::GetResult(TIME time, RESULT& result)
{
    GetBestHypo();
    HypoNode* p;
    for(p=bestHypo; p; p=p->parent)
    {
        if(p->beginTime<=time) break;
    }
    if(p==NULL) return MM_Result;                                       /*Result not available, target memory not modified*/

    result.time = time;
    result.linkID = p->link->id;
    result.direction = p->direction;

    /*Calculate belief*/
    DATATYPE secondWt;
    UINT ui;
    for(ui=0,secondWt=1; ui<=HypoSpace-1; ui++)
    {
        if(hypos[ui].active!=1 || bestHypo==hypos+ui) continue;
//        if(hypos[ui].link==bestHypo->link && hypos[ui].parent==bestHypo->parent) continue;
        if(secondWt>0 || hypos[ui].weight>secondWt) secondWt = hypos[ui].weight;
    }
//    if(secondWt>0) secondWt = bestHypo->weight*2;
    result.belief = 1 - exp( (secondWt - bestHypo->weight) );

    return MM_OK;
}

/*Recent route, including several links according to count, return actual count in return value*/
UINT MultiHypo::GetRoute(UINT count, ROUTE route[])
{
    GetBestHypo();
    if(bestHypo==NULL || count==0) return 0;                            /*Result not available, target memory not modified*/

    HypoNode* p;
    UINT ui;
    route[0].linkID = bestHypo->link->id;
    route[0].direction = bestHypo->direction;
    route[0].beginTime = bestHypo->beginTime / 1000 +1;

    for(p=bestHypo,ui=0; p; p=p->parent)
    {
        if(p->link->id==route[ui].linkID && p->direction==route[ui].direction) route[ui].beginTime = p->beginTime / 1000 +1;
        else
        {
            ui++;
            if(ui>count)
            {
                ui--;
                break;
            }

            route[ui].beginTime = p->beginTime / 1000 +1;
            route[ui].linkID = p->link->id;
            route[ui].direction = p->direction;
        }
    }

    return ui+1;                                                        /*The number of links given by this function*/
}