hmi04.cpp

This example shows how to use the stateFlag objects to segment an analysis
This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#include <iostream>
#include <fstream>
#include <sstream>
#include <list>
#include <string>
#include <vector>
#include <limits>
#include <math.h>

#include "dataFrame.h"
#include "dataFile.h"
#include "timeSeries.h"
#include "timeSeriesArray.h"
#include "lookupTable.h"
#include "filter.h"
#include "measures.h"
#include "stateFlag.h"

using namespace std;
#include "hmi04.h"
#include "hmi04_utilities.h"

#ifdef WIN32
string convertLink(const char *fname);
int analysisMain (int argc, char * const argv[])
#else
int main (int argc, char * const argv[])
#endif
{   
    // define a lookup table for subject meta-data
    string subjectTablePath="subjects.txt";
    
    try {
        static lookupTable subjectTable(subjectTablePath,"subject");
        
        for (int filenum=1;filenum<argc;++filenum) {
            string fileName = argv[filenum];
            dataFrame expDataFrame(removePathExt(fileName)+".hdr");
            string dataFileName(removePathExt(fileName)+".bin");
            cout << endl;
            cout << "reading file " << dataFileName << " " << filenum << " of ";
            cout << argc-1 << "..." << flush;
            try {
                vector<measures> dependentMeasures=process_file(dataFileName, 
                                                                expDataFrame,
                                                                subjectTable);
                outputMeasures(dataFileName,
                               "hmi04.txt",
                               dependentMeasures,
                               filenum);
            }
            // these are all errors that cause us to skip a data file
            // but not abort the run
            catch (tsException& tse) {
                tse.debug_print();
                cerr << "skipping to next file..." << endl;
            }
            catch (dataFile::dataInputExceptions& die) {
                die.debug_print();
                cerr << "skipping to next file..." << endl;
            }
            catch (lookupTable::lookupExceptions& lookupError) {
                lookupError.debug_print();
                cerr << "skipping to next file..." << endl;
            }
        }
    }
    catch (lookupTable::lookupExceptions& lookupError) {
        // if we can't read a lookup file we have to just exit
        lookupError.debug_print();
        exit(1);
    }
}

vector<measures> process_file(string dataFileName, 
                              dataFrame& frame,
                              lookupTable& subjectTable)
{   const double Ts=0.02;
#ifdef WIN32
    dataFileName=convertLink(dataFileName.c_str());
#endif
    
    // load the entire data file into memory
    
    dataFile data(dataFileName,frame,Ts,dataFile::BigEndian);
    cout << "done" << endl;
    
    // retrieve data objects from the file that we'll be needing to complete
    // the analysis
    
    // continuous variables 
    timeSeries<float> swa(frame.findName("STEERIN"),data);
    timeSeries<float> lane_pos(frame.findName("LANE_POS"),data);
    timeSeries<float> Ax(frame.findName("BCGAX"),data);
    
    // create an aggregate braking torque
    timeSeries<float> b1(frame.findName("BRAKE_TORQUE1"),data);
    timeSeries<float> b2(frame.findName("BRAKE_TORQUE2"),data);
    timeSeries<float> b3(frame.findName("BRAKE_TORQUE3"),data);
    timeSeries<float> b4(frame.findName("BRAKE_TORQUE4"),data);
    timeSeries<float> brake = (b1+b2+b3+b4)/4.0;
    
    // create a throttle opening above closed plate. TAPA has a non-zero
    // angle at closed plate
    timeSeries<float> tapa(frame.findName("TAPA"),data);
    float closedPlateAngle = tapa[tapa.min()];
    timeSeries<float> throttle = tapa - closedPlateAngle;
    
    // discrete variables
    timeSeries<float> ldw_type_float(frame.findName("LDW_TYPE"),data);
    timeSeries<int> ldw_type = timeSeries<int>(ldw_type_float);
    timeSeries<float> ownship_lane_float(frame.findName("OWNSHIP_LANE"),data);
    timeSeries<int> ownship_lane = timeSeries<int>(ownship_lane_float);
    timeSeries<float> num_veh_float(frame.findName("NUM_VEHS_AHEAD"),data);
    timeSeries<int> num_vehs_ahead = timeSeries<int>(num_veh_float);
    
    // warning system flags
    stateFlag accFlag=
        getVirttexStateFlag(frame.findName("ACC_WARN"),data, "acc warning flag");
    stateFlag fcwFlag=
        getVirttexStateFlag(frame.findName("FCW_WARN"),data,"fcw warning flag");
    stateFlag ldwFlag=
        getVirttexStateFlag(frame.findName("LDW_WARN"),data,"ldw warning flag");
    
    // experimental condition flags
    stateFlag yawdevFlag=
        getVirttexStateFlag(frame.findName("YAWDEV"),data,"yaw deviation flag");
    stateFlag scenarioFlag=
        getVirttexStateFlag(frame.findName("CURRENT_SCENARIO"),data,"scenario flag");
    stateFlag badDataFlag=
        getVirttexStateFlag(frame.findName("BAD_DATA"),data,"bad data flag");
    stateFlag scenarioStep=
        getVirttexStateFlag(frame.findName("CURRENT_STEP"),data,"scene position");
    stateFlag ldwTypeFlag=
        getVirttexStateFlag(frame.findName("LDW_TYPE"),data,"ldw type flag");
    
    // calculate steering wheel velocities and accelerations
    
    timeSeries<float> swavel = swa.differentiate();
    timeSeries<float> swavel_f = swavel;
    iir_t *lpf = init_sw_lp();
    filtfilt(lpf, swavel_f);
    set_iir_initial_values(lpf,0.0);    // reset the filter
    
    timeSeries<float> swaacc = swavel.differentiate();
    timeSeries<float> swaacc_f = swaacc;
    filtfilt(lpf, swaacc_f);
    free_iir_filter(lpf);               // free the filter
    
    timeSeries<float> subjectNumber(frame.findName("SUBJECT"),data);
    int subno=subjectNumber[10];    // don't look at sample 0, since the subject
                                    // variable takes a litle while to inititlalize
                                    // convert to a string
    ostringstream subjectStr;
    subjectStr << subno;
    
    // get the gender and age group from the subjectTable
    string age = subjectTable.find<string>(subjectStr.str(),"age");
    string gender = subjectTable.find<string>(subjectStr.str(),"gender");
    
    // find the 4 ldw warning blocks
    vector<state> ldwStates = findLdwBlocks(ldwTypeFlag);
    
    // variables needed to collect measures
    vector<measures> event_measures;
    int nPreceedingLDWFp=0;
    int nPreceedingFCWFp=0;
    reactionTime_t rt;
    
    // now iterate over every scenario in the experiment and compute relevant measures
    
    vector<state> Scenarios=scenarioFlag.findStates();
    for (int i=0;i<Scenarios.size();i++) {
        int begIndex = Scenarios[i].begin();
        scenario_t currentScenarioType =  intToScenario(scenarioFlag[begIndex],
                                                        num_vehs_ahead[begIndex]);
        string currentScenarioName = scenarioName(currentScenarioType);
        
        bool recordMeasures=false;
        measures meas;
        meas.entry("subno",subno);
        meas.entry("age",age);  
        meas.entry("gender", gender);
        
        int ldwOrder= findLdwOrder(ldwStates,begIndex);
        meas.entry("ldwblock", ldwOrder);
        
        switch (currentScenarioType) {
            case foil:
                break;
            case ldw_tp_a:
                break;
                
                // compute reaction time for the LDW true positve distracted
                // case
            case ldw_tp_d:
                cout << "ldw_tp_d at: " << begIndex;
                rt = calculateSWReact(Scenarios[i],
                                      yawdevFlag,
                                      ldwFlag,
                                      lane_pos,
                                      swa,
                                      swavel_f,
                                      swaacc_f);
                cout << " rt, rt_alarm = \t" << rt.rt << "\t" \
                    <<rt.rt_alarm;
                cout << endl;
                // enter the appropriate ldw measures into the output
                meas.entry("sc_type","ldw");
                meas.entry("distract", "d");
                meas.entry("sc_name",intToLdwType(ldw_type[begIndex]));
                recordMeasures=true;
                
                break;
                
                // acc cases
            case acc_tp_a:
            case acc_tp_d:
            case acc_fp_a:
            case acc_fp_d:
                break;
                
                // FCW false positive cases
            case fcw_fp_a:
            case fcw_fp_d:
                ++nPreceedingFCWFp;
                break;
                
                // FCW true positive cases and reaction time cases
            case fcw_tp_a:
            case fcw_tp_d:
            case rti_tp_a:
            case rti_tp_d:
                rt = findBrakingReactionTime(Scenarios[i],
                                             scenarioStep,
                                             Ax,
                                             throttle,
                                             brake,
                                             fcwFlag);
                cout << " bt= " << rt.rt << endl;
                // enter the appropriate fcw measures into the output
                meas.entry("sc_type","fcw");
                meas.entry("distract",(isDistracted(currentScenarioName) ? "d":"a"));
                meas.entry("sc_name",currentScenarioName);
                recordMeasures=true;
                
                break;
                
                // LDW false positive cases
            case ldw_fp_a_L:
            case ldw_fp_d_L:
            case ldw_fp_a_R:
            case ldw_fp_d_R:
                ++nPreceedingLDWFp;
                break;
                
            case no_op:
                break;
        }
        
        // compute measures with common formulae
        
        if (recordMeasures) {
            meas.entry("rt",rt.rt);
            meas.entry("rt_alarm", rt.rt_alarm);
            // count the previous number of false positives
            // from the beginning of the experiment
            meas.entry("prvldwfp",nPreceedingLDWFp);
            
            // and now count the previous number of false positives
            // from the beginning of the current LDW block
            int blockStart=ldwStates[ldwOrder-1].begin();
            int nfp_b=numPreceedingLdwFp(scenarioFlag.range(blockStart,
                                                            begIndex));
            meas.entry("prvldwfp_b",nfp_b);
            meas.entry("prvfcwfp",nPreceedingFCWFp);
            // the number of ldw true positives seen is the 
            // number of ldw warnings minus the number of false positives
            int nLdw = ldwFlag.range(0,begIndex).findStates().size();
            int nLdwTp = nLdw -  nPreceedingLDWFp;
            meas.entry("prvldwtp",nLdwTp);
            
            // save this measures object
            event_measures.push_back(meas);
        }
    }
    return event_measures;
}


void outputMeasures(const string& dataFileName,
                    const string& headerFileName,
                    const vector<measures>& dependentMeasures,
                    const int filenum)
{   // write the header file out the first time only
    if (filenum == 1) {
        ofstream headerFile(headerFileName.c_str(), ios::out);
        print_labels(headerFile,dependentMeasures[0]);
        headerFile.close();
    }
    int extpos = dataFileName.find(".bin");
    string measFileName = dataFileName.substr(0,extpos) + ".dat";
    ofstream mFile(measFileName.c_str(), ios::out);
    unsigned int i;
    for (i=0;i<dependentMeasures.size();i++) {
        mFile << dependentMeasures[i];
    }
    return;
    
}

reactionTime_t calculateSWReact(state Scenario,
                                stateFlag& yawdevFlag,
                                stateFlag& ldwFlag,
                                timeSeries<float>& lane_pos,
                                timeSeries<float>& swa,
                                timeSeries<float>& swavel,
                                timeSeries<float>& swaacc)
{   float lw = 12.5;    // lane width
    enum {left, right} devDir;
    reactionTime_t output;
    
    // extend the scenario by 1 second since we now start the yaw event so late it
    // is often still in progress when the scenario event is over
    Scenario.extend(int(1.0/swa.sampleTime()));
    
    // there should be one and only one yaw event contained within the scenario
    // let's find it
    vector<state> yawStates = yawdevFlag.range(Scenario).findStates();
    if (yawStates.size() != 1) {
        cout << "illegal number of yaw events found: " << yawStates.size() << endl;
    }
    int beginYawDev = yawStates[0].begin();
    // find out if this was a deviation left or right and mark the extrema
    timeSeries<float> laneExcursion = lane_pos.range(Scenario) - lw/2.0;
    int minExcurIndex, maxExcurIndex;
    float minExcur = laneExcursion[minExcurIndex=laneExcursion.min()];
    float maxExcur = laneExcursion[maxExcurIndex=laneExcursion.max()];
    if (abs(minExcur) > abs(maxExcur)) {
        devDir = left;
        maxExcur = minExcur;
        maxExcurIndex = minExcurIndex;
    } else {
        devDir = right;
    }
    
    // if the maximum excursion happened *before* the yaw deviation (!)
    // then this person is out of control (or just came out of a turn). 
    // Set the maxExcursionIndex to the end of the deviation interval
    if (maxExcurIndex < beginYawDev) {
        maxExcurIndex = yawStates[0].end();
        maxExcur = laneExcursion[maxExcurIndex];
    }
    
    // find the largest steering wheel velocity peak between the
    // start of the yaw event and the maximum lane excursion.
    // If we start at a maxima, keep
    // looking since this can't be associated with the yaw
    // event. 
    //
    // jmax will be the location of the velocity extrema
    int start_search = beginYawDev;
    int jmax = start_search;
    while (jmax <= start_search) {
        if (devDir==left) {
            jmax = swavel.range(start_search,maxExcurIndex).max();
        } else {
            jmax = swavel.range(start_search,maxExcurIndex).min();
        }
        ++start_search;
    }
    
    // look backward in time to the first steering wheel reversal
    
    vector<int> reversals;
    int thresh_iter=0;
    const int thresh_iter_max=15;
    float thr = -1.0;
    while (reversals.empty()) {
        if (devDir==left) {
            reversals = find(swavel.range(beginYawDev,jmax) < thr);
        } else {
            reversals = find(swavel.range(beginYawDev,jmax) > -thr);
        }
        thr = thr + 1.0;
        thresh_iter++;
        if (thresh_iter > thresh_iter_max) {
            cout << "max iterations " << thresh_iter << " reached" << endl;
            break;
        }
    }
    
    int ireact = reversals.back();
    
    // ireact is the beginning of steering motion in the direction of
    // of the final correction.
    //
    // jmax is the point at which the steering correction reaches
    // maximum velocity.
    //
    // the 'true' reaction point is almost certainly between these
    // points. We take the maximum steering wheel acceleration in this
    // interval as the location of the steering wheel reaction
    //
    
    int sw_react;
    if (devDir==left) {
        sw_react = swaacc.range(ireact,jmax).max();
    } else {
        sw_react = swaacc.range(ireact,jmax).min();
    }
    
    // finally the 'reaction time' is the interval between the start of the yaw
    // deviation and sw_react
    
    output.rt = (sw_react - beginYawDev)*swa.sampleTime();;
    
    // see if an ldw warning was given during this scenario
    vector<int> ldwWarnings = ldwFlag.range(Scenario).rising();
    if (ldwWarnings.empty()) {
        output.rt_alarm = measures::MISSING;
    } else {    
        output.rt_alarm = (sw_react - ldwWarnings.back())*swa.sampleTime();
    }
    
    return output;
}

/*
    The brake reaction time is defined as 167ms prior to the
    point at which the SV accel dips below 0.1g. This is the CAMP
    definition for FCW type events.
 */

reactionTime_t findBrakingReactionTime(state& Scenario,
                                       stateFlag& scenarioStep,
                                       timeSeries<float>& Ax,
                                       timeSeries<float>& throttle,
                                       timeSeries<float>& brake,
                                       stateFlag& fcwFlag) 
{   const float bDelay = 0.07;  // 167ms in CAMP but 70ms in virttex because
                                // our brakes are too fast  
    int peak = Ax.range(Scenario).min();
    reactionTime_t output;
    
    // find the last throttle release prior to the peak decel
    const float tthresh=0.1;
    int beg = find((throttle.range(Scenario.begin(),peak) > tthresh)).back();
    
    // if this is a left-footed braker and still has the right foot on
    // the gas, give up and just set the window back an arbitrary 5s
    const float bthresh=2.0;
    if (brake[beg] > bthresh) {
        beg = Ax.index(Ax.time(beg) - 5.0);
    }
    // now find the first decel of more than 0.1 g's
    const float decelThresh = -0.1;
    int j = find(Ax.range(beg,peak)<decelThresh).front();
    
    // interpolate to find  the time at which Ax crossed thre threshold
    float deltaT = Ax.sampleTime();
    float deltaA = Ax[j+1] - Ax[j];
    float tcross = (deltaT/deltaA)*(decelThresh - Ax[j]) + Ax.time(j);
    tcross -= bDelay;
    
    // The brake reaction time is relative to the start of lead vehicle braking
    // which occurs on the transition of the scenario from step 5 to step 6
    transition beginPOVDecel(5,6);
    transition endPOVDecel(6,7);
    vector<state> POVDecel = scenarioStep.range(Scenario).findStates(beginPOVDecel,
                                                                     endPOVDecel);
    // there should be one and only one decel in this scenario
    if (POVDecel.size() != 1) {
        cout << "illegal number of POVDecel states found!!!" << endl;
        exit(1);
    }
    
    output.rt = tcross - Ax.time(POVDecel[0].begin());
    
    // see if an fcw warning was given during this scenario
    vector<int> fcwWarnings = fcwFlag.range(Scenario).rising();
    if (fcwWarnings.empty()) {
        output.rt_alarm = measures::MISSING;
    } else {    
        output.rt_alarm = tcross - fcwWarnings[0]*Ax.sampleTime();
    }
    
    return output;
}   

bool isDistracted(string name) {
    if (name.find("_d") == string::npos) {
        return false;
    } else {
        return true;
    }
}

vector<state> findLdwBlocks(stateFlag& ldwTypeFlag) {
    vector<state> ldwStates;
    const int minDuration = int(500.0/ldwTypeFlag.sampleTime());
    
    stateFlag::iterator<change> nextEdge;
    stateFlag::iterator<change> prevEdge;
    for (nextEdge=ldwTypeFlag.begin<change>();nextEdge<ldwTypeFlag.end<change>();) {
        prevEdge=nextEdge;
        state s(prevEdge.index(),(++nextEdge).index());
        if (s.length() >= minDuration) {
            ldwStates.push_back(s);
        }
    }
    
    return ldwStates;
}

int findLdwOrder(vector<state>& ldwStates,int begIndex) {
    int ldwOrder=0;
    for (int j=0;j<ldwStates.size();++j) {
        if (ldwStates[j].contains(begIndex)) {
            ldwOrder=j+1;
            break;
        }
    }
    
    return ldwOrder;
}

int numPreceedingLdwFp(stateFlag sf) {
    int nfp=0;
    
    stateFlag::iterator<rising> nextEdge;
    for (nextEdge=sf.begin<rising>();nextEdge<sf.end<rising>();++nextEdge) {
        // just check to see if this is one of the ldw FP's
        if (*nextEdge>=11 && *nextEdge <=14) ++nfp;
    }
    
    return nfp;
}