mmcorrate.cpp

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

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

    File Name:   test/MMCorRate.cpp
    Description: calculate correct rates of map matching results

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

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

    Author: [e-mail]	
    Upadate:
        2007/12/07  File founded
        2007/12/18  Program completed
        2008/03/17  Junction range modified to both sides
        2008/03/17  Location file format changed

    Copyright 2007-2008 Dept. of Automation, Tsinghua Unviersity

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

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

#include "../MM.h"

#define ALLOWALL

const UINT maxStep = 1000;

const char LoadLocFormat[] = "%d,%lf";
const char LoadMatchFormat[] = "%*d,%d,%*f,%*d";
const char SaveCRFormat[] = "%6d,%11.6f\n";

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

int main(int argc, char* argv[])
{
    int c;
    UINT fileID = 0;
    UINT delayStep = 0;
    DATATYPE radius = 0;
    bool save = false;
    bool newSave = false;

    /*Input arguments*/
    while ((c = getopt (argc, argv, "l:d:r:snh")) != -1)
    {
        switch (c) {
        case 'l':
            sscanf(optarg,"%d",&fileID);
            break;
        case 'd':
            sscanf(optarg,"%d",&delayStep);
            break;
        case 'r':
            sscanf(optarg,"%lf",&radius);
            break;
        case 's':
            save = true;
            break;
        case 'n':
            newSave = true;
            break;
        case 'h':
            printf("    Provided options:\n");
            printf("      -l fileID        determine the result of data set to load\n");
            printf("      -d delaytime     determine the result of given delay time\n");
            printf("      -r radius        ignore mismatches around intersections nearer than radius metres, default is 0\n");
            printf("      -s               save results to csv file in folder ./result/delaytime/(if exists)\n");
            printf("      -n               drop results already saved\n");
            printf("      -h               get instructions on input options\n");
            break;
        }
    }

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

    /*Open real location file*/
    FILE* fpLoc;
    char pathLoc[StrLength]="";
    sprintf(pathLoc,"%s/location.csv",Dir);
    if( (fpLoc=fopen(pathLoc,"r"))==NULL )
    {
        #ifdef DEBUG
            printf("Fail to load location file.%s\n",pathLoc);
        #endif
        return MM_Access;
    }
    fscanf(fpLoc,"%*s");
 
    /*Open matching result file*/
    FILE* fpMatch;
    char pathMatch[StrLength]="";
    sprintf(pathMatch,"result/%d/delay_output_list%d.csv",delayStep,fileID);
    if( (fpMatch=fopen(pathMatch,"r"))==NULL )
    {
        #ifdef DEBUG
            printf("Fail to load result file.\n");
        #endif
        fclose(fpLoc);
        return MM_Access;
    }
    fscanf(fpMatch,"%*s");

    /*Save correct rate*/
    FILE *fpCorRate = NULL;
    char str[StrLength] = "";
    if(save==true)
    {
        /*Add result to the file*/
        sprintf(str,"result/%d/correct_rate_list_r%g.csv",delayStep,radius);
        if( newSave==true || (fpCorRate = fopen(str,"r"))==NULL )
        {
            if( (fpCorRate = fopen(str,"w"))==NULL )
            {
                #ifdef DEBUG
                    printf("Fail to open save file.\n");
                #endif
            }
            else
            {
                fprintf(fpCorRate,"FileID,CorrectRate\n");
            }
        }
        else
        {
            fclose(fpCorRate);
            if( (fpCorRate = fopen(str,"a"))==NULL )
            {
                #ifdef DEBUG
                    printf("Fail to open save file.\n");
                #endif
            }
        }
    }

    /*Accumulate correct count*/
    UINT ui,correct;
    IDTYPE realRoad,matchRoad;
    DATATYPE nodeDis;
    IDTYPE oldRoad = 0, newRoad = 0;
    for(ui=0,correct=0; ui<=maxStep; ui++)
    {
        fscanf(fpLoc,LoadLocFormat,&realRoad,&nodeDis);
        if(feof(fpLoc)) break;
        if(realRoad!=newRoad)
        {
            oldRoad = newRoad;
            newRoad = realRoad;
        }

        fscanf(fpMatch,LoadMatchFormat,&matchRoad);
        if(feof(fpMatch)) break;

        if(matchRoad==realRoad)
        {
            correct++;
            continue;
        }
#ifdef ALLOWALL
        if(nodeDis<radius)
        {
            correct++;
            continue;
        }
#else
        if(matchRoad==oldRoad && nodeDis<radius)
        {
            correct++;
            continue;
        }
#endif
    }
    UINT totalStep = ui;

    /*Output correct rate*/
    DATATYPE corRate = DATATYPE(correct)/DATATYPE(totalStep);
    if(fpCorRate==NULL)
    {
        printf("The correct rate in file %d is %.4f%%, delayed %d seconds, and ignored radius=%g(m)\n",fileID,corRate*100,delayStep,radius);
    }
    else
    {
        fprintf(fpCorRate,SaveCRFormat,fileID,corRate);
    }

    if(fpLoc) fclose(fpLoc);
    if(fpMatch) fclose(fpMatch);
    if(fpCorRate) fclose(fpCorRate);

    return 0;
}