mhbase.cpp

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

            {
                /*Bearing selection*/
                pLink = pLinkage->links[uk];
                pRoad = (pNode==pLink->nodes[0]) ? pLink->sections[0] : pLink->sections[pLink->sectionCount-1];
                linkBear = (pNode==pRoad->beginPtr) ? pRoad->roadBear : pRoad->roadBear+Pi;
                angleError = ABS(hypos[ui].finalBear-linkBear);
                while(angleError>2*Pi) angleError -= 2*Pi;
                if(angleError>Pi) angleError = 2*Pi - angleError;
                if(angleError > MaxAngleError) continue;

                /*New hypothesis*/
                for(; uj<=HypoSpace-1; uj++)
                {
                    if(hypos[uj].inUse==0) break;
                }

                hypos[uj].type = accelerating;
                hypos[uj].beginTime = time;
                hypos[uj].link = pLink;
                hypos[uj].direction = (pNode==pLink->nodes[0]) ? positive : negative;
                hypos[uj].turnAngle = hypos[uj].runDis = 0;
                hypos[uj].finalBear = hypos[ui].finalBear;
                hypos[uj].disOnLink = 0;
                hypos[uj].active = hypos[uj].inUse = 1;
                hypos[uj].parent = hypos + ui;
                hypos[uj].weight = hypos[ui].weight + Weigh(hypos[ui],hypos[uj]);
                flags[uj] = 1;
            }

            /*Keep parent hypothesis*/
            hypos[ui].weight += Weigh(hypos[ui],hypos[ui]);
            continue;
        }

        /*Running from accelerating*/
        if(hypos[ui].type==accelerating && dynamic==running)
        {
            #ifdef MHDEBUG
                printf(" a->r!");
            #endif

            pLink = hypos[ui].link;
            pNode = (hypos[ui].direction==positive) ? pLink->nodes[pLink->sectionCount] : pLink->nodes[0];
            pLinkage = submap.linkage + (pNode - submap.nodes);

            for(uk=0,uj=0; uk<=pLinkage->linkCount-1; uk++)
            {
                /*Bearing selection*/
                pLink = pLinkage->links[uk];
                pRoad = (pNode==pLink->nodes[0]) ? pLink->sections[0] : pLink->sections[pLink->sectionCount-1];
                linkBear = (pNode==pRoad->beginPtr) ? pRoad->roadBear : pRoad->roadBear+Pi;
                angleError = ABS(hypos[ui].finalBear-linkBear);
                while(angleError>2*Pi) angleError -= 2*Pi;
                if(angleError>Pi) angleError = 2*Pi - angleError;
                if(angleError > MaxAngleError) continue;

                /*New hypothesis*/
                for(; uj<=HypoSpace-1; uj++)
                {
                    if(hypos[uj].inUse==0) break;
                }
                hypos[uj].type = running;
                hypos[uj].beginTime = time;
                hypos[uj].link = pLink;
                hypos[uj].direction = (pNode==pLink->nodes[0]) ? positive : negative;
                hypos[uj].turnAngle = hypos[uj].runDis = 0;
                hypos[uj].finalBear = linkBear;
                hypos[uj].disOnLink = 0;
                hypos[uj].active = hypos[uj].inUse = 1;
                hypos[uj].parent = hypos + ui;
                hypos[uj].weight = hypos[ui].weight + Weigh(hypos[ui],hypos[uj]);
                flags[uj] = 1;
            }

            /*Hypothesis: still on the same link*/
            for(; uj<=HypoSpace-1; uj++)
            {
                if(hypos[uj].inUse==0) break;
            }
            hypos[uj].type = running;
            hypos[uj].beginTime = time;
            hypos[uj].link = hypos[ui].link;
            hypos[uj].direction = hypos[ui].direction;
            hypos[uj].turnAngle = hypos[uj].runDis = 0;
            hypos[uj].finalBear = hypos[ui].finalBear;
            hypos[uj].disOnLink = hypos[ui].disOnLink;
            hypos[uj].active = hypos[uj].inUse = 1;
            hypos[uj].parent = hypos + ui;
            hypos[uj].weight = hypos[ui].weight + Weigh(hypos[ui],hypos[uj]);
            flags[uj] = 1;

            /*Drop parent hypothesis*/
            hypos[ui].active = 0;

            continue;
        }

        /*Keep running*/
        if(hypos[ui].type==running && dynamic==running)
        {
            #ifdef MHDEBUG
                printf(" r->r!");
            #endif

            if(hypos[ui].disOnLink*(1+MaxDisError) + MaxDisOffset < hypos[ui].link->linkLen)/*Still on the same link*/
            {
                hypos[ui].weight += Weigh(hypos[ui],hypos[ui]);
                continue;
            }

            pLink = hypos[ui].link;
            pNode = (hypos[ui].direction==positive) ? pLink->nodes[pLink->sectionCount] : pLink->nodes[0];
            pLinkage = submap.linkage + (pNode - submap.nodes);

            for(uk=0,uj=0; uk<=pLinkage->linkCount-1; uk++)
            {
                /*Bearing selection*/
                pLink = pLinkage->links[uk];
                pRoad = (pNode==pLink->nodes[0]) ? pLink->sections[0] : pLink->sections[pLink->sectionCount-1];
                linkBear = (pNode==pRoad->beginPtr) ? pRoad->roadBear : pRoad->roadBear+Pi;
                angleError = ABS(hypos[ui].finalBear-linkBear);
                while(angleError>2*Pi) angleError -= 2*Pi;
                if(angleError>Pi) angleError = 2*Pi - angleError;
                if(angleError > MaxAngleError) continue;

                /*New hypothesis*/
                for(; uj<=HypoSpace-1; uj++)
                {
                    if(hypos[uj].inUse==0) break;
                }

                hypos[uj].type = running;
                hypos[uj].beginTime = time;
                hypos[uj].link = pLink;
                hypos[uj].direction = (pNode==pLink->nodes[0]) ? positive : negative;
                hypos[uj].turnAngle = hypos[uj].runDis = 0;
                hypos[uj].finalBear = linkBear;
                hypos[uj].disOnLink = 0;
                hypos[uj].active = hypos[uj].inUse = 1;
                hypos[uj].parent = hypos + ui;
                hypos[uj].weight = hypos[ui].weight + Weigh(hypos[ui],hypos[uj]);
                flags[uj] = 1;
            }

            /*Keep parent hypothesis*/
            hypos[ui].weight += Weigh(hypos[ui],hypos[ui]);

            continue;
        }

        /*Accelerating from running*/
        if(hypos[ui].type==running && dynamic==accelerating)
        {
            #ifdef MHDEBUG
                printf(" r->a!");
            #endif

            /*New hypothesis*/
            for(uj=0; uj<=HypoSpace-1; uj++)
            {
                if(hypos[uj].inUse==0) break;
            }
            hypos[uj].type = accelerating;
            hypos[uj].beginTime = time;
            hypos[uj].link = hypos[ui].link;
            hypos[uj].direction = hypos[ui].direction;
            hypos[uj].turnAngle = hypos[uj].runDis = 0;
            hypos[uj].finalBear = hypos[ui].finalBear;
            hypos[uj].disOnLink = hypos[ui].disOnLink;
            hypos[uj].active = hypos[uj].inUse = 1;
            hypos[uj].parent = hypos + ui;
            hypos[uj].weight = hypos[ui].weight + Weigh(hypos[ui],hypos[uj]);
            flags[uj] = 1;

            /*Drop parent hypothesis*/
            hypos[ui].active = 0;

            continue;
        }

        /*Stopping from accelerating*/
        if(hypos[ui].type==accelerating && dynamic==stopping)
        {
            #ifdef MHDEBUG
                printf(" a->s!");
            #endif

            /*New hypothesis*/
            for(uj=0; uj<=HypoSpace-1; uj++)
            {
                if(hypos[uj].inUse==0) break;
            }
            hypos[uj].type = stopping;
            hypos[uj].beginTime = time;
            hypos[uj].link = hypos[ui].link;
            hypos[uj].direction = hypos[ui].direction;
            hypos[uj].turnAngle = hypos[uj].runDis = 0;
            hypos[uj].finalBear = hypos[ui].finalBear;
            hypos[uj].disOnLink = hypos[ui].disOnLink;
            hypos[uj].active = hypos[uj].inUse = 1;
            hypos[uj].parent = hypos + ui;
            hypos[uj].weight = hypos[ui].weight + Weigh(hypos[ui],hypos[uj]);
            flags[uj] = 1;

            /*Drop parent hypothesis*/
            hypos[ui].active = 0;

            continue;
        }
    }

    /*Calculate active hypothesis count*/
    for(ui=0,activeCount=0; ui<=HypoSpace-1; ui++)
    {
        if(hypos[ui].active==1) activeCount++;
    }

    #ifdef MHDEBUG
        printf("\n");
    #endif

    return MM_OK;
}

/*Update with sensor measurements*/
RETCHECK MHBase::Update(void* sensors[], TIME time)
{

    DynamicStat(sensors,time);                                          /*Dynamic calculation*/

    if(activeCount==0 && gpsAvail==0) return MM_Result;                 /*Cannot match*/

    /*Generate and weigh hypotheses*/
    if(activeCount==0 && gpsAvail==1)
    {
        GenInitialHypos(time);                                          /*Initialization*/
        accumCount = 1;
    }
    else
    {
        UpdateHypos(time);                                              /*Update*/
        accumCount++;
    }

    /*Select hypotheses*/
    UINT setActiveCount = (gpsAvail==1) ? MaxObserved : MaxActive;
    SetActiveHypos(setActiveCount);
    CutOldNodes();

    curTime = time;

    return MM_OK;
}