loadsubmap.cpp

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

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

    File Name:    map/LoadSubMap.cpp
    Description:  implementation of LoadSubMap, to load submap data from file

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

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

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

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

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

#include <stdio.h>
#include <string.h>
#include <math.h>
#include "../map/LoadSubMap.h"

RETCHECK LoadSubMap(const char* path, const GPS& gpsCenter, 
                    SUBMAP& submap)
{
    char str[StrLength];
    UINT ui,uj;

    /*Initiallization*/
    submap.nodeCount = 0;
    submap.roadCount = 0;
    submap.linkCount = 0;

    /**************************                   Node Information                          ***************************************************/
    FILE *fpNodes;
    strcpy(str,path);
    strcat(str,"/nodes.csv\0");
    if( (fpNodes=fopen(str,"r"))==NULL )                                /*Load nodes.csv*/
    {
        #ifdef DEBUG
            printf("Failed to open nodes.csv!\n");
        #endif
        return MM_Access;
    }
    fscanf(fpNodes,"%*[^\n]%*c");                                       /*Drop titles (first line)*/
    NODEPTR pNode;
    GPS gps;
    for(ui=0; ui<NodeSpace; ui++)
    {
        pNode = &(submap.nodes[ui]);
        fscanf(fpNodes,"%d,%lf,%lf",&(pNode->id),&(gps.longitude),&(gps.latitude));
        if(feof(fpNodes)) break;                                        /*End of file*/
        GPS2Coord(gps,gpsCenter,pNode->coord);                          /*Transform to coordinates*/

        /*Initiallization*/
        submap.linkage[ui].linkCount = 0;
    }
    if(ui==NodeSpace)                                                   /*No enough space*/
    {
        #ifdef DEBUG
            printf("No enough space to store node data!\n");
        #endif
        return MM_Space;
    }
    submap.nodeCount = ui;
    if(fpNodes) fclose(fpNodes);


    /**************************                   Road Information                          ***************************************************/
    FILE *fpRoads;
    strcpy(str,path);
    strcat(str,"/roads.csv\0");
    if( (fpRoads=fopen(str,"r"))==NULL )                                /*Load roads.csv*/
    {
        #ifdef DEBUG
            printf("Failed to open roads.csv!\n");
        #endif
        return MM_Access;
    }
    fscanf(fpRoads,"%*[^\n]%*c");                                       /*Drop titles (first line)*/
    ROADPTR pRoad;
    UINT beginID,endID;
    UINT allowedDir;
    UINT beginSub,endSub;
    for(ui=0; ui<RoadSpace; ui++)
    {
        pRoad = &(submap.roads[ui]);
        fscanf(fpRoads,"%d,%d,%d,%d",&(pRoad->id),&beginID,&endID,&allowedDir);
        if(feof(fpRoads)) break;                                        /*End of file*/

        /*Find begin and end nodes in the list*/
        for( beginSub=0; submap.nodes[beginSub].id != beginID && beginSub<submap.nodeCount; beginSub++ );
        pRoad->beginPtr = submap.nodes + beginSub;
        for( endSub=0; submap.nodes[endSub].id != endID && endSub<submap.nodeCount; endSub++ );
        pRoad->endPtr = submap.nodes + endSub;
        if(beginSub==submap.nodeCount || endSub==submap.nodeCount || beginSub==endSub)/*Invalid begin or end*/
        {
            #ifdef DEBUG
                printf("Invalid road begin or end!\n");
            #endif
            return MM_Inconsistancy;
        }

        /*Calculate parameters*/
        pRoad->midPt = (pRoad->endPtr->coord + pRoad->beginPtr->coord) *0.5;
        pRoad->vecRoad = pRoad->endPtr->coord - pRoad->beginPtr->coord;
        pRoad->roadLen = DATATYPE(sqrt(pRoad->vecRoad.ProdWith(pRoad->vecRoad)));
        pRoad->vecRoad *= (1/pRoad->roadLen);
        pRoad->vecOtho.east = -pRoad->vecRoad.north;
        pRoad->vecOtho.north = pRoad->vecRoad.east;
        pRoad->vecRoadProd = pRoad->midPt.ProdWith(pRoad->vecRoad);
        pRoad->vecOthoProd = pRoad->midPt.ProdWith(pRoad->vecOtho);
        pRoad->roadBear = DATATYPE(atan2(pRoad->vecRoad.north,pRoad->vecRoad.east));
    }
    if(ui==RoadSpace)                                                   /*No enough space*/
    {
        #ifdef DEBUG
            printf("No enough space to store road data!\n");
        #endif
        return MM_Space;
    }
    submap.roadCount = ui;
    if(fpRoads) fclose(fpRoads);


    /**************************                   Link Information                          ***************************************************/
    FILE *fpLinks;
    strcpy(str,path);
    strcat(str,"/links.csv\0");
    if( (fpLinks=fopen(str,"r"))==NULL )                                /*Load links.csv*/
    {
        #ifdef DEBUG
            printf("Failed to open links.csv!\n");
        #endif
        return MM_Access;
    }
    fscanf(fpLinks,"%*[^\n]%*c");                                       /*Drop titles (first line)*/
    LINKPTR pLink;
    char separator;
    IDTYPE roadID;
    UINT secSub,roadSub,nodeSub,nodeSub2;
    DATATYPE secTurns[RoadSpace],turnSum;
    for(ui=0; ui<submap.roadCount; ui++)
    {
        /*Section information*/
        pLink = &(submap.links[ui]);
        fscanf(fpLinks,"%[^,\n]%c",str,&separator);                     /*Check separator*/
        if(feof(fpLinks)) break;

        sscanf(str,"%d",&(pLink->id));                                  /*Link ID*/
        

        for(secSub=0; separator!='\n'; secSub++)
        {
            if(secSub>=MaxSectionCount)                                 /*No enough space*/
            {
                #ifdef DEBUG
                    printf("No enough space to store link data!\n");
                #endif
                return MM_Space;
            }
            fscanf(fpLinks,"%[^,\n]%c",str,&separator);                 /*Check separator*/
            sscanf(str,"%d",&roadID);                                   /*Road ID*/
            if(roadID==0)                                               /*End of link*/
            {
                if(separator!='\n') fscanf(fpLinks,"%*[^\n]%*c");
                break;
            }
            for(roadSub=0; roadSub<submap.roadCount; roadSub++)
            {
                if(submap.roads[roadSub].id==roadID) break;
            }
            if(roadSub==submap.roadCount)                               /*Unknown section ID*/
            {
                #ifdef DEBUG
                    printf("Unknown section ID in a link!\n");
                #endif
                return MM_Inconsistancy;
            }
            pLink->sections[secSub] = &(submap.roads[roadSub]);         /*Road pointer*/
        }
        pLink->sectionCount = secSub;

        /*Complete node information*/
        if(pLink->sectionCount==1)
        {
            pLink->nodes[0] = pLink->sections[0]->beginPtr;
            pLink->nodes[1] = pLink->sections[0]->endPtr;
        }
        else
        {
            /*Begin junction*/
            if(pLink->sections[0]->beginPtr==pLink->sections[1]->beginPtr || pLink->sections[0]->beginPtr==pLink->sections[1]->endPtr)
            {
                pLink->nodes[0] = pLink->sections[0]->endPtr;
            }
            else
            {
                pLink->nodes[0] = pLink->sections[0]->beginPtr;
            }
            for(nodeSub=1; nodeSub<=pLink->sectionCount; nodeSub++)
            {
                if(pLink->nodes[nodeSub-1]==pLink->sections[nodeSub-1]->beginPtr)
                {
                    pLink->nodes[nodeSub] = pLink->sections[nodeSub-1]->endPtr;
                }
                else
                {
                    pLink->nodes[nodeSub] = pLink->sections[nodeSub-1]->beginPtr;
                }
            }
        }

        /*Complete turning information*/
        for(nodeSub=1; nodeSub<pLink->sectionCount; nodeSub++)
        {
            secTurns[nodeSub] = pLink->sections[nodeSub]->roadBear - pLink->sections[nodeSub-1]->roadBear;
            if(secTurns[nodeSub]>Pi) secTurns[nodeSub] -= 2*Pi;
            if(secTurns[nodeSub]<-Pi) secTurns[nodeSub] += 2*Pi;
        }
        for(pLink->posTurn=pLink->negTurn=0,nodeSub=1; nodeSub<pLink->sectionCount; nodeSub++)
        {
            for(turnSum=0,nodeSub2=nodeSub; nodeSub2<pLink->sectionCount; nodeSub2++)
            {
                turnSum += secTurns[nodeSub2];
                if(turnSum>pLink->posTurn) pLink->posTurn = turnSum;
                if(turnSum<pLink->negTurn) pLink->negTurn = turnSum;
            }
        }
        pLink->linkAngle = pLink->nodes[pLink->sectionCount]->coord.AngleFrom(pLink->nodes[0]->coord);
    }
    submap.linkCount = ui;
    for(ui=0; ui<=submap.linkCount-1; ui++)
    {
        pLink = submap.links + ui;
        for(pLink->linkLen=0,uj=0; uj<=pLink->sectionCount-1; uj++) pLink->linkLen += pLink->sections[uj]->roadLen;
    }
    if(fpLinks) fclose(fpLinks);


    /**************************                   Linkage Information                       ***************************************************/
    UINT juncSub;
    LINKPTR curLink;
    LINKAGEPTR juncLinkage;
    for(ui=0; ui<submap.nodeCount; ui++) submap.linkage[ui].linkCount = 0;
    for(ui=0; ui<submap.linkCount; ui++)
    {
        curLink = &(submap.links[ui]);
        /*Begin junction*/
        juncSub = UINT( curLink->nodes[0] - submap.nodes );
        juncLinkage = &(submap.linkage[juncSub]);
        if(juncLinkage->linkCount==MaxNodeDegree)                       /*No enough space*/
        {
            #ifdef DEBUG
                printf("No enough space to store linkage data!\n");
            #endif
            return MM_Space;
        }
        juncLinkage->links[juncLinkage->linkCount] = curLink;
        juncLinkage->linkCount++;
        /*End junction*/
        juncSub = UINT( curLink->nodes[curLink->sectionCount] - submap.nodes );
        juncLinkage = &(submap.linkage[juncSub]);
        if(juncLinkage->linkCount==MaxNodeDegree)                       /*No enough space*/
        {
            #ifdef DEBUG
                printf("No enough space to store linkage data!\n");
            #endif
            return MM_Space;
        }
        juncLinkage->links[juncLinkage->linkCount] = curLink;
        juncLinkage->linkCount++;
    }

    return MM_OK;
}