mhbase.cpp

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

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

    File Name:   MH/MHBase.cpp
    Description: Hypothesis generation,
                 for multi-hypothesis map matching algorithms

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

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

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

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

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

#include <stdio.h>
#include "../include/GPS.h"
#include "../include/IMU.h"
#include "../include/Speedometer.h"
#include "../map/SubMap.h"
#include "../MH/MHBase.h"

#define _MHDEBUG

/******************************                   Multi-hypothesis matching implementations ***************************************************/
/*Constructor*/
MHBase::MHBase()
:gpsAvail(0),dynamic(initial)
{
    return;
}

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

/*Calculate dynamic status according to sensor measurements*/
RETCHECK MHBase::DynamicStat(void* sensors[], TIME time)
{
    UINT ui;
    DATATYPE temp;
    TIME t;

    /*First sensor is GPS, sometimes available, take only one*/
    gpsAvail = (sensors[0]!=NULL);
    if(gpsAvail==1)
    {
        GPS* pGPS = (GPS*) sensors[0];
        GPS2Coord(*pGPS, GPSCenter, coord);
    }

    /*Second sensor is IMU, always available, take a sequence*/
    IMURAW* pIMURAW = (IMURAW*) sensors[1];
    IMU imu;
    for(ui=0,temp=0,t=curTime; t<time; ui++,t+=IMUPeriod)
    {
        IMURAW2IMU(pIMURAW[ui], imu);
        temp += imu.angularVelocity;
    }
    DATATYPE angVel = temp / ui;
    turnAngle = angVel * (time-curTime)/1000;

    /*Third sensor is speedometer, always available, take a sequence*/
    VELRAW* pVELRAW = (VELRAW*) sensors[2];
    VEL vel;
    for(ui=0,temp=0,t=curTime; t<time; ui++,t+=VELPeriod)
    {
        VELRAW2VEL(pVELRAW[ui], vel);
        temp += vel;
    }
    DATATYPE velocity = temp / ui;
    runDis = velocity * (time-curTime)/1000;

    /*Determine dynamic*/
    if(velocity<StopVel && ABS(angVel)<StopAngVel)
    {
        dynamic = stopping;
    }
    else if(velocity>RunVel && ABS(angVel)<RunAngVel)
    {
        dynamic = running;
    }
    else
    {
        dynamic = accelerating;
    }

    return MM_OK;
}

/*Generate initial hypotheses (after dynamics calculated)*/
RETCHECK MHBase::GenInitialHypos(TIME time)
{
    UINT ui,uj;
    for(ui=0; ui<=HypoSpace-1; ui++) hypos[ui].active = hypos[ui].inUse = 0;/*Reset*/

    DATATYPE disSq,disBegin,disEnd,bear;
    for(uj=0,ui=0; uj<=submap.linkCount-1; uj++)
    {
        submap.links[uj].ProjectToLink(coord,disSq,disBegin,disEnd,bear);/*Calculate projection*/
        if(disSq > InitialDisSq) continue;

        /*Forward hypothesis*/
        hypos[ui].type = dynamic;
        hypos[ui].beginTime = time;
        hypos[ui].link = submap.links + uj;
        hypos[ui].direction = positive;
        hypos[ui].turnAngle = hypos[ui].runDis = 0;
        hypos[ui].finalBear = bear;
        hypos[ui].disOnLink = disBegin;
        hypos[ui].active = hypos[ui].inUse = 1;
        hypos[ui].parent = NULL;
        hypos[ui].weight = 0;
        hypos[ui].weight += Weigh(hypos[ui],hypos[ui]);
        ui++;
        if(ui>=HypoSpace) break;                                        /*Memory protection*/

        /*Backward hypothesis*/
        hypos[ui].type = dynamic;
        hypos[ui].beginTime = time;
        hypos[ui].link = submap.links + uj;
        hypos[ui].direction = negative;
        hypos[ui].turnAngle = hypos[ui].runDis = 0;
        hypos[ui].finalBear = (bear>0) ? bear-Pi : bear+Pi;
        hypos[ui].disOnLink = disEnd;
        hypos[ui].active = hypos[ui].inUse = 1;
        hypos[ui].parent = NULL;
        hypos[ui].weight = 0;
        hypos[ui].weight += Weigh(hypos[ui],hypos[ui]);
        ui++;
        if(ui>=HypoSpace) break;                                        /*Memory protection*/
    }

    if(ui>=HypoSpace)
    {
        activeCount = HypoSpace;
        return MM_Space;
    }

    activeCount = ui;

    return MM_OK;
}

/*Update hypotheses after a move (after dynamics calculated)*/
RETCHECK MHBase::UpdateHypos(TIME time)
{
    UINT ui,uj,uk;
    LINKPTR pLink;
    NODEPTR pNode;
    LINKAGEPTR pLinkage;
    ROADPTR pRoad;
    DATATYPE linkBear,angleError;

    for(ui=0; ui<=HypoSpace-1; ui++) flags[ui] = 0;                     /*Set flags zero*/
    for(ui=0; ui<=HypoSpace-1; ui++)
    {
        if(hypos[ui].active==0 || flags[ui]==1) continue;               /*inactive hypos*/

        /*Angle update*/
        hypos[ui].turnAngle += turnAngle;
        if(hypos[ui].turnAngle > Pi) hypos[ui].turnAngle -=2*Pi;
        if(hypos[ui].turnAngle < -Pi) hypos[ui].turnAngle +=2*Pi;
        hypos[ui].finalBear += turnAngle;
        if(hypos[ui].finalBear > Pi) hypos[ui].finalBear -=2*Pi;
        if(hypos[ui].finalBear < -Pi) hypos[ui].finalBear +=2*Pi;

        /*distance update*/
        if(dynamic!=stopping)
        {
            hypos[ui].runDis += runDis;
            hypos[ui].disOnLink += runDis;
        }

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

            hypos[ui].weight += Weigh(hypos[ui],hypos[ui]);
            continue;
        }

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

            /*Same direction*/
            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;

            /*Opposite direction*/
            for(; 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==positive) ? negative : positive;
            hypos[uj].turnAngle = hypos[uj].runDis = 0;
            hypos[uj].finalBear = hypos[ui].finalBear;
            hypos[uj].disOnLink = hypos[ui].link->linkLen - 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 accelerating*/
        if(hypos[ui].type==accelerating && dynamic==accelerating)
        {
            #ifdef MHDEBUG
                printf(" a->a!");
            #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++)