Exemple #1
0
double
get(Matrix *matrix, unsigned row, unsigned col)
{
    if (!matrix) {
        return 0;
    }
    return matrix->data[midx(matrix, row, col)];
}
Exemple #2
0
void
elementwise(Matrix *A, Matrix *B, char op)
{
    if (!A || !B) {
        return;
    }
    unsigned i, j;
    if (A->transposed == B->transposed) {
        if (A->rows != B->rows || A->cols != B->cols) {
            return;
        }
    } else {
        if (A->rows != B->cols || A->cols != B->rows) {
            return;
        }
    }
    #pragma omp parallel for
    for (j = 0; j < A->cols; ++j) {
        for (i = 0; i < A->rows; ++i) {
            switch (op) {
            case '+':
                A->data[midx(A, i, j)] += get(B, i, j);
                break;
            case '-':
                A->data[midx(A, i, j)] -= get(B, i, j);
                break;
            case '*':
                A->data[midx(A, i, j)] *= get(B, i, j);
                break;
            case '/':
                A->data[midx(A, i, j)] /= get(B, i, j) + 1e-6;
                break;
            default:
                break;
            }
        }
    }
}
Exemple #3
0
Matrix *make_matrix_random(int rows, int cols, Random *rnd)
{
    if (!rnd) {
        return NULL;
    }
    Matrix *matrix = make_matrix_zero(rows, cols);
    if (!matrix) {
        return NULL;
    }
    unsigned i, j;
    for (j = 0; j < cols; ++j) {
        for (i = 0; i < rows; ++i) {
            matrix->data[midx(matrix, i, j)] = rnd->ops->next(rnd);
        }
    }
    return matrix;
}
Exemple #4
0
void
calculate_lcs(it xo, it xlo, it xhi, it ylo, it yhi, members & xs_in_lcs)
{
    unsigned const nx = distance(xlo, xhi);
    
    if (nx == 0)
    {
        // empty range. all done
    }
    else if (nx == 1)
    {
        // single item in x range.
        // If it's in the yrange, mark its position in the LCS
        xs_in_lcs[distance(xo, xlo)] = find(ylo, yhi, *xlo) != yhi;
    }
    else
    {
        // split the xrange
        it xmid = xlo + nx / 2;
        
        // Find LCS lengths at xmid, working from both ends of the range
        lengths ll_b, ll_e;
        std::reverse_iterator<it> hix(xhi), midx(xmid), hiy(yhi), loy(ylo);
        
        lcs_lens(xlo, xmid, ylo, yhi, ll_b);
        lcs_lens(hix, midx, hiy, loy, ll_e);
        
        // Find the optimal place to split the y range
        lengths::const_reverse_iterator e = ll_e.rbegin();
        int lmax = -1;
        it y = ylo, ymid = ylo;
        
        for (lengths::const_iterator b = ll_b.begin();
             b != ll_b.end(); ++y, ++b, ++e)
        {
            if (*b + *e > lmax)
            {
                lmax = *b + *e;
                ymid = y;
            }
        }
        // Split the range and recurse
        calculate_lcs(xo, xlo, xmid, ylo, ymid, xs_in_lcs);
        calculate_lcs(xo, xmid, xhi, ymid, yhi, xs_in_lcs);
    }
}
Exemple #5
0
Matrix *read_matrix_uci(FILE *fin)
{
    if (!fin) {
        return NULL;
    }
    unsigned rows = 0, cols = 0, nnz = 0;
    fscanf(fin, "%u%u%u", &cols, &rows, &nnz);
    Matrix *matrix = make_matrix_zero(rows, cols);
    if (!matrix) {
        return NULL;
    }
    unsigned i = 0, j = 0, line;
    double val = 0;
    for (line = 0; line < nnz; ++line) {
        fscanf(fin ,"%u%u%lf", &j, &i, &val);
        matrix->data[midx(matrix, i - 1, j - 1)] = val;
    }
    return matrix;
}
Exemple #6
0
Matrix *read_matrix(FILE *fin)
{
    if (!fin) {
        return NULL;
    }
    unsigned rows = 0, cols = 0;
    fscanf(fin, "%u%u", &cols, &rows);
    Matrix *matrix = make_matrix_zero(rows, cols);
    if (!matrix) {
        return NULL;
    }
    unsigned i, j;
    double val = 0;
    for (j = 0; j < cols; ++j) {
        for (i = 0; i < rows; ++i) {
            fscanf(fin, "%lf", &val);
            matrix->data[midx(matrix, i, j)] = val;
        }
    }
    return matrix;
}
Exemple #7
0
Matrix *submatrix(Matrix *matrix, int offset_r, int offset_c, int rows, int cols)
{
    if (!matrix) {
        return NULL;
    }
    if (offset_r + rows > matrix->rows || offset_c + cols > matrix->cols) {
        return NULL;
    }
    Matrix *submatrix = make_matrix_zero(rows, cols);
    if (!submatrix) {
        return NULL;
    }
    unsigned i, j, mi, mj;
    submatrix->transposed = matrix->transposed;
    for (j = 0; j < cols; ++j) {
        mj = j + offset_c;
        for (i = 0; i < rows; ++i) {
            mi = i + offset_r;
            submatrix->data[midx(submatrix, i, j)] = get(matrix, mi, mj);
        }
    }
    return submatrix;
}
Exemple #8
0
Matrix *dot(Matrix *A, Matrix *B)
{
    if (!A || !B) {
        return NULL;
    }
    unsigned idx;
    int Arows = A->rows;
    int Acols = A->cols;
    int Brows = B->rows;
    int Bcols = B->cols;
    if (A->transposed) {
        Arows = A->cols;
        Acols = A->rows;
    }
    if (B->transposed) {
        Brows = B->cols;
        Bcols = B->rows;
    }
    if (Acols != Brows) {
        return NULL;
    }
    Matrix *C = make_matrix_zero(Arows, Bcols);
    #pragma omp parallel for
    for (idx = 0; idx < Arows * Bcols; ++idx) {
        unsigned i, j;
        i = idx % Arows;
        j = idx / Arows;
        double sum = 0.0;
        unsigned k;
        for (k = 0; k < Acols; ++k) {
            sum += get(A, i, k) * get(B, k, j);
        }
        C->data[midx(C, i, j)] = sum;
    }
    return C;
}
DWORD WINAPI LogCallback(LPVOID lpparam)
{
	/*
	Callback function for reading data from log file.

	INPUTS:
		lpparam - a pointer to the LogSensor that owns this reader thread.

	OUTPUTS:
		none.  (always returns 0)
	*/

	//cast the input argument to an instance of the sensor container class
	SceneEstimatorLogSensor *SensorLogs = (SceneEstimatorLogSensor *)lpparam;

	errno_t retcode;
	//this will be used to create each new event as it arrives
	Event* NewEvent = NULL;
	Event* NewEvent2 = NULL;
	//access to the posterior pose event queue
	extern SynchronizedEventQueue* ThePosteriorPoseEventQueue;
	extern SynchronizedEventQueue* TheTrackGeneratorEventQueue;
	//access to the posterior pose odometry queue
	extern RelativePoseQueue* TheRelativePoseQueue;
	//access to the car time
	extern CarTime* TheCarTime;
	//access to the logging queue
	#ifdef SE_PRINTLOGS
		extern EventQueue* TheLoggingQueue;
		Event* LogEvent;
	#endif

	int i;
	int j;
	int k;
	//for determining which event happens next
	double eventtimes[9];
	int enext;
	double etime = DBL_MAX;

	while (SensorLogs->IsRunning() == true)
	{
		//create an event each time sensor data is present

		//this will store the event data (i.e. the entire packet)
		int iEventType = INVALID_EVENT;
		//the event's time stamp
		double iEventTime = DBL_MAX;
		//the number of elements in the event
		int ine = 0;
		//the number of columns per row of event data
		int inc = 0;
		int ine2 = 0;
		int inc2 = 0;
		int ine3 = 0;
		int inc3 = 0;
		double* iEventData = NULL;
		double* iEventData2 = NULL;
		double* iEventData3 = NULL;

		//find which event happens next
		eventtimes[0] = SensorLogs->OdomLog.LogTime();
		eventtimes[1] = SensorLogs->PoseLog.LogTime();
		eventtimes[2] = SensorLogs->FrontMobileyeRoadLog.LogTime();
		eventtimes[3] = SensorLogs->BackMobileyeRoadLog.LogTime();
		eventtimes[4] = SensorLogs->FrontJasonRoadLog.LogTime();
		eventtimes[5] = SensorLogs->BackJasonRoadLog.LogTime();
		eventtimes[6] = SensorLogs->StoplineLog.LogTime();
		eventtimes[7] = SensorLogs->LocalMapLog.LogTime();
		eventtimes[8] = SensorLogs->LocalPointsLog.LogTime();
		enext = -1;
		etime = DBL_MAX;

		for (i = 0; i < 9; i++)
		{
			if (eventtimes[i] < etime)
			{
				enext = i;
				etime = eventtimes[i];
			}
		}

		switch (enext)
		{
			case 0:
				{
					//odometry packet comes next

					//parse out the odometry event information
					iEventData = new double[SE_ODOMPACKETSIZE];
					retcode = StringToArrayOfDoubles(iEventData, SE_ODOMPACKETSIZE, SensorLogs->OdomLog.LogBuffer());

					if (retcode != SE_ODOMPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed odometry log packet.\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						delete [] iEventData;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully

						//for log files, use the odometry to drive the car time
						TheCarTime->SetCurrentCarTime(iEventData[0]);

						#ifdef SE_PRINTLOGS
							//extract the event time

							iEventTime = iEventData[0];
							iEventType = ODOM_EVENT;
							ine = 1;
							inc = SE_ODOMPACKETSIZE;

							//create a separate logging event with the same data
							LogEvent = new Event();
							double* iLogData = new double[SE_ODOMPACKETSIZE];
							memcpy(iLogData, iEventData, sizeof(double) * inc);
							LogEvent->SetEventType(iEventType, iEventTime);
							LogEvent->SetEventData(ine, inc, iLogData);
						#endif

						bool spush = TheRelativePoseQueue->PushPacket(iEventData);
						#ifdef SE_PRINTLOGS
							//only log the packet if it successfully entered the event queue
							if (spush == true)
							{
								TheLoggingQueue->PushEvent(LogEvent);
							}
							else
							{
								delete LogEvent;
							}
						#endif

						//note: now the event gets pushed directly to the relative pose queue
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						delete [] iEventData;
						iEventData = NULL;
					}

					//done reading the odometry event; read the next line
					SensorLogs->OdomLog.GetNextLogLine();
				}

				break;

			case 1:
				{
					//pose packet comes next

					//parse out the odometry event information
					iEventData = new double[SE_POSEPACKETSIZE];
					retcode = StringToArrayOfDoubles(iEventData, SE_POSEPACKETSIZE, SensorLogs->PoseLog.LogBuffer());

					if (retcode != SE_POSEPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed pose log packet.\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						delete [] iEventData;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully
						//extract the event time
						iEventTime = iEventData[0];
						iEventType = POSE_EVENT;
						ine = 1;
						inc = SE_POSEPACKETSIZE;
					}

					//done reading the event; read the next line
					SensorLogs->PoseLog.GetNextLogLine();
				}

				break;

			case 2:
				{
					//front mobileye packet comes next

					//parse out the mobileye event information
					iEventData = new double[SE_MOBILEYEROADPACKETSIZE];
					retcode = StringToArrayOfDoubles(iEventData, SE_MOBILEYEROADPACKETSIZE, SensorLogs->FrontMobileyeRoadLog.LogBuffer());

					if (retcode != SE_MOBILEYEROADPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed front mobileye log packet.\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						delete [] iEventData;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully
						//extract the event time
						iEventTime = iEventData[0];
						iEventType = FRONTMOBILEYEROAD_EVENT;
						ine = 1;
						inc = SE_MOBILEYEROADPACKETSIZE;
					}

					//done reading the event; read the next line
					SensorLogs->FrontMobileyeRoadLog.GetNextLogLine();
				}

				break;

			case 3:
				{
					//back mobileye packet comes next

					//parse out the mobileye event information
					iEventData = new double[SE_MOBILEYEROADPACKETSIZE];
					retcode = StringToArrayOfDoubles(iEventData, SE_MOBILEYEROADPACKETSIZE, SensorLogs->BackMobileyeRoadLog.LogBuffer());

					if (retcode != SE_MOBILEYEROADPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed back mobileye log packet.\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						delete [] iEventData;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully
						//extract the event time
						iEventTime = iEventData[0];
						iEventType = BACKMOBILEYEROAD_EVENT;
						ine = 1;
						inc = SE_MOBILEYEROADPACKETSIZE;
					}

					//done reading the event; read the next line
					SensorLogs->BackMobileyeRoadLog.GetNextLogLine();
				}

				break;

			case 4:
				{
					//front jason packet comes next

					//parse out the jason event information
					double tEventData[SE_JASONROADPACKETSIZE];
					retcode = StringToArrayOfDoubles(tEventData, SE_JASONROADPACKETSIZE, SensorLogs->FrontJasonRoadLog.LogBuffer());

					if (retcode != SE_JASONROADPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed front jason log packet.\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully

						//extract the event time
						iEventTime = tEventData[0];
						iEventType = FRONTJASONROAD_EVENT;
						ine = (int) (tEventData[2]);
						inc = SE_JASONROADPACKETSIZE;

						iEventData = new double[inc*ine];
						for (j = 0; j < inc; j++)
						{
							//copy the first line of the packet into iEventData
							iEventData[midx(0, j, ine)] = tEventData[j];
						}

						for (i = 1; i < ine; i++)
						{
							//read the rest of the packet line by line
							SensorLogs->FrontJasonRoadLog.GetNextLogLine();
							retcode = StringToArrayOfDoubles(tEventData, SE_JASONROADPACKETSIZE, SensorLogs->FrontJasonRoadLog.LogBuffer());

							if (retcode != SE_JASONROADPACKETSIZE)
							{
								//improper packet read: throw out this packet
								printf("Malformed front jason log packet.\n");
								iEventTime = DBL_MAX;
								iEventType = INVALID_EVENT;
								ine = 0;
								inc = 0;
								delete [] iEventData;
								iEventData = NULL;

								break;
							}

							for (j = 0; j < SE_JASONROADPACKETSIZE; j++)
							{
								//copy the next line of the packet into iEventData
								iEventData[midx(i, j, ine)] = tEventData[j];
							}
						}
					}

					//done reading the event; read the next line
					SensorLogs->FrontJasonRoadLog.GetNextLogLine();
				}

				break;

			case 5:
				{
					//back jason packet comes next

					//parse out the jason event information
					double tEventData[SE_JASONROADPACKETSIZE];
					retcode = StringToArrayOfDoubles(tEventData, SE_JASONROADPACKETSIZE, SensorLogs->BackJasonRoadLog.LogBuffer());

					if (retcode != SE_JASONROADPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed back jason log packet.\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully

						//extract the event time
						iEventTime = tEventData[0];
						iEventType = BACKJASONROAD_EVENT;
						ine = (int) (tEventData[2]);
						inc = SE_JASONROADPACKETSIZE;

						iEventData = new double[inc*ine];
						for (j = 0; j < inc; j++)
						{
							//copy the first line of the packet into iEventData
							iEventData[midx(0, j, ine)] = tEventData[j];
						}

						for (i = 1; i < ine; i++)
						{
							//read the rest of the packet line by line
							SensorLogs->FrontJasonRoadLog.GetNextLogLine();
							retcode = StringToArrayOfDoubles(tEventData, SE_JASONROADPACKETSIZE, SensorLogs->BackJasonRoadLog.LogBuffer());

							if (retcode != SE_JASONROADPACKETSIZE)
							{
								//improper packet read: throw out this packet
								printf("Malformed back jason log packet.\n");
								iEventTime = DBL_MAX;
								iEventType = INVALID_EVENT;
								ine = 0;
								inc = 0;
								delete [] iEventData;
								iEventData = NULL;

								break;
							}

							for (j = 0; j < SE_JASONROADPACKETSIZE; j++)
							{
								//copy the next line of the packet into iEventData
								iEventData[midx(i, j, ine)] = tEventData[j];
							}
						}
					}

					//done reading the event; read the next line
					SensorLogs->BackJasonRoadLog.GetNextLogLine();
				}

				break;

			case 6:
				{
					//stopline packet comes next

					//parse out the stopline event information
					double tEventData[SE_STOPLINEPACKETSIZE];
					retcode = StringToArrayOfDoubles(tEventData, SE_STOPLINEPACKETSIZE, SensorLogs->StoplineLog.LogBuffer());

					if (retcode != SE_STOPLINEPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed stopline log packet.\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully

						//extract the event time
						iEventTime = tEventData[0];
						iEventType = STOPLINE_EVENT;
						ine = (int) (tEventData[2]);
						inc = SE_STOPLINEPACKETSIZE;

						iEventData = new double[inc*ine];
						for (j = 0; j < SE_STOPLINEPACKETSIZE; j++)
						{
							//copy the first line of the packet into iEventData
							iEventData[midx(0, j, ine)] = tEventData[j];
						}

						for (i = 1; i < ine; i++)
						{
							//read the rest of the packet line by line
							SensorLogs->StoplineLog.GetNextLogLine();
							retcode = StringToArrayOfDoubles(tEventData, SE_STOPLINEPACKETSIZE, SensorLogs->StoplineLog.LogBuffer());

							if (retcode != SE_STOPLINEPACKETSIZE)
							{
								//improper packet read: throw out this packet
								printf("Malformed stopline log packet.\n");
								iEventTime = DBL_MAX;
								iEventType = INVALID_EVENT;
								ine = 0;
								inc = 0;
								delete [] iEventData;
								iEventData = NULL;

								break;
							}

							for (j = 0; j < inc; j++)
							{
								//copy the next line of the packet into iEventData
								iEventData[midx(i, j, ine)] = tEventData[j];
							}
						}
					}

					//done reading the event; read the next line
					SensorLogs->StoplineLog.GetNextLogLine();
				}

				break;

			case 7:
				{
					//local map packet comes next

					//parse out the localmap event information
					double tEventData[SE_LMTHEADERPACKETSIZE];
					double tEventData2[SE_LMTCOVARIANCEPACKETSIZE];
					double tEventData3[SE_LMTPOINTSPACKETSIZE];

					//parse the first target header
					retcode = StringToArrayOfDoubles(tEventData, SE_LMTHEADERPACKETSIZE, SensorLogs->LocalMapLog.LogBuffer());

					if (retcode != SE_LMTHEADERPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed localmap target log packet.\n");
						//try to advance the log
						SensorLogs->LocalMapLog.GetNextLogLine();
					}
					else
					{
						//if code gets here, tEventData was created successfully

						//extract the event time
						iEventTime = tEventData[0];
						iEventType = LOCALMAPTARGETS_EVENT;

						//extract the number of targets and store target headers in iEventData
						ine = (int) (tEventData[2]);
						inc = SE_LMTHEADERPACKETSIZE;
						iEventData = new double[inc*ine];

						ine2 = ine;
						inc2 = SE_LMTCOVARIANCEPACKETSIZE;
						iEventData2 = new double[ine2*inc2];

						ine3 = 0;
						inc3 = SE_LMTPOINTSPACKETSIZE;
						//create temporary space to hold all the points
						int tidx = 0;
						double tSpace[LS_MAXTARGETPOINTS*SE_LMTPOINTSPACKETSIZE];

						for (i = 0; i < ine; i++)
						{
							//read in each target

							//1. copy the target header into iEventData
							if (retcode != SE_LMTHEADERPACKETSIZE)
							{
								//improper packet read: throw out this packet
								printf("Malformed localmap target log packet.\n");
								iEventTime = DBL_MAX;
								iEventType = INVALID_EVENT;

								ine = 0;
								inc = 0;
								delete [] iEventData;
								iEventData = NULL;

								ine2 = 0;
								inc2 = 0;
								delete [] iEventData2;
								iEventData2 = NULL;

								ine3 = 0;
								inc3 = 0;
								delete [] iEventData3;
								iEventData3 = NULL;

								break;
							}

							for (j = 0; j < SE_LMTHEADERPACKETSIZE; j++)
							{
								iEventData[midx(i, j, ine)] = tEventData[j];
							}
							//pull the number of points for this target
							int np = (int) tEventData[10];

							//2. read the covariance packet for this target
							SensorLogs->LocalMapLog.GetNextLogLine();
							retcode = StringToArrayOfDoubles(tEventData2, SE_LMTCOVARIANCEPACKETSIZE, SensorLogs->LocalMapLog.LogBuffer());

							if (retcode != SE_LMTCOVARIANCEPACKETSIZE)
							{
								//improper packet read: throw out this packet
								printf("Malformed localmap target log packet.\n");
								iEventTime = DBL_MAX;
								iEventType = INVALID_EVENT;

								ine = 0;
								inc = 0;
								delete [] iEventData;
								iEventData = NULL;

								ine2 = 0;
								inc2 = 0;
								delete [] iEventData2;
								iEventData2 = NULL;

								ine3 = 0;
								inc3 = 0;
								delete [] iEventData3;
								iEventData3 = NULL;

								break;
							}

							//copy the target covariance into iEventData2
							for (j = 0; j < inc2; j++)
							{
								iEventData2[midx(i, j, ine)] = tEventData2[j];
							}

							//3. read each points packet for this target
							bool pSuccess = true;
							if (np > 0)
							{
								for (j = 0; j < np; j++)
								{
									SensorLogs->LocalMapLog.GetNextLogLine();
									retcode = StringToArrayOfDoubles(tEventData3, SE_LMTPOINTSPACKETSIZE, SensorLogs->LocalMapLog.LogBuffer());

									if (retcode != SE_LMTPOINTSPACKETSIZE)
									{
										//improper packet read: throw out this packet
										pSuccess = false;
										break;
									}

									//copy the point into tSpace
									for (k = 0; k < inc3; k++)
									{
										tSpace[midx(tidx, k, LS_MAXTARGETPOINTS)] = tEventData3[k];
									}
									tidx++;
								}
							}

							if (pSuccess == false)
							{
								//improper packet read: throw out this packet
								printf("Malformed localmap target log packet.\n");
								iEventTime = DBL_MAX;
								iEventType = INVALID_EVENT;

								ine = 0;
								inc = 0;
								delete [] iEventData;
								iEventData = NULL;

								ine2 = 0;
								inc2 = 0;
								delete [] iEventData2;
								iEventData2 = NULL;

								ine3 = 0;
								inc3 = 0;
								delete [] iEventData3;
								iEventData3 = NULL;

								//try to advance the log
								SensorLogs->LocalMapLog.GetNextLogLine();

								break;
							}

							//done parsing one target, move to the next
							SensorLogs->LocalMapLog.GetNextLogLine();
							retcode = StringToArrayOfDoubles(tEventData, SE_LMTHEADERPACKETSIZE, SensorLogs->LocalMapLog.LogBuffer());
						}

						//done parsing all targets, now set the event data
						ine3 = tidx;
						if (ine3 > 0)
						{
							//if there are any points, allocate memory and copy them into iEventData3
							iEventData3 = new double[ine3*inc3];

							for (i = 0; i < ine3; i++)
							{
								for (j = 0; j < inc3; j++)
								{
									iEventData3[midx(i, j, ine3)] = tSpace[midx(i, j, LS_MAXTARGETPOINTS)];
								}
							}
						}

						//all the targets have been processed
						//NOTE: the next target header is already loaded
					}
				}

				break;

			case 8:
				{
					//local points packet comes next

					//parse out the localmap event information
					double tEventData[SE_LOCALPOINTSPACKETSIZE];
					retcode = StringToArrayOfDoubles(tEventData, SE_LOCALPOINTSPACKETSIZE, SensorLogs->LocalPointsLog.LogBuffer());

					if (retcode != SE_LOCALPOINTSPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed local points log packet\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully

						//extract the event time
						iEventTime = tEventData[0];
						iEventType = LOCALMAPPOINTS_EVENT;
						ine = (int) (tEventData[2]);
						inc = SE_LOCALPOINTSPACKETSIZE;

						iEventData = new double[inc*ine];
						for (j = 0; j < inc; j++)
						{
							//copy the first line of the packet into iEventData
							iEventData[midx(0, j, ine)] = tEventData[j];
						}

						for (i = 1; i < ine; i++)
						{
							//read the rest of the packet line by line
							SensorLogs->LocalPointsLog.GetNextLogLine();
							retcode = StringToArrayOfDoubles(tEventData, SE_LOCALPOINTSPACKETSIZE, SensorLogs->LocalPointsLog.LogBuffer());

							if (retcode != SE_LOCALPOINTSPACKETSIZE)
							{
								//improper packet read: throw out this packet
								printf("Malformed local points log packet\n");
								iEventTime = DBL_MAX;
								iEventType = INVALID_EVENT;
								ine = 0;
								delete [] iEventData;
								iEventData = NULL;

								break;
							}

							for (j = 0; j < inc; j++)
							{
								//copy the next line of the packet into iEventData
								iEventData[midx(i, j, ine)] = tEventData[j];
							}
						}
					}

					//done reading the event; read the next line
					SensorLogs->LocalPointsLog.GetNextLogLine();
				}

				break;

			default:
				{
					//invalid event
					iEventType = INVALID_EVENT;
					iEventTime = DBL_MAX;
					ine = 0;
					inc = 0;
					iEventData = NULL;
				}

				break;
		}

		//when no data is left, "disconnect" the sensor
		if (etime == DBL_MAX)
		{
			printf("Log sensors terminating...\n");
			iEventType = INVALID_EVENT;
			iEventTime = DBL_MAX;
			ine = 0;
			inc = 0;
			iEventData = NULL;
			SensorLogs->StopRunning();
		}

		if (iEventType == INVALID_EVENT || SensorLogs->IsRunning() == false)
		{
			//for invalid reads or terminating logfiles, go to sleep
			//note: memory has already been deallocated for bad reads

			//sleep for the delay time of the sensor
			Sleep(SensorLogs->SamplePeriod());
			continue;
		}

		//when the code gets here, iEventData is populated with a complete sensor packet
		//create the event data
		NewEvent = new Event();
		NewEvent->SetEventType(iEventType, iEventTime);
		NewEvent->SetEventData(ine, inc, iEventData);
		NewEvent->SetEventData2(ine2, inc2, iEventData2);
		NewEvent->SetEventData3(ine3, inc3, iEventData3);
		#ifdef SE_PRINTLOGS
			//create a separate logging event with the same data
			LogEvent = new Event(NewEvent);
		#endif

		bool spush = false;

		switch (NewEvent->EventType())
		{
		case ODOM_EVENT:
		case POSE_EVENT:
		case FRONTMOBILEYEROAD_EVENT:
		case BACKMOBILEYEROAD_EVENT:
		case FRONTJASONROAD_EVENT:
		case BACKJASONROAD_EVENT:
			//positioning packets go to PosteriorPose

			spush = ThePosteriorPoseEventQueue->PushEvent(NewEvent);

			break;

		case LOCALMAPTARGETS_EVENT:
		case LOCALMAPPOINTS_EVENT:
			//localmap packets go to track generator

			spush = TheTrackGeneratorEventQueue->PushEvent(NewEvent);

			break;

		case QUIT_EVENT:
		case TRANSMIT_EVENT:
			//message events go to both queues

			NewEvent2 = new Event(NewEvent);

			spush = ThePosteriorPoseEventQueue->PushEvent(NewEvent);
			TheTrackGeneratorEventQueue->PushEvent(NewEvent2);

			break;
		}

		#ifdef SE_PRINTLOGS
			//only log the packet if it successfully entered the event queue
			if (spush == true)
			{
				TheLoggingQueue->PushEvent(LogEvent);
			}
			else
			{
				delete LogEvent;
			}
		#endif

		//sleep for the delay time of the sensor
		Sleep(SensorLogs->SamplePeriod());
	}

	return 0;
}
DWORD WINAPI LogCallback(LPVOID lpparam)
{
	/*
	Callback function for reading data from log file.

	INPUTS:
		lpparam - a pointer to the LogSensor that owns this reader thread.

	OUTPUTS:
		none.  (always returns 0)
	*/

	//cast the input argument to an instance of the sensor container class
	LocalMapLogSensor *SensorLogs = (LocalMapLogSensor *)lpparam;

	errno_t retcode;
	//this will store the event data (i.e. the entire packet)
	int iEventType = INVALID_EVENT;
	//the event's time stamp
	double iEventTime = DBL_MAX;
	//the number of elements in the event
	int ine = 0;
	//the number of columns per row of event data
	int inc = 0;
	double *iEventData = NULL;
	//this will be used to create each new event as it arrives
	Event* NewEvent = NULL;
	Event* NewEvent2 = NULL;
	//access to the event queues
	extern SynchronizedEventQueue* TheLocalMapEventQueue;
	extern SynchronizedEventQueue* TheLocalRoadEventQueue;
	//access to the odometry queue
	extern RelativePoseQueue* TheRelativePoseQueue;
	//access to the car time
	extern CarTime* TheCarTime;
	//access to the logging queue
	#ifdef LM_PRINTLOGS
		extern EventQueue* TheLoggingQueue;
		Event* LogEvent;
	#endif

	int i;
	int j;
	//for determining which event happens next
	double eventtimes[20];
	int enext;
	double etime = DBL_MAX;
	ReaderLogFile* TheReaderLogFile;

	while (SensorLogs->IsRunning() == true)
	{
		//create an event each time sensor data is present

		//find which event happens next
		eventtimes[0] = SensorLogs->OdomLog.LogTime();
		eventtimes[1] = SensorLogs->FrontMobileyeObstacleLog.LogTime();
		eventtimes[2] = SensorLogs->FrontMobileyeRoadLog.LogTime();
		eventtimes[3] = SensorLogs->BackMobileyeObstacleLog.LogTime();
		eventtimes[4] = SensorLogs->BackMobileyeRoadLog.LogTime();
		eventtimes[5] = SensorLogs->FrontJasonRoadLog.LogTime();
		eventtimes[6] = SensorLogs->BackJasonRoadLog.LogTime();
		eventtimes[7] = SensorLogs->IbeoLog.LogTime();
		eventtimes[8] = SensorLogs->BackClusteredSickLog.LogTime();
		eventtimes[9] = SensorLogs->Front0RadarLog.LogTime();
		eventtimes[10] = SensorLogs->Driv0RadarLog.LogTime();
		eventtimes[11] = SensorLogs->Driv1RadarLog.LogTime();
		eventtimes[12] = SensorLogs->Driv2RadarLog.LogTime();
		eventtimes[13] = SensorLogs->Driv3RadarLog.LogTime();
		eventtimes[14] = SensorLogs->Pass0RadarLog.LogTime();
		eventtimes[15] = SensorLogs->Pass1RadarLog.LogTime();
		eventtimes[16] = SensorLogs->Pass2RadarLog.LogTime();
		eventtimes[17] = SensorLogs->Pass3RadarLog.LogTime();
		eventtimes[18] = SensorLogs->DrivSideSickLog.LogTime();
		eventtimes[19] = SensorLogs->PassSideSickLog.LogTime();
		enext = -1;
		etime = DBL_MAX;

		for (i = 0; i < 20; i++)
		{
			if (eventtimes[i] < etime)
			{
				enext = i;
				etime = eventtimes[i];
			}
		}

		switch (enext)
		{
			case 0:
				{
					//odometry packet comes next

					//parse out the odometry event information
					iEventData = new double[LM_ODOMPACKETSIZE];
					retcode = StringToArrayOfDoubles(iEventData, LM_ODOMPACKETSIZE, SensorLogs->OdomLog.LogBuffer());

					if (retcode != LM_ODOMPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed odometry log packet\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						delete [] iEventData;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully

						//for log files, use the odometry to drive the car time
						TheCarTime->SetCurrentCarTime(iEventData[0]);

						#ifdef LM_PRINTLOGS
							//extract the event time

							iEventTime = iEventData[0];
							iEventType = ODOM_EVENT;
							ine = 1;
							inc = LM_ODOMPACKETSIZE;

							//create a separate logging event with the same data
							LogEvent = new Event();
							double* iLogData = new double[LM_ODOMPACKETSIZE];
							memcpy(iLogData, iEventData, sizeof(double) * inc);
							LogEvent->SetEventType(iEventType, iEventTime);
							LogEvent->SetEventData(ine, inc, iLogData);
						#endif

						bool spush = TheRelativePoseQueue->PushPacket(iEventData);
						#ifdef LM_PRINTLOGS
							//only log the packet if it successfully entered the event queue
							if (spush == true)
							{
								TheLoggingQueue->PushEvent(LogEvent);
							}
							else
							{
								delete LogEvent;
							}
						#endif

						//note: now the event gets pushed directly to the relative pose queue
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						delete [] iEventData;
						iEventData = NULL;
					}

					//done reading the odometry event; read the next line
					SensorLogs->OdomLog.GetNextLogLine();
				}

				break;

			case 1:
			case 3:
				{
					//front / back mobileye obstacle packet comes next

					switch(enext)
					{
						case 1:
							iEventType = FRONTMOBILEYEOBSTACLE_EVENT;
							TheReaderLogFile = &(SensorLogs->FrontMobileyeObstacleLog);
							break;
						case 3:
							iEventType = BACKMOBILEYEOBSTACLE_EVENT;
							TheReaderLogFile = &(SensorLogs->BackMobileyeObstacleLog);
							break;
					}

					//parse out the mobileye event information
					double tEventData[LM_MOBILEYEOBSTACLEPACKETSIZE];
					retcode = StringToArrayOfDoubles(tEventData, LM_MOBILEYEOBSTACLEPACKETSIZE, TheReaderLogFile->LogBuffer());

					if (retcode != LM_MOBILEYEOBSTACLEPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed mobileye obstacle log packet\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully

						//extract the event time
						iEventTime = tEventData[0];
						ine = (int) (tEventData[2]);
						inc = LM_MOBILEYEOBSTACLEPACKETSIZE;

						iEventData = new double[inc*ine];
						for (j = 0; j < inc; j++)
						{
							//copy the first line of the packet into iEventData
							iEventData[midx(0, j, ine)] = tEventData[j];
						}

						for (i = 1; i < ine; i++)
						{
							//read the rest of the packet line by line
							TheReaderLogFile->GetNextLogLine();
							retcode = StringToArrayOfDoubles(tEventData, LM_MOBILEYEOBSTACLEPACKETSIZE, TheReaderLogFile->LogBuffer());

							if (retcode != LM_MOBILEYEOBSTACLEPACKETSIZE)
							{
								//improper packet read: throw out this packet
								printf("Malformed mobileye obstacle log packet\n");
								iEventTime = DBL_MAX;
								iEventType = INVALID_EVENT;
								ine = 0;
								inc = 0;
								delete [] iEventData;
								iEventData = NULL;

								break;
							}

							for (j = 0; j < LM_MOBILEYEOBSTACLEPACKETSIZE; j++)
							{
								//copy the next line of the packet into iEventData
								iEventData[midx(i, j, ine)] = tEventData[j];
							}
						}
					}

					//done reading the event; read the next line
					TheReaderLogFile->GetNextLogLine();
				}

				break;

			case 2:
			case 4:
				{
					//front / back mobileye road packet comes next

					switch(enext)
					{
						case 2:
							iEventType = FRONTMOBILEYEROAD_EVENT;
							TheReaderLogFile = &(SensorLogs->FrontMobileyeRoadLog);
							break;
						case 4:
							iEventType = BACKMOBILEYEROAD_EVENT;
							TheReaderLogFile = &(SensorLogs->BackMobileyeRoadLog);
							break;
					}

					//parse out the mobileye event information
					iEventData = new double[LM_MOBILEYEROADPACKETSIZE];
					retcode = StringToArrayOfDoubles(iEventData, LM_MOBILEYEROADPACKETSIZE, TheReaderLogFile->LogBuffer());

					if (retcode != LM_MOBILEYEROADPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed mobileye road log packet\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						delete [] iEventData;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully
						//extract the event time
						iEventTime = iEventData[0];
						ine = 1;
						inc = LM_MOBILEYEROADPACKETSIZE;
					}

					//done reading the event; read the next line
					TheReaderLogFile->GetNextLogLine();
				}

				break;

			case 5:
			case 6:
				{
					//front / back jason packet comes next

					switch(enext)
					{
						case 5:
							iEventType = FRONTJASONROAD_EVENT;
							TheReaderLogFile = &(SensorLogs->FrontJasonRoadLog);
							break;
						case 6:
							iEventType = BACKJASONROAD_EVENT;
							TheReaderLogFile = &(SensorLogs->BackJasonRoadLog);
							break;
					}

					//parse out the jason event information
					double tEventData[LM_JASONROADPACKETSIZE];
					retcode = StringToArrayOfDoubles(tEventData, LM_JASONROADPACKETSIZE, TheReaderLogFile->LogBuffer());

					if (retcode != LM_JASONROADPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed jason road log packet\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully

						//extract the event time
						iEventTime = tEventData[0];
						ine = (int) (tEventData[2]);
						inc = LM_JASONROADPACKETSIZE;

						iEventData = new double[inc*ine];
						for (j = 0; j < inc; j++)
						{
							//copy the first line of the packet into iEventData
							iEventData[midx(0, j, ine)] = tEventData[j];
						}

						for (i = 1; i < ine; i++)
						{
							//read the rest of the packet line by line
							SensorLogs->FrontJasonRoadLog.GetNextLogLine();
							retcode = StringToArrayOfDoubles(tEventData, LM_JASONROADPACKETSIZE, TheReaderLogFile->LogBuffer());

							if (retcode != LM_JASONROADPACKETSIZE)
							{
								//improper packet read: throw out this packet
								printf("Malformed jason road log packet\n");
								iEventTime = DBL_MAX;
								iEventType = INVALID_EVENT;
								ine = 0;
								inc = 0;
								delete [] iEventData;
								iEventData = NULL;

								break;
							}

							for (j = 0; j < LM_JASONROADPACKETSIZE; j++)
							{
								//copy the next line of the packet into iEventData
								iEventData[midx(i, j, ine)] = tEventData[j];
							}
						}
					}

					//done reading the event; read the next line
					TheReaderLogFile->GetNextLogLine();
				}

				break;

			case 7:
				{
					//front ibeo packet comes next

					//parse out the front ibeo event information
					double tEventData[LM_CLUSTEREDIBEOPACKETSIZE];
					retcode = StringToArrayOfDoubles(tEventData, LM_CLUSTEREDIBEOPACKETSIZE, SensorLogs->IbeoLog.LogBuffer());

					if (retcode != LM_CLUSTEREDIBEOPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed front ibeo log packet\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully

						//extract the event time
						iEventTime = tEventData[0];
						iEventType = IBEO_EVENT;
						ine = (int) (tEventData[2]);
						inc = LM_CLUSTEREDIBEOPACKETSIZE;

						iEventData = new double[inc*ine];
						for (j = 0; j < LM_CLUSTEREDIBEOPACKETSIZE; j++)
						{
							//copy the first line of the packet into iEventData
							iEventData[midx(0, j, ine)] = tEventData[j];
						}

						for (i = 1; i < ine; i++)
						{
							//read the rest of the packet line by line
							SensorLogs->IbeoLog.GetNextLogLine();
							retcode = StringToArrayOfDoubles(tEventData, LM_CLUSTEREDIBEOPACKETSIZE, SensorLogs->IbeoLog.LogBuffer());

							if (retcode != LM_CLUSTEREDIBEOPACKETSIZE)
							{
								//improper packet read: throw out this packet
								printf("Malformed front ibeo log packet\n");
								iEventTime = DBL_MAX;
								iEventType = INVALID_EVENT;
								ine = 0;
								inc = 0;
								delete [] iEventData;
								iEventData = NULL;

								break;
							}

							for (j = 0; j < inc; j++)
							{
								//copy the next line of the packet into iEventData
								iEventData[midx(i, j, ine)] = tEventData[j];
							}
						}
					}

					//done reading the event; read the next line
					SensorLogs->IbeoLog.GetNextLogLine();
				}

				break;

			case 8:
				{
					//back clustered sick packet comes next

					//parse out the front ibeo event information
					double tEventData[LM_CLUSTEREDSICKPACKETSIZE];
					retcode = StringToArrayOfDoubles(tEventData, LM_CLUSTEREDSICKPACKETSIZE, SensorLogs->BackClusteredSickLog.LogBuffer());

					if (retcode != LM_CLUSTEREDSICKPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed back clustered sick log packet\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully

						//extract the event time
						iEventTime = tEventData[0];
						iEventType = BACKCLUSTEREDSICK_EVENT;
						ine = (int) (tEventData[2]);
						inc = LM_CLUSTEREDSICKPACKETSIZE;

						iEventData = new double[inc*ine];
						for (j = 0; j < LM_CLUSTEREDSICKPACKETSIZE; j++)
						{
							//copy the first line of the packet into iEventData
							iEventData[midx(0, j, ine)] = tEventData[j];
						}

						for (i = 1; i < ine; i++)
						{
							//read the rest of the packet line by line
							SensorLogs->BackClusteredSickLog.GetNextLogLine();
							retcode = StringToArrayOfDoubles(tEventData, LM_CLUSTEREDSICKPACKETSIZE, SensorLogs->BackClusteredSickLog.LogBuffer());

							if (retcode != LM_CLUSTEREDSICKPACKETSIZE)
							{
								//improper packet read: throw out this packet
								printf("Malformed back clustered sick log packet\n");
								iEventTime = DBL_MAX;
								iEventType = INVALID_EVENT;
								ine = 0;
								inc = 0;
								delete [] iEventData;
								iEventData = NULL;

								break;
							}

							for (j = 0; j < inc; j++)
							{
								//copy the next line of the packet into iEventData
								iEventData[midx(i, j, ine)] = tEventData[j];
							}
						}
					}

					//done reading the event; read the next line
					SensorLogs->BackClusteredSickLog.GetNextLogLine();
				}

				break;

			case 9:
			case 10:
			case 11:
			case 12:
			case 13:
			case 14:
			case 15:
			case 16:
			case 17:

				switch(enext)
				{
					case 9:
						iEventType = FRONT0RADAR_EVENT;
						TheReaderLogFile = &(SensorLogs->Front0RadarLog);
						break;
					case 10:
						iEventType = DRIV0RADAR_EVENT;
						TheReaderLogFile = &(SensorLogs->Driv0RadarLog);
						break;
					case 11:
						iEventType = DRIV1RADAR_EVENT;
						TheReaderLogFile = &(SensorLogs->Driv1RadarLog);
						break;
					case 12:
						iEventType = DRIV2RADAR_EVENT;
						TheReaderLogFile = &(SensorLogs->Driv2RadarLog);
						break;
					case 13:
						iEventType = DRIV3RADAR_EVENT;
						TheReaderLogFile = &(SensorLogs->Driv3RadarLog);
						break;
					case 14:
						iEventType = PASS0RADAR_EVENT;
						TheReaderLogFile = &(SensorLogs->Pass0RadarLog);
						break;
					case 15:
						iEventType = PASS1RADAR_EVENT;
						TheReaderLogFile = &(SensorLogs->Pass1RadarLog);
						break;
					case 16:
						iEventType = PASS2RADAR_EVENT;
						TheReaderLogFile = &(SensorLogs->Pass2RadarLog);
						break;
					case 17:
						iEventType = PASS3RADAR_EVENT;
						TheReaderLogFile = &(SensorLogs->Pass3RadarLog);
						break;
				}

				{
					//radar packet comes next

					//parse out the radar event information
					double tEventData[LM_DELPHIPACKETSIZE];
					retcode = StringToArrayOfDoubles(tEventData, LM_DELPHIPACKETSIZE, TheReaderLogFile->LogBuffer());

					if (retcode != LM_DELPHIPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed radar log packet\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully

						//extract the event time
						iEventTime = tEventData[0];
						//NOTE: iEventType is already set in the cascading cases
						ine = (int) (tEventData[2]);
						inc = LM_DELPHIPACKETSIZE;

						iEventData = new double[inc*ine];
						for (j = 0; j < LM_DELPHIPACKETSIZE; j++)
						{
							//copy the first line of the packet into iEventData
							iEventData[midx(0, j, ine)] = tEventData[j];
						}

						for (i = 1; i < ine; i++)
						{
							//read the rest of the packet line by line
							TheReaderLogFile->GetNextLogLine();
							retcode = StringToArrayOfDoubles(tEventData, LM_DELPHIPACKETSIZE, TheReaderLogFile->LogBuffer());

							if (retcode != LM_DELPHIPACKETSIZE)
							{
								//improper packet read: throw out this packet
								printf("Malformed radar log packet\n");
								iEventTime = DBL_MAX;
								iEventType = INVALID_EVENT;
								ine = 0;
								inc = 0;
								delete [] iEventData;
								iEventData = NULL;

								break;
							}

							for (j = 0; j < inc; j++)
							{
								//copy the next line of the packet into iEventData
								iEventData[midx(i, j, ine)] = tEventData[j];
							}
						}
					}

					//done reading the event; read the next line
					TheReaderLogFile->GetNextLogLine();
				}

				break;

			case 18:
			case 19:
				{
					//driv / pass side sick event came next

					switch(enext)
					{
						case 18:
							iEventType = SIDESICKDRIV_EVENT;
							TheReaderLogFile = &(SensorLogs->DrivSideSickLog);
							break;
						case 19:
							iEventType = SIDESICKPASS_EVENT;
							TheReaderLogFile = &(SensorLogs->PassSideSickLog);
							break;
					}

					//parse out the side sick event information
					double tEventData[LM_SIDESICKPACKETSIZE];
					retcode = StringToArrayOfDoubles(tEventData, LM_SIDESICKPACKETSIZE, TheReaderLogFile->LogBuffer());

					if (retcode != LM_SIDESICKPACKETSIZE)
					{
						//improper packet read: throw out this packet
						printf("Malformed side sick log packet\n");
						iEventTime = DBL_MAX;
						iEventType = INVALID_EVENT;
						ine = 0;
						inc = 0;
						iEventData = NULL;
					}
					else
					{
						//if code gets here, iEventData was created successfully

						//extract the event time
						iEventTime = tEventData[0];
						//NOTE: iEventType is already set in the cascading cases
						ine = (int) (tEventData[2]);
						inc = LM_SIDESICKPACKETSIZE;

						iEventData = new double[inc*ine];
						for (j = 0; j < LM_SIDESICKPACKETSIZE; j++)
						{
							//copy the first line of the packet into iEventData
							iEventData[midx(0, j, ine)] = tEventData[j];
						}

						for (i = 1; i < ine; i++)
						{
							//read the rest of the packet line by line
							TheReaderLogFile->GetNextLogLine();
							retcode = StringToArrayOfDoubles(tEventData, LM_SIDESICKPACKETSIZE, TheReaderLogFile->LogBuffer());

							if (retcode != LM_SIDESICKPACKETSIZE)
							{
								//improper packet read: throw out this packet
								printf("Malformed side sick packet\n");
								iEventTime = DBL_MAX;
								iEventType = INVALID_EVENT;
								ine = 0;
								inc = 0;
								delete [] iEventData;
								iEventData = NULL;

								break;
							}

							for (j = 0; j < inc; j++)
							{
								//copy the next line of the packet into iEventData
								iEventData[midx(i, j, ine)] = tEventData[j];
							}
						}
					}

					//done reading the event; read the next line
					TheReaderLogFile->GetNextLogLine();
				}

				break;

			default:
				{
					//invalid event
					iEventType = INVALID_EVENT;
					iEventTime = DBL_MAX;
					ine = 0;
					inc = 0;
					iEventData = NULL;
				}

				break;
		}

		//when no data is left, "disconnect" the sensor
		if (etime == DBL_MAX)
		{
			printf("Log sensors terminating...\n");
			iEventType = INVALID_EVENT;
			iEventTime = DBL_MAX;
			ine = 0;
			inc = 0;
			iEventData = NULL;
			SensorLogs->StopRunning();
		}

		if (iEventType == INVALID_EVENT || SensorLogs->IsRunning() == false)
		{
			//for invalid reads or terminating logfiles, go to sleep
			//note: memory has already been deallocated for bad reads

			//sleep for the delay time of the sensor
			Sleep(SensorLogs->SamplePeriod());
			continue;
		}

		//when the code gets here, iEventData is populated with a complete sensor packet
		//create the event data
		NewEvent = new Event();
		NewEvent->SetEventType(iEventType, iEventTime);
		NewEvent->SetEventData(ine, inc, iEventData);
		#ifdef LM_PRINTLOGS
			//create a separate logging event with the same data
			LogEvent = new Event(NewEvent);
		#endif

		bool spush = false;

		switch (NewEvent->EventType())
		{
		case IBEO_EVENT:
		case BACKCLUSTEREDSICK_EVENT:
		case FRONTMOBILEYEOBSTACLE_EVENT:
		case BACKMOBILEYEOBSTACLE_EVENT:
		case DRIV3RADAR_EVENT:
		case DRIV2RADAR_EVENT:
		case DRIV1RADAR_EVENT:
		case DRIV0RADAR_EVENT:
		case FRONT0RADAR_EVENT:
		case PASS0RADAR_EVENT:
		case PASS1RADAR_EVENT:
		case PASS2RADAR_EVENT:
		case PASS3RADAR_EVENT:
		case SIDESICKDRIV_EVENT:
		case SIDESICKPASS_EVENT:
			//obstacle packets go to LocalMap

			spush = TheLocalMapEventQueue->PushEvent(NewEvent);

			break;

		case FRONTJASONROAD_EVENT:
		case BACKJASONROAD_EVENT:
		case FRONTMOBILEYEROAD_EVENT:
		case BACKMOBILEYEROAD_EVENT:
			//local road packets go to LocalRoad
			spush = TheLocalRoadEventQueue->PushEvent(NewEvent);

			break;

		case ODOM_EVENT:
		case QUIT_EVENT:
		case TRANSMIT_EVENT:
			//message events go to both queues

			NewEvent2 = new Event(NewEvent);

			spush = TheLocalMapEventQueue->PushEvent(NewEvent);
			TheLocalRoadEventQueue->PushEvent(NewEvent2);

			break;
		}

		#ifdef LM_PRINTLOGS
			//only log the packet if it successfully entered the event queue
			if (spush == true)
			{
				TheLoggingQueue->PushEvent(LogEvent);
			}
			else
			{
				delete LogEvent;
			}
		#endif

		//sleep for the delay time of the sensor
		Sleep(SensorLogs->SamplePeriod());
	}

	return 0;
}
void TargetOccupancyGrid::SetOccupancyGrid(int iNumPoints, double* iPoints, 
	double iAnchorX, double iAnchorY, double iOrientation)
{
	/*
	Sets the occupancy grid for a set of target points.

	INPUTS:
		iNumPoints - number of cluster points for the target
		iPoints - the actual target points, in object storage frame
		iAnchorX, iAnchorY - the target's anchor point, in ego-vehicle
			coordinates
		iOrientation - the target's orientation angle

	OUTPUTS:
		none.
	*/

	int i;

	mIsValid = false;
	ResetOccupancyGrid();

	if (iNumPoints < 1)
	{
		//can't create an occupancy grid without points
		return;
	}

	double cosOrient = cos(iOrientation);
	double sinOrient = sin(iOrientation);

	//store the points in ego-vehicle coordinates
	mNumPoints = iNumPoints;
	mOccupancyPoints.resize(iNumPoints);
	double wt = 1.0 / ((double) iNumPoints);

	//these will be built up as the points are transformed to ego-vehicle
	mCenterX = 0.0;
	mCenterY = 0.0;
	double mMinX = DBL_MAX;
	double mMaxX = -DBL_MAX;
	double mMinY = DBL_MAX;
	double mMaxY = -DBL_MAX;

	//convert all points to ego-vehicle coordinates
	for (i = 0; i < iNumPoints; i++)
	{
		//extract each target point in object storage frme
		double osx = iPoints[midx(i, 0, iNumPoints)];
		double osy = iPoints[midx(i, 1, iNumPoints)];
		//convert to ego vehicle coordinates
		double evx;
		double evy;
		ObjectToEgoVehicle(evx, evy, osx, osy, cosOrient, sinOrient, iAnchorX, iAnchorY);

		//build up the center of mass as the grid center
		mCenterX += wt*evx;
		mCenterY += wt*evy;

		//keep track of the bounding box
		if (evx < mMinX)
		{
			mMinX = evx;
		}
		if (evx > mMaxX)
		{
			mMaxX = evx;
		}
		if (evy < mMinY)
		{
			mMinY = evy;
		}
		if (evy > mMaxY)
		{
			mMaxY = evy;
		}

		//store the ego-vehicle transformations for later
		mOccupancyPoints[i] = make_pair(evx, evy);
	}

	//calculate the span of the occupancy grid
	mSpanX = fabs(mMaxX - mMinX) + 2.0*TOG_PADSIZE;
	mSpanY = fabs(mMaxY - mMinY) + 2.0*TOG_PADSIZE;
	//calculate the cell lengths of the occupancy grid
	mCellLengthX = mSpanX / ((double) mNumCols);
	mCellLengthY = mSpanY / ((double) mNumRows);

	//populate the occupancy grid
	int nc = mNumRows*mNumCols;
	for (i = 0; i < iNumPoints; i++)
	{
		//pull each point
		DoublePair curpt = mOccupancyPoints[i];
		//and add it to the occupancy grid
		int r;
		int c;
		OccupancyGridCellRowColumn(r, c, curpt.first, curpt.second);
		//mark this cell as occupied
		if (r >= 0 && r < mNumRows && c >= 0 && c < mNumCols)
		{
			mOccupancyGrid[OccupancyGridCellIndex(r, c)] = TOG_OCCUPIED;
		}
	}

	//when done populating the grid, it is valid again
	mIsValid = true;

	return;
}
void LocalRoadCallback(LocalRoadModelEstimateMsg LRMessage, LocalMapInterfaceReceiver* LMRX, void* Arg)
{
	/*
	Callback for handling the local road model coming from LocalMap

	INPUTS:
		LRMessage - the actual local road message data
		LMRX - the local map interface receiver
		Arg - a void pointer (not used)

	OUTPUTS:
		none.  Creates an event and places it on the event queue
	*/

	//gain access to the event queue
	extern SynchronizedEventQueue* ThePosteriorPoseEventQueue;

	double iEventTime = LRMessage.timestamp;

	//check the event time for valid timestamp
	extern CarTime* TheCarTime;
	double cct = TheCarTime->CurrentCarTime();
	if (fabs(iEventTime - cct) > CT_TIMEJUMPREJECT)
	{
		//do not allow events with inconsistent timestamps into the event queues
		printf("Warning: got localroad timestamp %.12lg; expected something near %.12lg\n", iEventTime, cct);
		return;
	}

	int iNumDataRows = 1;
	int iNumDataCols = SE_LRHEADERPACKETSIZE;

	//construct the header packet
	double* iEventData = new double[SE_LRHEADERPACKETSIZE];
	iEventData[0] = LRMessage.timestamp;
	iEventData[1] = LRMessage.probabilityRoadModelValid;
	iEventData[2] = LRMessage.probabilityLeftLaneExists;
	iEventData[3] = LRMessage.laneWidthLeft;
	iEventData[4] = LRMessage.laneWidthLeftVariance;
	iEventData[5] = LRMessage.probabilityCenterLaneExists;
	iEventData[6] = LRMessage.laneWidthCenter;
	iEventData[7] = LRMessage.laneWidthCenterVariance;
	iEventData[8] = LRMessage.probabilityRightLaneExists;
	iEventData[9] = LRMessage.laneWidthRight;
	iEventData[10] = LRMessage.laneWidthRightVariance;
	iEventData[11] = LRMessage.numPointsLeft;
	iEventData[12] = LRMessage.numPointsCenter;
	iEventData[13] = LRMessage.numPointsRight;

	//construct the points packet
	int npL = LRMessage.numPointsLeft;
	int npC = LRMessage.numPointsCenter;
	int npR = LRMessage.numPointsRight;
	int np = npL + npC + npR;
	int iNumDataRows2 = np;
	int iNumDataCols2 = SE_LRPOINTSPACKETSIZE;
	double* iEventData2 = new double[np*SE_LRPOINTSPACKETSIZE];

	int i;
	int idx = 0;

	for (i = 0; i < npL; i++)
	{
		//set each left lane point in the points packet
		iEventData2[midx(idx, 0, np)] = iEventTime;
		iEventData2[midx(idx, 1, np)] = (double) i;
		iEventData2[midx(idx, 2, np)] = (double) npL;
		iEventData2[midx(idx, 3, np)] = 0.0;
		iEventData2[midx(idx, 4, np)] = LRMessage.LanePointsLeft[i].GetX();
		iEventData2[midx(idx, 5, np)] = LRMessage.LanePointsLeft[i].GetY();
		iEventData2[midx(idx, 6, np)] = LRMessage.LanePointsLeft[i].GetVariance();
		idx++;
	}
	for (i = 0; i < npC; i++)
	{
		//set each center lane point in the points packet
		iEventData2[midx(idx, 0, np)] = iEventTime;
		iEventData2[midx(idx, 1, np)] = (double) i;
		iEventData2[midx(idx, 2, np)] = (double) npC;
		iEventData2[midx(idx, 3, np)] = 1.0;
		iEventData2[midx(idx, 4, np)] = LRMessage.LanePointsCenter[i].GetX();
		iEventData2[midx(idx, 5, np)] = LRMessage.LanePointsCenter[i].GetY();
		iEventData2[midx(idx, 6, np)] = LRMessage.LanePointsCenter[i].GetVariance();
		idx++;
	}
	for (i = 0; i < npR; i++)
	{
		//set each center lane point in the points packet
		iEventData2[midx(idx, 0, np)] = iEventTime;
		iEventData2[midx(idx, 1, np)] = (double) i;
		iEventData2[midx(idx, 2, np)] = (double) npC;
		iEventData2[midx(idx, 3, np)] = 2.0;
		iEventData2[midx(idx, 4, np)] = LRMessage.LanePointsRight[i].GetX();
		iEventData2[midx(idx, 5, np)] = LRMessage.LanePointsRight[i].GetY();
		iEventData2[midx(idx, 6, np)] = LRMessage.LanePointsRight[i].GetVariance();
		idx++;
	}

	//set the event data
	Event* NewEvent = new Event();
	NewEvent->SetEventType(LOCALMAPROAD_EVENT, iEventTime);
	NewEvent->SetEventData(iNumDataRows, iNumDataCols, iEventData);
	NewEvent->SetEventData2(iNumDataRows2, iNumDataCols2, iEventData2);

	//and push to the event queue
	#ifdef SE_PRINTLOGS
		//create a separate logging event with the same data
		Event* LogEvent = new Event(NewEvent);
	#endif

	bool spush = ThePosteriorPoseEventQueue->PushEvent(NewEvent);

	#ifdef SE_PRINTLOGS
		//only log the packet if it successfully entered the event queue
		extern EventQueue* TheLoggingQueue;
		if (spush == true)
		{
			TheLoggingQueue->PushEvent(LogEvent);
		}
		else
		{
			delete LogEvent;
		}
	#endif

	return;
}
void LocalMapTargetsCallback(LocalMapTargetsMsg LMMessage, LocalMapInterfaceReceiver* LMRX, void* Arg)
{
	/*
	Callback for handling the targets tracked by LocalMap

	INPUTS:
		LMMessage - the LocalMap message data containing the targets
		LMRX - the local map interface receiver
		Arg - a void pointer, not used here

	OUTPUTS:
		none.  Creates an event and places it on the event queue
	*/

	//gain access to the event queue
	extern SynchronizedEventQueue* TheTrackGeneratorEventQueue;

	int nt = (int) LMMessage.targets.size();

	double iEventTime = LMMessage.timestamp;

	//check the event time for valid timestamp
	extern CarTime* TheCarTime;
	double cct = TheCarTime->CurrentCarTime();
	if (fabs(iEventTime - cct) > CT_TIMEJUMPREJECT)
	{
		//do not allow events with inconsistent timestamps into the event queues
		printf("Warning: got localmap targets timestamp %.12lg; expected something near %.12lg\n", iEventTime, cct);
		return;
	}

	int i;
	int j;
	int k;
	int idx;

	//declare memory for the header packet and the covariance packet
	int iEventType = LOCALMAPTARGETS_EVENT;
	int iNumDataRows = nt;
	int iNumDataCols = SE_LMTHEADERPACKETSIZE;
	double* iEventData = NULL;
	if (nt > 0)
	{
		iEventData = new double[nt*SE_LMTHEADERPACKETSIZE];
	}
	int iNumDataRows2 = nt;
	int iNumDataCols2 = SE_LMTCOVARIANCEPACKETSIZE;
	double* iEventData2 = NULL;
	if (nt > 0)
	{
		iEventData2 = new double[nt*SE_LMTCOVARIANCEPACKETSIZE];
	}
	int np = 0;

	for (i = 0; i < nt; i++)
	{
		//set the packet headers and count the total number of target points
		LocalMapTarget CurrentTarget = LMMessage.targets[i];
		np += (int) CurrentTarget.points.size();

		//HEADER PACKET
		//event time
		iEventData[midx(i, 0, nt)] = iEventTime;
		//target index
		iEventData[midx(i, 1, nt)] = (double) i;
		//number of targets
		iEventData[midx(i, 2, nt)] = (double) nt;
		//target type
		switch (CurrentTarget.type)
		{
		case LM_TT_INVALID:
			iEventData[midx(i, 3, nt)] = -1.0;
			break;
		case LM_TT_IBEO:
			iEventData[midx(i, 3, nt)] = 101.0;
			break;
		case LM_TT_IBEOMATURE:
			iEventData[midx(i, 3, nt)] = 102.0;
			break;
		case LM_TT_MOBILEYE:
			iEventData[midx(i, 3, nt)] = 103.0;
			break;
		case LM_TT_RADAR:
			iEventData[midx(i, 3, nt)] = 104.0;
			break;
		case LM_TT_QUASIMATURE:
			iEventData[midx(i, 3, nt)] = 105.0;
			break;
		case LM_TT_MATURE:
			iEventData[midx(i, 3, nt)] = 106.0;
			break;
		default:
			iEventData[midx(i, 3, nt)] = -1.0;
		}
		//target X, Y, Orientation, Speed, Heading, Width
		iEventData[midx(i, 4, nt)] = CurrentTarget.x;
		iEventData[midx(i, 5, nt)] = CurrentTarget.y;
		iEventData[midx(i, 6, nt)] = CurrentTarget.orientation;
		iEventData[midx(i, 7, nt)] = CurrentTarget.speed;
		iEventData[midx(i, 8, nt)] = CurrentTarget.heading;
		iEventData[midx(i, 9, nt)] = CurrentTarget.width;

		//number of points
		iEventData[midx(i, 10, nt)] = (double) CurrentTarget.points.size();

		//COVARIANCE PACKET
		//event time
		iEventData2[midx(i, 0, nt)] = iEventTime;
		//target index
		iEventData2[midx(i, 1, nt)] = (double) i;
		//number of targets
		iEventData2[midx(i, 2, nt)] = (double) nt;
		//lower triangle of covariance matrix, in column major order
		idx = 3;
		for (j = 0; j < 6; j++)
		{
			for (k = 0; k <= j; k++)
			{
				iEventData2[midx(i, idx, nt)] = CurrentTarget.covariance[midx(j, k, 6)];
				idx++;
			}
		}
	}

	//declare memory for target points data
	int iNumDataRows3 = np;
	int iNumDataCols3 = SE_LMTPOINTSPACKETSIZE;
	double* iEventData3 = NULL;
	if (np > 0)
	{
		iEventData3 = new double[np*SE_LMTPOINTSPACKETSIZE];
	}
	idx = 0;
	int npt;

	for (i = 0; i < nt; i++)
	{
		//set the points packet for each target

		LocalMapTarget CurrentTarget = LMMessage.targets[i];
		npt = (int) CurrentTarget.points.size();
		for (j = 0; j < npt; j++)
		{
			//event time
			iEventData3[midx(idx, 0, np)] = iEventTime;
			iEventData3[midx(idx, 1, np)] = (double) i;
			iEventData3[midx(idx, 2, np)] = (double) nt;
			iEventData3[midx(idx, 3, np)] = (double) j;
			iEventData3[midx(idx, 4, np)] = (double) npt;
			LocalMapPoint CurrentPoint = CurrentTarget.points[j];
			iEventData3[midx(idx, 5, np)] = CurrentPoint.GetX();
			iEventData3[midx(idx, 6, np)] = CurrentPoint.GetY();
			idx++;
		}
	}

	//create the event and push it to the track generator
	Event* NewEvent = new Event();
	NewEvent->SetEventType(iEventType, iEventTime);
	NewEvent->SetEventData(iNumDataRows, iNumDataCols, iEventData);
	NewEvent->SetEventData2(iNumDataRows2, iNumDataCols2, iEventData2);
	NewEvent->SetEventData3(iNumDataRows3, iNumDataCols3, iEventData3);
	#ifdef SE_PRINTLOGS
		//create a separate logging event with the same data
		Event* LogEvent = new Event(NewEvent);
	#endif

	bool spush = TheTrackGeneratorEventQueue->PushEvent(NewEvent);

	#ifdef SE_PRINTLOGS
		//only log the packet if it successfully entered the event queue
		extern EventQueue* TheLoggingQueue;
		if (spush == true)
		{
			TheLoggingQueue->PushEvent(LogEvent);
		}
		else
		{
			delete LogEvent;
		}
	#endif

	return;
}
void LocalMapLooseClustersCallback(LocalMapLooseClustersMsg LMMessage, LocalMapInterfaceReceiver* LMRX, void* Arg)
{
	/*
	Callback for the localmap loose clusters message

	INPUTS:
		LMMessage - the LocalMap message packet
		LMRX - pointer to the localmap receiver
		Arg - a void pointer (not used)

	OUTPUTS:
		none.  Drops an event onto the event queue.
	*/

	//gain access to the event queue
	extern SynchronizedEventQueue* TheTrackGeneratorEventQueue;

	double iEventTime = LMMessage.timestamp;

	//check the event time for valid timestamp
	extern CarTime* TheCarTime;
	double cct = TheCarTime->CurrentCarTime();
	if (fabs(iEventTime - cct) > CT_TIMEJUMPREJECT)
	{
		//do not allow events with inconsistent timestamps into the event queues
		printf("Warning: got localmap loose clusters timestamp %.12lg; expected something near %.12lg\n", iEventTime, cct);
		return;
	}

	int nc = (int) LMMessage.clusters.size();

	int i;
	int j;
	int np = 0;
	for (i = 0; i < nc; i++)
	{
		//count the total number of obstacle points across all clusters
		LocalMapLooseCluster cc = LMMessage.clusters[i];
		np += (int) cc.points.size();
	}

	//declare memory for the packet and assign values
	int iEventType = LOCALMAPPOINTS_EVENT;
	int iNumDataRows = np;
	int iNumDataCols = SE_LOCALPOINTSPACKETSIZE;
	double* iEventData = NULL;
	if (np > 0)
	{
		iEventData = new double[np*SE_LOCALPOINTSPACKETSIZE];
	}

	int idx = 0;
	for (i = 0; i < nc; i++)
	{
		//pull all clusters and copy into an event packet
		LocalMapLooseCluster cc = LMMessage.clusters[i];
		int npc = (int) cc.points.size();

		for (j = 0; j < npc; j++)
		{
			//pull each point from the cluster
			LocalMapPoint cp = cc.points[j];

			//set the event data
			iEventData[midx(idx, 0, np)] = iEventTime;
			iEventData[midx(idx, 1, np)] = (double) idx;
			iEventData[midx(idx, 2, np)] = (double) np;
			iEventData[midx(idx, 3, np)] = (double) i;
			switch (cc.clusterClass)
			{
			case LOCALMAP_HighObstacle:
				iEventData[midx(idx, 4, np)] = 1.0;
				break;
			case LOCALMAP_LowObstacle:
				iEventData[midx(idx, 4, np)] = 0.0;
				break;
			}
			iEventData[midx(idx, 5, np)] = cp.GetX();
			iEventData[midx(idx, 6, np)] = cp.GetY();

			//done setting this point
			idx++;
		}
	}

	//create the event and push it to the track generator
	Event* NewEvent = new Event();
	NewEvent->SetEventType(iEventType, iEventTime);
	NewEvent->SetEventData(iNumDataRows, iNumDataCols, iEventData);
	#ifdef SE_PRINTLOGS
		//create a separate logging event with the same data
		Event* LogEvent = new Event(NewEvent);
	#endif

	bool spush = TheTrackGeneratorEventQueue->PushEvent(NewEvent);

	#ifdef SE_PRINTLOGS
		//only log the packet if it successfully entered the event queue
		extern EventQueue* TheLoggingQueue;
		if (spush == true)
		{
			TheLoggingQueue->PushEvent(LogEvent);
		}
		else
		{
			delete LogEvent;
		}
	#endif

	return;
}
void JasonCallback(RoadFitterOutput JasonMessage, RoadFitterInterfaceReceiver* JRX, RoadFitterID ID, void* Arg)
{
	/*
	Called when jason's algorithm returns a road fix

	INPUTS:
		JasonMessage - the results stored in Jason's data packet
		JRX - pointer to the interface receiver (not used here)
		ID - enumerated ID indicating which camera was used to create the message
		Arg - null pointer (not used here)

	OUTPUTS:
		none.  Reformats the packet into an event and drops it onto ThePosteriorPoseEventQueue
	*/

	int i;
	int j;

	//extract the number of jason's road segments (always RF_MAX_FITS)
	int nj = RF_VALID_FITS;

	//gain access to the event queue
	extern SynchronizedEventQueue* ThePosteriorPoseEventQueue;

	//set timestamp from car time
	double iEventTime = JasonMessage.carTime;

	//check the event time for valid timestamp
	extern CarTime* TheCarTime;
	double cct = TheCarTime->CurrentCarTime();
	if (fabs(iEventTime - cct) > CT_TIMEJUMPREJECT)
	{
		//do not allow events with inconsistent timestamps into the event queues
		printf("Warning: got roadfinder timestamp %.12lg; expected something near %.12lg\n", iEventTime, cct);
		return;
	}

	//declare space for the event data
	//NOTE: this memory is cleared automatically when the event is pulled from the queue
	Event* NewEvent = new Event();
	int iEventType;
	double* iEventData = new double[nj*SE_JASONROADPACKETSIZE];

	for (i = 0; i < nj; i++)
	{
		for (j = 0; j < SE_JASONROADPACKETSIZE; j++)
		{
			iEventData[midx(i, j, nj)] = 0.0;
		}
	}

	for (i = 0; i < nj; i++)
	{
		//timestamp (synchronized vehicle time)
		iEventData[midx(i, 0, nj)] = iEventTime;
		//index of the segmentation and number of segmentations
		iEventData[midx(i, 1, nj)] = (double) (i);
		iEventData[midx(i, 2, nj)] = (double) (nj);
		//number of lines / boundaries found (0 - 4)
		int nb = JasonMessage.roadFits[i].borders_observed;
		iEventData[midx(i, 3, nj)] = (double) nb;
		for (j = 0; j < nb; j++)
		{
			//distance to boundaries found (m from camera, positive for lines to the left of center)
			iEventData[midx(i, j+4, nj)] = JasonMessage.roadFits[i].border_offsets[j];
			//boundary types
			switch (JasonMessage.roadFits[i].observed_border_types[j])
			{
			case RoadFitterOutputData::BT_SingleLaneLine:
				iEventData[midx(i, j+8, nj)] = 1.0;
				break;
			case RoadFitterOutputData::BT_Edge:
				iEventData[midx(i, j+8, nj)] = 0.0;
				break;
			default:
				iEventData[midx(i, j+8, nj)] = 0.0;
				break;
			}
		}
		//heading of road wrt camera (m/m, positive for roads heading right)
		iEventData[midx(i, 12, nj)] = -JasonMessage.roadFits[i].road_heading;
		//curvature of road wrt camera (m/m^2, positive for roads curving right)
		iEventData[midx(i, 13, nj)] = -JasonMessage.roadFits[i].system_curvature;
		//confidence in segmentation fit
		iEventData[midx(i, 14, nj)] = JasonMessage.roadFits[i].confidence;
	}

	switch (ID)
	{
	case RF_CTR:
		iEventType = FRONTJASONROAD_EVENT;
		break;

	default:
		iEventType = INVALID_EVENT;
		break;
	}

	//store the event data in the event container
	NewEvent->SetEventType(iEventType, iEventTime);
	NewEvent->SetEventData(nj, SE_JASONROADPACKETSIZE, iEventData);
	#ifdef SE_PRINTLOGS
		//create a separate logging event with the same data
		Event* LogEvent = new Event(NewEvent);
	#endif

	//drop the event onto the event queue
	bool spush = ThePosteriorPoseEventQueue->PushEvent(NewEvent);
	#ifdef SE_PRINTLOGS
		//only log the packet if it successfully entered the event queue
		extern EventQueue* TheLoggingQueue;
		if (spush == true)
		{
			TheLoggingQueue->PushEvent(LogEvent);
		}
		else
		{
			delete LogEvent;
		}
	#endif

	return;
}
void StoplineCallback(StopLineMessage SLMessage, StopLineInterfaceReceiver* SRX, void* Arg)
{
	/*
	Sensor callback for receiving stopline information from Aaron's stopline algorithm

	INPUTS:
		SLMessage - class containing the stopline packet data
		SRX - stopline interface receiver (not used here)
		Arg - void pointer (not used here)

	OUTPUTS:
		none.  Creates an event for the stopline information and pushes the 
		event onto ThePosteriorPoseEventQueue
	*/

	int i;

	//gain access to the event queue
	extern SynchronizedEventQueue* ThePosteriorPoseEventQueue;

	//set timestamp from car time
	double iEventTime = SLMessage.carTime;

	//check the event time for valid timestamp
	extern CarTime* TheCarTime;
	double cct = TheCarTime->CurrentCarTime();
	if (fabs(iEventTime - cct) > CT_TIMEJUMPREJECT)
	{
		//do not allow events with inconsistent timestamps into the event queues
		printf("Warning: got stopline timestamp %.12lg; expected something near %.12lg\n", iEventTime, cct);
		return;
	}

	//extract the number of stoplines
	int ns = SLMessage.numStopLines;

	//declare space for the event data
	//NOTE: this memory is cleared automatically when the event is pulled from the queue
	Event* NewEvent = new Event();
	double* iEventData = NULL;
	if (ns > 0)
	{
		iEventData = new double[ns*SE_STOPLINEPACKETSIZE];
	}

	for (i = 0; i < ns; i++)
	{
		//timestamp (synchronized vehicle time)
		iEventData[midx(i, 0, ns)] = iEventTime;
		//index of the stopline and number of stoplines
		iEventData[midx(i, 1, ns)] = (double) (i);
		iEventData[midx(i, 2, ns)] = (double) (ns);
		//distance to stopline
		iEventData[midx(i, 3, ns)] = SLMessage.stopLines[i].distance;
		//confidence in stopline
		iEventData[midx(i, 4, ns)] = SLMessage.stopLines[i].confidence;
	}

	//store the event data in the event container
	NewEvent->SetEventType(STOPLINE_EVENT, iEventTime);
	NewEvent->SetEventData(ns, SE_STOPLINEPACKETSIZE, iEventData);
	#ifdef SE_PRINTLOGS
		//create a separate logging event with the same data
		Event* LogEvent = new Event(NewEvent);
	#endif

	//drop the event onto the event queue
	bool spush = ThePosteriorPoseEventQueue->PushEvent(NewEvent);
	#ifdef SE_PRINTLOGS
		//only log the packet if it successfully entered the event queue
		extern EventQueue* TheLoggingQueue;
		if (spush == true)
		{
			TheLoggingQueue->PushEvent(LogEvent);
		}
		else
		{
			delete LogEvent;
		}
	#endif

	return;
}