mmtest.cpp

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

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

    File Name:   test/MMTest.cpp
    Description: run the algorithm, then show and save matching results

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

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

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

    Copyright 2008-2009 Dept. of Automation, Tsinghua University

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

#include <stdio.h>
#include <getopt.h>
#include <time.h>

#include "../MM.h"

const UINT maxStep = 1000;
const UINT SensorCount = 3;
const UINT RouteSpace = 50;

const char Positive[] = "+";
const char Negative[] = "-";
const char Uncertain[] = "+/-";
const char GPSFormat[] = "%lf,%lf";
const char RawFormat[] = "%lf,%lf,%lf";
const char OutputFormat[] = "%4d%10.6f%4s";
const char CurOutputGap[] = "%31s";
const char SaveStepFormat[] = "%4u,%4u,%8.4f,%6d\n";
const char LoadStepFormat[] = "%*u,%u,%*lf,%d\n";
const char SaveRouteFormat[] = "%4u,%4u,%6d\n";
const char PrintRouteFormat[] = "%-6d %-6d %-6s\n";

const char Dir[] = "./data";

MHGPSDR mhGPSDR;

int main(int argc, char* argv[])
{
    int c;
    UINT fileID = 0;
    UINT delayStep = 0;
    long waitms = 0;
    bool save = false;
    bool noScreen = false;
    timespec sleepTime,remainTime;

    /*Input arguments*/
    while ((c = getopt (argc, argv, "l:d:w:snh")) != -1)
    {
        switch (c) {
        case 'l':
            sscanf(optarg,"%d",&fileID);
            break;
        case 'd':
            sscanf(optarg,"%d",&delayStep);
            break;
        case 'w':
            sscanf(optarg,"%ld",&waitms);
            waitms *= 1000000;
            sleepTime.tv_sec = waitms/1000000000;
            sleepTime.tv_nsec = waitms%1000000000;
            break;
        case 's':
            save = true;
            break;
        case 'n':
            noScreen = true;
            break;
        case 'h':
            printf("    Provided options:\n");
            printf("      -l fileID        determine the data set to load\n");
            printf("      -d delaytime     fix the delay time in seconds for delay mode results\n");
            printf("      -w sleeptime     wait 'sleeptime' milliseconds every step\n");
            printf("      -s               save results to csv file in folder ./result/delaytime/(if exists)\n");
            printf("      -n               disable screen output\n");
            printf("      -h               get instructions on input options\n");
            break;
        }
    }

    if(fileID==0)
    {
        printf("Use '-l fileID' to determine the data set to load.\n");
        return MM_Parameter;
    }

    /*Algorithm version*/
    Interface *pAlgo = &mhGPSDR;

    /*Test using given data file*/
    FILE* fpGPS;
    char pathGPS[StrLength]="";
    sprintf(pathGPS,"%s/gps%d.csv",Dir,fileID);
    if( (fpGPS=fopen(pathGPS,"r"))==NULL )
    {
        #ifdef DEBUG
            printf("Fail to load GPS file.\n");
        #endif
        return MM_Access;
    }
    fscanf(fpGPS,"%*s");
    #ifdef DEBUG
        if(noScreen==false) printf("GPS file loaded.\n");
    #endif

    FILE* fpSensor;
    char pathSensor[StrLength]="";
    sprintf(pathSensor,"%s/sensor%d.csv",Dir,fileID);
    if( (fpSensor=fopen(pathSensor,"r"))==NULL )
    {
        #ifdef DEBUG
            printf("Fail to load sensor file.\n");
        #endif
        fclose(fpGPS);
        return MM_Access;
    }
    fscanf(fpSensor,"%*s");
    #ifdef DEBUG
        if(noScreen==false) printf("Sensor file loaded.\n");
    #endif

    pAlgo->LoadSubMapFile(Dir);
    #ifdef DEBUG
        if(noScreen==false) printf("Map file loaded.\n");
    #endif

    GPS gps;
    IMURAW imuRaws[GPSPeriod/IMUPeriod+1];
    VELRAW velRaws[GPSPeriod/VELPeriod+1];
    RESULT resultRT,resultDelayed;

    FILE *fpCur = NULL;
    FILE *fpDelay = NULL;
    char str[StrLength] = "";
    if(save==true)
    {
        sprintf(str,"result/%d/output_list%d.csv",delayStep,fileID);
        fpCur = fopen(str,"w");
        sprintf(str,"result/%d/delay_output_list%d.csv",delayStep,fileID);
        fpDelay = fopen(str,"w");

        if(fpCur==NULL || fpDelay==NULL)
        {
            #ifdef DEBUG
                printf("Fail to open save file.\n");
            #endif
        }
        else
        {
            fprintf(fpCur,"Step,RoadID,Belief,Direction\n");
            fprintf(fpDelay,"Step,RoadID,Belief,Direction\n");
        }
    }

    /*Main loop*/
    UINT ui,uj;
    UINT SensorPerGPS = 100;
    const char *direction;
    int dir;
    void* sensors[SensorCount];
    sensors[0] = (void*) &gps;
    sensors[1] = (void*) imuRaws;
    sensors[2] = (void*) velRaws;

    for(ui=0; ui<=maxStep; ui++)
    {
        fscanf(fpGPS,GPSFormat,&(gps.longitude),&(gps.latitude));
        if(feof(fpGPS)) break;

        for(uj=0; uj<SensorPerGPS; uj++)
        {
            fscanf(fpSensor,RawFormat,&(velRaws[uj]),&(imuRaws[uj].acceleration),&(imuRaws[uj].angularVelocity));
            if(feof(fpSensor)) break;
        }
        if(feof(fpSensor)) break;

        pAlgo->Update(sensors,ui*1000);
        pAlgo->GetResult(ui*1000,resultRT);
        pAlgo->GetResult((ui-delayStep)*1000,resultDelayed);

        if(noScreen==false)
        {
            printf("Current:%4d:",ui+1);
            direction = (resultRT.direction==positive) ? Positive : (resultRT.direction==negative) ? Negative : Uncertain;
            printf(OutputFormat,resultRT.linkID,resultRT.belief,direction);

            if(ui>=delayStep)
            {
                printf("    Delayed:%4d:",ui+1-delayStep);
                direction = (resultDelayed.direction==positive) ? Positive : (resultDelayed.direction==negative) ? Negative : Uncertain;
                printf(OutputFormat,resultDelayed.linkID,resultDelayed.belief,direction);
            }

            printf("\n");
        }

        if(fpCur && fpDelay)
        {
            dir = (resultRT.direction==positive) ? 1 : (resultRT.direction==negative) ? 0 : 2;
            fprintf(fpCur,SaveStepFormat,ui+1,resultRT.linkID,resultRT.belief,dir);

            if(ui>=delayStep)
            {
                dir = (resultDelayed.direction==positive) ? 1 : (resultDelayed.direction==negative) ? 0 : 2;
                fprintf(fpDelay,SaveStepFormat,ui+1-delayStep,resultDelayed.linkID,resultDelayed.belief,dir);
            }
        }

        if(waitms) nanosleep(&sleepTime,&remainTime);	                /*wait for screen output*/
    }
    UINT totalStep = ui;

    for(ui=1; ui<=delayStep; ui++)
    {
        pAlgo->GetResult((totalStep+ui-delayStep)*1000,resultDelayed);

        if(noScreen==false)
        {
            printf(CurOutputGap,"");
        
            printf("    Delayed:%4d:",ui+totalStep-delayStep);
            direction = (resultDelayed.direction==positive) ? Positive : (resultDelayed.direction==negative) ? Negative : Uncertain;
            printf(OutputFormat,resultDelayed.linkID,resultDelayed.belief,direction);

            printf("\n");
        }
        
        if(fpCur && fpDelay)
        {
            dir = (resultDelayed.direction==positive) ? 1 : (resultDelayed.direction==negative) ? 0 : 2;
            fprintf(fpDelay,SaveStepFormat,ui+totalStep-delayStep,resultDelayed.linkID,resultDelayed.belief,dir);
        }
    
        if(waitms) nanosleep(&sleepTime,&remainTime);	                /*wait for screen output*/
    }

    /*Get route*/
    ROUTE route[RouteSpace];
    UINT routeCount = pAlgo->GetRoute(RouteSpace,route);
    UINT dirSign;
    if(noScreen==false)
    {
        printf("Recent links are:\nlink   step   direction\n");
        for(ui=0; ui<routeCount; ui++)
        {
            direction = (route[ui].direction==positive) ? Positive : (route[ui].direction==negative) ? Negative : Uncertain;
            printf(PrintRouteFormat,route[ui].linkID,route[ui].beginTime,direction);
        }
        printf("\n");
    }
    FILE *fpRoute = NULL;
    char strRoute[StrLength] = "";
    if(save==true)
    {
        sprintf(strRoute,"result/%d/route%d.csv",delayStep,fileID);
        fpRoute = fopen(strRoute,"w");

        if(fpRoute==NULL)
        {
            #ifdef DEBUG
                printf("Fail to open save file.\n");
            #endif
        }
        else
        {
            fprintf(fpRoute,"Step,RoadID,Direction\n");
            for(ui=0; ui<routeCount; ui++){}
            for(; ui>=1; ui--)
            {
                dirSign = (route[ui-1].direction==positive) ? 1 : (route[ui-1].direction==negative) ? 0 : 2;
                fprintf(fpRoute,SaveRouteFormat,route[ui-1].beginTime,route[ui-1].linkID,dirSign);
            }
        }
    }

    fclose(fpGPS);
    fclose(fpSensor);
    if(fpCur) fclose(fpCur);
    if(fpDelay) fclose(fpDelay);

    /*Route results*/
    IDTYPE road,pre;
    if(fpCur && fpDelay)
    {
        sprintf(str,"result/%d/output_list%d.csv",delayStep,fileID);
        fpCur = fopen(str,"r");
        sprintf(str,"result/%d/delay_output_list%d.csv",delayStep,fileID);
        fpDelay = fopen(str,"r");

        if(fpCur==NULL || fpDelay==NULL)
        {
            #ifdef DEBUG
                printf("Fail to open save file.\n");
            #endif
        }
        else
        {
            fscanf(fpCur,"%*s");
            fscanf(fpDelay,"%*s");

            sprintf(str,"result/%d/definite_sequence%d.csv",delayStep,fileID);
            FILE* fpCurRoute = fopen(str,"w");
            sprintf(str,"result/%d/delay_definite_sequence%d.csv",delayStep,fileID);
            FILE* fpDelayRoute = fopen(str,"w");
 
            if(fpCurRoute==NULL || fpDelayRoute==NULL)
            {
                #ifdef DEBUG
                    printf("Fail to open save file.\n");
                #endif
            }
            else
            {
                fprintf(fpCurRoute,"Step,RoadID,Direction\n");
                fprintf(fpDelayRoute,"Step,RoadID,Direction\n");

                for(ui=0,pre=0;ui<maxStep;ui++)
                {
                    fscanf(fpCur,LoadStepFormat,&road,&dir);
                    if(feof(fpCur)) break;
        
                    if(road!=pre)
                    {
                        fprintf(fpCurRoute,SaveRouteFormat,ui+1,road,dir);
                        pre = road;
                    }
                }

                for(ui=0,pre=0;ui<maxStep;ui++)
                {
                    fscanf(fpDelay,LoadStepFormat,&road,&dir);
                    if(feof(fpDelay)) break;
        
                    if(road!=pre)
                    {
                        fprintf(fpDelayRoute,SaveRouteFormat,ui+1,road,dir);
                        pre = road;
                    }
                }
            }
            if(fpCurRoute) fclose(fpCurRoute);
            if(fpDelayRoute) fclose(fpDelayRoute);
        }
        if(fpCur) fclose(fpCur);
        if(fpDelay) fclose(fpDelay);
    }

    return MM_OK;
}