submap.h

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

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

    File Name:    map/SubMap.h
    Description:  definition of submap containing nearby map data

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

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

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

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

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

#ifndef __SUBMAP_H__
#define __SUBMAP_H__

#include "../include/Coordinate.h"
#include "../include/GPS.h"

/******************************                   Constants                                 ***************************************************/
/*Constants for space allocation*/
const UINT MaxNodeDegree = 8;                                           /*Maximum degree of nodes*/
const UINT NodeSpace = 300;                                             /*Size limit for a submap*/
const UINT RoadSpace = 300;                                             /*Size limit for a submap*/
const UINT MaxSectionCount = 15;                                        /*Maximum section count in a link*/

/*Transformation reference, center point GPS*/
const GPS GPSCenter = {DATATYPE(168.8487),DATATYPE(1.2795)};            /*Center of map in data080307*/


/******************************                   Data structures                           ***************************************************/
/*Node position*/
typedef struct
{
    IDTYPE          id;                                                 /*ID of the node*/
    COORD           coord;                                              /*Planar Cartesian coordinates*/
}NODE,*NODEPTR;

/*Road data*/
typedef struct  
{
    IDTYPE          id;                                                 /*ID of the road*/
    NODEPTR         beginPtr;                                           /*Begin node of the road*/
    NODEPTR         endPtr;                                             /*End node of the road*/

    COORD           midPt;                                              /*Middle point of the road*/
    COORD           vecRoad;                                            /*Road direction vector*/
    COORD           vecOtho;                                            /*Direction vector a right angle anticlockwise*/

    DATATYPE        roadLen;                                            /*Length of th road*/
    DATATYPE        roadBear;                                           /*Bearing angle of the road*/
    DATATYPE        vecRoadProd;                                        /*Inner product of middle point coordinate and vecRoad*/
    DATATYPE        vecOthoProd;                                        /*Inner product of middle point coordinate and vecOtho*/
}ROAD,*ROADPTR;

/*Link data*/
typedef struct LINK
{
    IDTYPE          id;                                                 /*ID of the link*/
    UINT            sectionCount;                                       /*Count of sections in the link*/
    ROADPTR         sections[MaxSectionCount];                          /*Road sections of the link in sequence*/
    NODEPTR         nodes[MaxSectionCount+1];                           /*Nodes on the link in sequence*/

    DATATYPE        linkLen;                                            /*Length of th link*/
    DATATYPE        posTurn;                                            /*Maximum positive turning angle*/
    DATATYPE        negTurn;                                            /*Maximum negative turning angle*/
    DATATYPE        linkAngle;                                          /*Approximate angle of the link*/

    RETCHECK PointOnLink(RoadDir dir, DATATYPE dis, COORD& coord, DATATYPE& angle);/*Find a point on the link*/
    RETCHECK ProjectToLink(const COORD& coord, DATATYPE& minDisSq, 
                           DATATYPE& disToBegin, DATATYPE& disToEnd, DATATYPE& bear);/*Calculate projection from a point to the link*/
}LINK,*LINKPTR;

/*Linkage data*/
typedef struct
{
    UINT            linkCount;                                          /*Count of links*/
    LINKPTR         links[MaxNodeDegree];                               /*linked link*/
}LINKAGE,*LINKAGEPTR;


/******************************                   Submap data                               ***************************************************/
typedef struct
{
    UINT            nodeCount;                                          /*Number of nodes in the submap*/
    NODE            nodes[NodeSpace];                                   /*Node data in the submap*/
    LINKAGE         linkage[NodeSpace];                                 /*Linkage data of nodes in the submap*/

    UINT            roadCount;                                          /*Number of roads in the submap*/
    ROAD            roads[RoadSpace];                                   /*Road data in the submap*/

    UINT            linkCount;                                          /*Number of links in the submap*/
    LINK            links[RoadSpace];                                   /*Link data in the submap*/
}SUBMAP;

#endif /*__SUBMAP_H__*/