submap.cpp

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

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

    File Name:    map/SubMap.cpp
    Description:  implementations of submap structures

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

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

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

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

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

#include "../map/SubMap.h"

/******************************                   Implementation of link structure          ***************************************************/
/*Find a point on the link*/
RETCHECK LINK::PointOnLink(RoadDir dir, DATATYPE dis, COORD& coord, DATATYPE& angle)
{
    if(dis<0 || dir==twoway) return MM_Result;

    DATATYPE leftDis = dis;
    UINT ui;
    if(dir==positive)
    {
        for(ui=0; ui<=sectionCount-1; ui++)
        {
            if(leftDis < sections[ui]->roadLen) break;
            leftDis -= sections[ui]->roadLen;
        }
        if(ui==sectionCount) return MM_Result;
        coord = nodes[ui]->coord + (nodes[ui+1]->coord - nodes[ui]->coord) * (leftDis/sections[ui]->roadLen);
        angle = sections[ui]->roadBear;
    }
    else
    {
        for(ui=sectionCount-1; ui; ui--)
        {
            if(leftDis < sections[ui]->roadLen) break;
            leftDis -= sections[ui]->roadLen;
        }
        if(leftDis > sections[ui]->roadLen) return MM_Result;
        coord = nodes[ui+1]->coord + (nodes[ui]->coord - nodes[ui+1]->coord) * (leftDis/sections[ui]->roadLen);
        angle = sections[ui]->roadBear + Pi;
        if(angle>Pi) angle -= 2*Pi;
    }

    return MM_OK;
}

/*Calculate distance square from a point to the link*/
RETCHECK LINK::ProjectToLink(const COORD& coord, DATATYPE& minDisSq, 
                             DATATYPE& disToBegin, DATATYPE& disToEnd, DATATYPE& bear)
{
    UINT ui,ref;
    DATATYPE disOnTrack,disOffTrack;                                    /*Distance on different directions*/
    DATATYPE disSq;                                                     /*Distance square on different sections*/
    ROADPTR pRoad;

    /*Calculate distance square*/
    minDisSq = coord.DisSquareTo(nodes[0]->coord);                      /*Initilize minimum distance square*/
    for(ui=0,ref=0; ui<sectionCount; ui++)
    {
        pRoad = sections[ui];
        disOnTrack = ABS( coord.ProdWith(pRoad->vecRoad)-pRoad->vecRoadProd ) - pRoad->roadLen/2;/*On-track direction distance to the road*/
        disOffTrack = ABS( coord.ProdWith(pRoad->vecOtho)-pRoad->vecOthoProd );/*Off-track direction distance to the road*/
        if(disOnTrack>0) disSq = disOnTrack*disOnTrack + disOffTrack*disOffTrack;
        else disSq = disOffTrack*disOffTrack;
        if(disSq < minDisSq)
        {
            minDisSq = disSq;
            ref = ui;
        }
    }

    DATATYPE disToRoadMid = coord.ProdWith(sections[ref]->vecRoad) - sections[ref]->vecRoadProd;
    int roadDirToLinkDir = (sections[ref]->beginPtr==nodes[ref]) ? 1 : -1;
    /*Calculate distance to the begin of the link*/
    for(ui=0,disToBegin=0; ui<ref; ui++)
    {
        disToBegin += sections[ui]->roadLen;
    }
    disToBegin += sections[ref]->roadLen/2;
    disToBegin += disToRoadMid * roadDirToLinkDir;

    /*Calculate distance to the end of the link*/
    for(ui=ref+1,disToEnd=0; ui<=sectionCount-1; ui++)
    {
        disToEnd += sections[ui]->roadLen;
    }
    disToEnd += sections[ref]->roadLen/2;
    disToEnd -= disToRoadMid * roadDirToLinkDir;

    /*Bearing*/
    bear = sections[ref]->roadBear;
    if(roadDirToLinkDir==-1) bear += Pi;
    if(bear > Pi) bear -= 2*Pi;

    return MM_OK;
}