campAnalysis.cpp

The following example is an actual analysis program used to process 4025 data files generated by the National Advanced Driving Simulator. The program listing is cross-referenced to the timeSeries library documentation and provides a non-trivial example of how the classes can be used to perform 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 <list>
#include <string>
#include <vector>

using namespace std;
#include "dataFrame.h"
#include "dataFile.h"
#include "timeSeries.h"
#include "timeSeriesArray.h"
#include "lookupTable.h"
#include "TTC2.h"
#include "filter.h"
#include "measures.h"

const double MPHPERFPS=0.68182;
const double FPSPERMPS=3.28084;

string getLeafName(string&);
bool isBrakingTrial(string&);
measures  process_file(string dataFileName, 
                                  dataFrame& df, 
                                  lookupTable& fileTable, 
                                  lookupTable& brakeTable,
                                  lookupTable& steeringTable);
iir_t *init_lowpass();
iir_t *init_cheby1();


template <class T>
void filtfilt(iir_t *lpf,timeSeries<T>& POVspeed_filt);

template <class T>
int findBrakingReactionFrame(timeSeries<T>& SVaccel,
                             timeSeries<T>& SVspeed, 
                             timeSeries<T>& throttle, 
                             timeSeries<T>& brake);

template <class T>
int findSteeringReactionFrame(timeSeries<T>& SVaccel,
                              timeSeries<T>& swavel, 
                              timeSeries<T>& swa,
                              timeSeries<T>& lanePos, 
                              timeSeries<T>& SVYawRate);

void outputMeasures(const string& measuresFile,
                    const measures& dependentMeasures,
                    const int filenum);
#ifdef WIN32
string convertLink(const char *fname);
int analysisMain (int argc, char * const argv[])
#else
int main (int argc, char * const argv[])
#endif
{   using namespace std;
    
    // there are 3 lookup tables that we'll need access to:
    //
    // fileTable, which associates files with misc metadata (subj #, run name, etc)
    // brakingTable, which associates the run name with the braking onset frame #
    // steeringTable, which associates the run name with the steering onset frame #
    //
    // these lookup tables are initialized here, before we begin processing data files
    //
#ifdef WIN32    
    string fileTablePath=
        "\\\\Osprey\\CAMP\\NADS non-data deliverables\\CAMP LookupTables.txt";
    string brakeTablePath=
        "\\\\Osprey\\CAMP\\NADS non-data deliverables\\AllBrakeWithFrame_new.txt";
    string steeringTablePath=
        "\\\\Osprey\\CAMP\\NADS non-data deliverables\\AllSteeringWithFrame.txt";
#else
    string fileTablePath=
        "/Volumes/CAMP/NADS non-data deliverables/CAMP LookupTables.txt";
    string brakeTablePath=
        "/Volumes/CAMP/NADS non-data deliverables/AllBrakeWithFrame_new.txt";
    string steeringTablePath=
        "/Volumes/CAMP/NADS non-data deliverables/AllSteeringWithFrame.txt";
#endif

    try {
        static lookupTable fileTable(fileTablePath,"File Name");
        static lookupTable brakeTable(brakeTablePath,"Run");
        static lookupTable steeringTable(steeringTablePath,"Run");

        // All of the data files are assumed to have the same format so
        // parse the data definition file once before we begin
#ifdef WIN32
        string framePath="\\\\Osprey\\CAMP\\NADS non-data deliverables\\nadsCampFrame.dfm";
#else
        string framePath="/Users/jgreenb2/campAnalysis/analysis/nadsCampFrame.dfm";
#endif

        dataFrame nadsCampFrame(framePath);
        
        for (int filenum=1;filenum<argc;++filenum) {
            string dataFileName(argv[filenum]);
            cout << "reading file " << dataFileName << " " << filenum << " of ";
            cout << argc-1 << "..." << flush;
            try {
                measures dependentMeasures=process_file(dataFileName, 
                                                        nadsCampFrame, 
                                                        fileTable, 
                                                        brakeTable, 
                                                        steeringTable);
                outputMeasures(dataFileName,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);
    }       
    
}

measures process_file(string dataFileName, 
                              dataFrame& nadsCampFrame, 
                              lookupTable& fileTable, 
                              lookupTable& brakeTable,
                              lookupTable& steeringTable)
{   const double Ts=1.0/120.0;
#ifdef WIN32
            dataFileName=convertLink(dataFileName.c_str());
#endif
            
    // load the entire data file into memory

    dataFile data(dataFileName,nadsCampFrame,Ts,dataFile::LittleEndian);
    cout << "done" << endl;
    
    // get the variables we're interested in
    
    timeSeries<float> 
        throttle(nadsCampFrame.findName("CFS_Accelerator_Pedal_Position"),data);
    timeSeries<short> 
        prndl(nadsCampFrame.findName("CFS_Auto_Transmission_Mode"),data);
    timeSeries<float> 
        brake(nadsCampFrame.findName("CFS_Brake_Pedal_Force"),data);
    timeSeries<float> 
        swavel(nadsCampFrame.findName("CFS_Steering_Wheel_Angle_Rate"),data);
    timeSeries<float> 
        swa(nadsCampFrame.findName("CFS_Steering_Wheel_Angle"),data);   
    timeSeries<float> 
        range(nadsCampFrame.findName("SCC_OwnVehToLeadObjDist"),data);
    timeSeriesArray<float> 
        omega(nadsCampFrame.findName("VDS_Chassis_CG_Ang_Vel"),data);
    timeSeriesArray<float> 
        sforces(nadsCampFrame.findName("MIF_Head_Point_Specific_Forces"),data);
    timeSeriesArray<float> 
        lanepos(nadsCampFrame.findName("VDS_Chassis_CG_Position"),data);
    timeSeriesArray<float> 
        cgaccel(nadsCampFrame.findName("VDS_Chassis_CG_Accel"),data);
    timeSeriesArray<float> 
        scenario_vels(nadsCampFrame.findName("SCC_DynObj_Vel"),data);
    
    // the SVaccel is in ft/s, since this is (mostly) what NADS uses. We'll keep to ft 
    // and s for the kinematics here to be consistent
    timeSeries<float>SVaccel = cgaccel.element(0);
    
    // believe it or not, we didn't actually get vehicle speed from NADS so we have to 
    // compute it here
    timeSeries<float>SVspeed = SVaccel.integrate();
    
    // POV speed is buried in the SCC_DynObj_Vel structure
    // and it's in m/s instead of ft/s. We convert it here.
    timeSeries<float>POVspeed = scenario_vels.element(0)*FPSPERMPS;
    
    // POV accel is not directly available, so once again, we calculate it.
    // Use a 10Hz, acausal, phaseless filter to smooth POVspeed first, since the NADS 
    // SCC objects are updated at a much lower sample frequency 
    timeSeries<float>POVspeed_filt = POVspeed;
    iir_t *lpf = init_lowpass();
    filtfilt(lpf,POVspeed_filt);
    timeSeries<float>POVaccel = POVspeed_filt.differentiate();
    free_iir_filter(lpf);
    
    // extract the lateral lane position
    timeSeries<float>SVLatLanePos = lanepos.element(0);

    // and extract the SV yaw rate
    timeSeries<float> SVYawRate = omega.element(2);
    
    // find the frame number at which the steering or braking reaction
    // begins
    
    // start by finding out the name of this run
    string fileName = getLeafName(dataFileName);
    string runName = fileTable.find<string>(fileName,"Run Folder");
    
    cout << runName << endl;
    
    // run names ending in NB or HB are braking while runs ending in NS or HS 
    // are steering trials
    
    bool brakingTrial = isBrakingTrial(runName);
    
    // get the relevant reaction frame from the lookup file
    
    int NADSreactionFrame;
    if (brakingTrial) {
        NADSreactionFrame = brakeTable.find<int>(runName,"FrameOnSVBrakeOnset");
    } else {
        NADSreactionFrame = steeringTable.find<int>(runName,"OwnOnsetFrame");
    }
    
    // since we cannot trust the NADS braking reaction frames we calculate our own
    // and compare the two
    //
        
    int reactionFrame;
    if (brakingTrial) {
        reactionFrame = findBrakingReactionFrame(SVaccel,SVspeed,throttle,brake);
    } else {
        timeSeries<float> SVLatAccel = cgaccel.element(1);
        reactionFrame = findSteeringReactionFrame(SVLatAccel,
                                                  swavel,
                                                  swa,
                                                  SVLatLanePos, 
                                                  SVYawRate);
    }
    
    string alert="";
    if (abs(NADSreactionFrame-reactionFrame) > 30) alert="***";
    cout << (brakingTrial ? "braking " : "steering ") << "nads onset frame= ";
    cout << NADSreactionFrame << alert << endl;
    cout << (brakingTrial ? "braking " : "steering ") << "camp onset frame= ";
    cout << reactionFrame << alert << endl;

    double ttc;
    try {
        ttc = TTC2(SVspeed[reactionFrame],
                  POVspeed[reactionFrame],
                  SVaccel[reactionFrame],
                  POVaccel[reactionFrame],
                  range[reactionFrame]);
    }
    catch (TTC::TTCExceptions& ttce) {
        ttc = -999.0;
    }
    double NADSttc;
    if (brakingTrial) {
        NADSttc = brakeTable.find<double>(runName,"TTC2SVBrakeOnset");
    } else {
        NADSttc = steeringTable.find<double>(runName,"TTC2OnSteerOnset");
    }
    
    const double SystemDelay=0.0;
    double reqDec;
    try {
        reqDec= requiredDecel(SystemDelay,
                              SVspeed[reactionFrame],
                              POVspeed[reactionFrame],
                              SVaccel[reactionFrame],
                              POVaccel[reactionFrame],
                              range[reactionFrame]);
    }
    catch (TTC::TTCExceptions& ttce) {
        reqDec = -999.0;
    }
    
    double NADSreqDec;  
    if (brakingTrial) {
        NADSreqDec = brakeTable.find<double>(runName,"PredictDecelSVBrakeOnset");
    } else {
        NADSreqDec = steeringTable.find<double>(runName,"PreDecelOnSteerOnset");
    }
    
    
    cout << "Camp TTC2= " << ttc << " NADS TTC2= " << NADSttc << endl;
    cout << "Camp rDec=" << reqDec/32 << " g's NADS rDec= " << NADSreqDec/32;
    cout << " g's" << endl;
    cout << endl;
    
    // create the measures
    measures meas;
    
    meas.entry("subno", fileTable.find<int>(fileName,"Subject Number"));
    meas.entry("filename",fileName);
    meas.entry("condition", fileTable.find<string>(fileName,"Condition"));
    meas.entry("runName", runName);
    meas.entry("instruction", fileTable.find<string>(fileName,"Instruction"));
    meas.entry("frameno",reactionFrame);
    meas.entry("NADSframeno", NADSreactionFrame);
    meas.entry("reqdec",reqDec);
    meas.entry("NADSreqdec",NADSreqDec);
    meas.entry("TTC2",ttc);
    meas.entry("NADSTTC2",NADSttc);
    meas.entry("SVspeed",SVspeed[reactionFrame]);
    meas.entry("POVspeed",POVspeed[reactionFrame]);
    meas.entry("SVaccel",SVaccel[reactionFrame]);
    meas.entry("POVaccel",POVaccel[reactionFrame]);
    meas.entry("Range", range[reactionFrame]);
                   
    return meas;
}

/* lowpass is a 3rd order butterworth filter with a break
   frequency at 10 Hz, sampling freq. at 120 Hz
*/

iir_t *init_lowpass()
{   double coeff[][6]= {
                    {1, 1.00000959440837, 0, 1, -0.577350269189628, 0},
                    {1, 1.99999040559162, 0.999990405683675, 1, -1.3856406460551, 0.6}
           };
    double gain=0.0113248654051871;
    iir_t *f;

    f=init_iir_filter(coeff, gain, 2);

    return f;
}

/* cheby1 is a 3rd order low pass Chebychev1 filter with a break frequency of
   5 hz and 1db of passband ripple
*/


iir_t *init_cheby1()
{   double coeff[][6]= {
    {1, 1.00000959440837, 0, 1, -0.877830604810662, 0},
    {1, 1.99999040559162, 0.999990405683675, 1, -1.81608886485109, 0.879775751929623}
    };
    double gain=0.000972573559484491;
    iir_t *f;

    f=init_iir_filter(coeff, gain, 2);

    return f;
}

template <class T>
void filtfilt(iir_t *lpf,timeSeries<T>& ts) {
    // filter in place....
    
    // forward...
    for ( int i=0;i<ts.length();i++) {
        ts[i] = iir_filter(lpf,ts[i]);
    }
    
    // then backward...
    for ( int i=ts.length()-1;i>=0;i--) {
        ts[i] = iir_filter(lpf,ts[i]);
    }
}

string getLeafName(string& file) {
#ifdef WIN32
    size_t beg=file.find_last_of("\\")+1;
#else
    size_t beg=file.find_last_of("/")+1;

#endif
    size_t end=file.find_first_of(".",beg)-1;
    
    return file.substr(beg,end-beg+1);
}

bool isBrakingTrial(string& runName) {
    bool brakingTrial;
    char lastChar = runName[runName.length()-1];
    if (lastChar == 'B') brakingTrial=true;
    else if (lastChar == 'S') brakingTrial=false;
    else {
        cerr<< "bad run name! " << runName << endl; 
        exit(1);
    }
    return brakingTrial;
}

/*
    The brake reaction time is defined as 20 samples prior to the
    point at which the SVaccel dips below 0.1g
 */

template <class T>
int findBrakingReactionFrame(timeSeries<T>& SVaccel,
                             timeSeries<T>& SVspeed,
                             timeSeries<T>& throttle,
                             timeSeries<T>& brake) 
{   
    vector<int> bindx;
    
    // there can be big accel/decel spikes at the prndl transition so
    // start the search for the peak decel after the SV is safely in
    // motion
    bindx = find(SVspeed > 20.0/MPHPERFPS);
    int moving = bindx[0];
    int peak = SVaccel.range(moving,SVaccel.getMaxIndex()).min();

    // find the last throttle release prior to the peak decel
    bindx = find((throttle.range(moving,peak) > 0.0));
    int beg = bindx.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 = SVaccel.index(SVaccel.time(beg) - 5.0);
    }
    // now find the first decel of more than 0.1 g's
    const double decelThresh = -0.1*32.0;
    bindx = find(SVaccel.range(beg,peak) < decelThresh);
    int onsetFrame = bindx[0] - 20;

    return onsetFrame;
}   

template <class T>
int findSteeringReactionFrame(timeSeries<T>& SVLatAccel,
                              timeSeries<T>& swavel, 
                              timeSeries<T>& swa,
                              timeSeries<T>& SVLatLanePos,
                              timeSeries<T>& SVYawRate)
{   const double endingLanePos = -4250;
    const double startingLanePos = -4237;
    
    
    // find the point at which the SV is 60% of the way between lanes
    const double Pt60 = (0.4*startingLanePos+0.6*endingLanePos);
    vector<int> indx;
    indx = find(SVLatLanePos <= Pt60);
    int lane60 = indx.front();
    double t60 = SVLatLanePos.time(lane60);
    const double TswaRange=7.0;
    double t0 = t60 - TswaRange;
    
    // search in a time range beginning TswaRange seconds before
    // the 60% lane change point
    int minaccel = SVLatAccel.range(t0,t60).min();
    double Tminaccel = SVLatAccel.time(minaccel);
    
    // smooth out the swa velocity using a 5Hz acausal filter
    timeSeries<T> swav_f = swavel;
    iir_t *lpf = init_cheby1();
    filtfilt(lpf,swav_f);
    free_iir_filter(lpf);

    const double swaThresh = -2.5;
    timeSeries<T> sv = swav_f.range(t0-10.0,Tminaccel);
    timeSeries<T> sa = swa.range(t0-10.0,Tminaccel);
    vector<int> zerocross =  find((((sv>>1)*sv)<=0.0) && 
                                  (sv<0.0) &&
                                  (sa>swaThresh));
    return zerocross.back();
}

void outputMeasures(const string& dataFileName,
                    const measures& meas,
                    const int filenum)
{   
    // write the header file out the first time only
    if (filenum == 1) {
        ofstream headerFile("campmeas.hdr", ios::out);
        print_labels(headerFile,meas);
        headerFile.close();
    }
    
    int extpos = dataFileName.find(".bin");
    string measFileName = dataFileName.substr(0,extpos) + ".dat";
    ofstream mFile(measFileName.c_str(), ios::out);
    mFile << meas;
    mFile.close();
    
    return;
}