public Point_Obj get_gaze_pt(Point_Obj right_eye, Point_Obj left_eye, float alpha, float beta, float gamma)
{
// Compute Z coordinate of head/eyes in the WCS (World Coordinate System)
float Z = (-f * EYE_DIST) / (left_eye.get_x() - right_eye.get_x());
//Console.WriteLine("EST Z: {0}", Z);
// Use computed Z coordinate to get X,Y world coordinates of the eyes
float X_r = Z * (right_eye.get_x() - princ_pt.get_x()) / (-f);
float Y_r = Z * (right_eye.get_y() - princ_pt.get_y()) / (-f);
float X_l = Z * (left_eye.get_x() - princ_pt.get_x()) / (-f);
float Y_l = Z * (left_eye.get_y() - princ_pt.get_y()) / (-f);
// Compute direction vector (d) of eyes using rotation angles (alpha, beta, gamma)
DenseVector k_hat = new DenseVector(3);
k_hat[2] = 1;
//DenseMatrix R = get_rotation_mat(alpha, beta, gamma);
DenseMatrix R = get_rotation_mat(alpha, beta, gamma);
DenseVector d = R * k_hat;
// Get point of intersection using eye points and direction vector d
DenseVector P_r = new DenseVector(new[]{Convert.ToDouble(X_r), Convert.ToDouble(Y_r), Convert.ToDouble(Z)});
DenseVector P_l = new DenseVector(new[] { Convert.ToDouble(X_l), Convert.ToDouble(Y_l), Convert.ToDouble(Z)});
DenseVector p_hat_r = P_r + d * (-P_r[2] / d[2]);
DenseVector p_hat_l = P_l + d * (-P_l[2] / d[2]);
DenseVector p_hat_avg = (p_hat_r + p_hat_l) / 2;
/*
Console.WriteLine("EYE_DIST (pix): {0}", (left_eye.get_x() - right_eye.get_x()).ToString());
Console.WriteLine("P_r: {0}", P_r.ToString());
Console.WriteLine("P_l: {0}", P_l.ToString());
Console.WriteLine("d: {0}", d);
Console.WriteLine("p_hat_r: {0}", p_hat_r);
Console.WriteLine("p_hat_l: {0}", p_hat_l);
Console.WriteLine("p_hat_avg: {0}", p_hat_avg);
*/
return new Point_Obj(Convert.ToSingle(p_hat_avg[0]), Convert.ToSingle(p_hat_avg[1]));
}
private void write_pt_to_hist(Point_Obj p, Screen_Prob_Map p_m, Subj_Data S, int frame_num)
{
int ID = S.get_ID();
if (((Chart)this.hist_chart_list[ID]).InvokeRequired)
{
ChartHistCallback cH = new ChartHistCallback(write_pt_to_hist);
this.Invoke(cH, new object[] { p, p_m, S, frame_num });
}
else
{
Chart chart = (Chart)this.hist_chart_list[ID];
chart.ChartAreas[0].AxisX.Maximum = frame_num; // display points up to current frame
Chart chart_f2 = (Chart)F2.hist_chart_list[ID];
int MONITOR_DIM_X = S.get_MONITOR_DIM_X();
int MONITOR_DIM_Y = S.get_MONITOR_DIM_Y();
// convert gaze point (world coordinates) => monitor coordinates
p.set_coord(-p.get_x(), p.get_y() + MONITOR_DIM_Y / 2, p.get_z());
if (p.get_x() < -MONITOR_DIM_X / 2 || p.get_x() > MONITOR_DIM_X / 2)
{
chart.Series["Series1"].Points.AddXY(frame_num, 0.0F);
Console.WriteLine("{0} - Confid Score: {1}", ID, 0.0F);
chart_f2.Series[0].Points.AddXY(DateTime.Now.ToLongTimeString(), 0.0F);
}
else if (p.get_y() < -MONITOR_DIM_Y / 2 || p.get_y() > MONITOR_DIM_Y / 2)
{
chart.Series["Series1"].Points.AddXY(frame_num, 0.0F);
Console.WriteLine("{0} - Confid Score: {1}", ID, 0.0F);
chart_f2.Series[0].Points.AddXY(DateTime.Now.ToLongTimeString(), 0.0F);
}
else
{
//chart.Series["Series1"].Points.AddXY(frame_num, 0);
//double D = p_m.look_up_value(Convert.ToInt32(-p.get_x()), Convert.ToInt32(p.get_y()) + MONITOR_DIM_Y / 2);
double D = p_m.look_up_value(Convert.ToInt32(p.get_x()), Convert.ToInt32(p.get_y()));
Console.WriteLine("{0} - Confid Score: {1}", ID, D);
//Console.WriteLine("Adding Point "+D.ToString());
chart.Series["Series1"].Points.AddXY(frame_num, Convert.ToSingle(D));
//write_to_msg_box(frame_num.ToString() + " " + D.ToString());
chart_f2.Series[0].Points.AddXY(DateTime.Now.ToLongTimeString(), Convert.ToSingle(D));
}
if (frame_num > 100)
{
chart.ChartAreas[0].AxisX.Minimum = chart.ChartAreas[0].AxisX.Maximum - 100;
}
}
}
private void plot_gaze_point(Point_Obj p, Subj_Data S)
{
if (this.gaze_plot.InvokeRequired)
{
//ChartCallback c = new ChartCallback(plot_point);
GazeChartCallback gC = new GazeChartCallback(plot_gaze_point);
this.Invoke(gC, new object[] { p, S });
}
else
{
this.gaze_plot.Series["Series1"].Points.AddXY(p.get_x(), p.get_y());
int L = this.gaze_plot.Series["Series1"].Points.Count;
int MONITOR_DIM_X = S.get_MONITOR_DIM_X();
int MONITOR_DIM_Y = S.get_MONITOR_DIM_Y();
int ID = S.get_ID();
if (p.get_x() < -MONITOR_DIM_X / 2 || p.get_x() > MONITOR_DIM_X / 2)
{
//Console.WriteLine("POINT RED!");
this.gaze_plot.Series["Series1"].Points[L - 1].Color = Color.Red;
}
else if (p.get_y() < -MONITOR_DIM_Y / 2 || p.get_y() > MONITOR_DIM_Y / 2)
{
//Console.WriteLine("POINT RED!");
this.gaze_plot.Series["Series1"].Points[L - 1].Color = Color.Red;
}
else
{
//Console.WriteLine("POINT BLUE!");
this.gaze_plot.Series["Series1"].Points[L - 1].Color = color_list[ID];
}
if(L > POINTS_TO_SHOW)
{
this.gaze_plot.Series["Series1"].Points.RemoveAt(0);
}
}
}
private Point_Obj process_float_data(float[] input_array, Subj_Data S)
{
float frame_num = input_array[0];
float success_flag = input_array[1];
if (success_flag == 1.0)
{
Console.WriteLine("{0}: Gaze Pt Package", Convert.ToInt32(frame_num));
Point_Obj left_eye = new Point_Obj(input_array[2], input_array[3], input_array[4]);
Point_Obj right_eye = new Point_Obj(input_array[5], input_array[6], input_array[7]);
float alpha = input_array[8] * 180;
float beta = input_array[9] * 180;
float gamma = input_array[10] * 180;
int ID = S.get_ID();
Gaze_Comp G = new Gaze_Comp();
Point_Obj p = G.get_gaze_pt(right_eye, left_eye, alpha, beta, gamma);
Console.WriteLine("{0} - GAZE POINT (X, Y): ({1}, {2})", ID, p.get_x(), p.get_y());
Point_Obj disp_pt = new Point_Obj(-p.get_x(), p.get_y() + S.get_MONITOR_DIM_Y() / 2);
plot_gaze_point(disp_pt, S);
Console.WriteLine("{0} - TRANSF GAZE POINT (X, Y): ({1}, {2})", ID, disp_pt.get_x(), disp_pt.get_y());
return p;
}
else if(success_flag == 2.0)
{
float age = input_array[2];
float gender = input_array[3];
Console.WriteLine("{0}: Age/Gender Package", Convert.ToInt32(frame_num));
S.set_age(Convert.ToDouble(age));
if (gender >= 0.5)
{
// Male
S.set_gender('M');
}
else
{
// Female
S.set_gender('F');
}
disp_age(S);
disp_gender(S);
if (S.get_pt_list().Count != 0)
{
return S.get_pt_list().Last().gaze_pt;
}
else
{
return new Point_Obj(0.0F, -S.get_MONITOR_DIM_Y()/2);
}
}
else
{
Console.WriteLine("{0}: Tracking Failure", Convert.ToInt32(frame_num));
//Point_Obj Gaze_Pt = new Point_Obj(Single.PositiveInfinity, Single.PositiveInfinity);
Point_Obj disp_pt_f = new Point_Obj(-1000F, -1000F);
plot_gaze_point(disp_pt_f, S);
//disp_pt_f.set_coord(Single.PositiveInfinity, Single.PositiveInfinity, 0);
return disp_pt_f;
}
}
private Subj_Data check_if_distract(Subj_Data S, Point_Obj est_gaze_pt)
{
int MONITOR_DIM_X = S.get_MONITOR_DIM_X();
int MONITOR_DIM_Y = S.get_MONITOR_DIM_Y();
//Console.WriteLine("INPUT PT: {0}", est_gaze_pt.ToString());
//est_gaze_pt.set_coord(-est_gaze_pt.get_x(), est_gaze_pt.get_y() + MONITOR_DIM_Y / 2, est_gaze_pt.get_z());
Point_Obj disp_pt = new Point_Obj(-est_gaze_pt.get_x(), est_gaze_pt.get_y() + MONITOR_DIM_Y / 2, est_gaze_pt.get_z());
int cur_num_frames_distracted = S.get_num_frames_distracted();
if (disp_pt.get_x() < -MONITOR_DIM_X / 2 || disp_pt.get_x() > MONITOR_DIM_X / 2)
{
//Console.WriteLine("INCREMENT!");
S.set_num_frames_distracted(++cur_num_frames_distracted);
}
else if (disp_pt.get_y() < -MONITOR_DIM_Y / 2 || disp_pt.get_y() > MONITOR_DIM_Y / 2)
{
//Console.WriteLine("INCREMENT!!");
S.set_num_frames_distracted(++cur_num_frames_distracted);
}
else
{
//Console.WriteLine("RESET");
S.set_num_frames_distracted(0);
}
//Console.WriteLine("NUM DISTRACT FRAMES PRIOR: {0}", S.get_num_frames_distracted());
return S;
}