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; return new Point_Obj(Convert.ToSingle(p_hat_avg[0]), Convert.ToSingle(p_hat_avg[1])); }
private void process_float_data(float[] input_array) { float frame_num = input_array[0]; float success_flag = input_array[1]; 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; if (success_flag == 0.0) { Console.WriteLine("{0}: Tracking Failure", Convert.ToInt32(frame_num)); plot_gaze_point(new Point_Obj(-1000,-1000)); return; } Gaze_Comp G = new Gaze_Comp(); Point_Obj p = G.get_gaze_pt(right_eye, left_eye, alpha, beta, gamma); Console.WriteLine("GAZE POINT (X, Y): ({0}, {1})", p.get_x(), p.get_y()); Point_Obj disp_pt = new Point_Obj(-p.get_x(), p.get_y()+ MONITOR_DIM_Y/2); plot_gaze_point(disp_pt); Console.WriteLine("TRANSF GAZE POINT (X, Y): ({0}, {1})", disp_pt.get_x(), disp_pt.get_y()); // Plot Rotation Angles plot_point("Pitch", Convert.ToDouble(alpha)); plot_point("Yaw", Convert.ToDouble(beta)); plot_point("Roll", Convert.ToDouble(gamma)); set_axis(); }
private void plot_gaze_point(Point_Obj p) { if (this.chart2.InvokeRequired) { //ChartCallback c = new ChartCallback(plot_point); GazeChartCallback gC = new GazeChartCallback(plot_gaze_point); this.Invoke(gC, new object[] { p }); } else { this.chart2.Series["Series1"].Points.AddXY(p.get_x(), p.get_y()); int L = this.chart2.Series["Series1"].Points.Count; if (p.get_x() < -MONITOR_DIM_X / 2 || p.get_x() > MONITOR_DIM_X / 2) { this.chart2.Series["Series1"].Points[L - 1].Color = Color.Red; } else if (p.get_y() < -MONITOR_DIM_Y / 2 || p.get_y() > MONITOR_DIM_Y / 2) { this.chart2.Series["Series1"].Points[L - 1].Color = Color.Red; } else { this.chart2.Series["Series1"].Points[L - 1].Color = Color.Blue; } if(L > 25) { DataPoint d = new DataPoint(this.chart2.Series["Series1"].Points[0].XValue, this.chart2.Series["Series1"].Points[0].YValues[0]); saved_pts.Add(d); this.chart2.Series["Series1"].Points.RemoveAt(0); } } }