1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
|
/* Copyright (c) 2023 Eyeware Tech SA https://www.eyeware.tech
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
* copyright notice and this permission notice appear in all copies.
*/
#include "eyeware_beam.h"
#include <QMutexLocker>
static constexpr double rad_to_deg = 180.0 / M_PI;
static constexpr double m_to_cm = 100.0;
static constexpr double epsilon = 0.000001;
eyeware_beam_tracker::eyeware_beam_tracker()
{
}
eyeware_beam_tracker::~eyeware_beam_tracker()
{
QMutexLocker lck(&mtx);
release_tracker_instance(tracker_client);
tracker_client = nullptr;
}
module_status eyeware_beam_tracker::start_tracker(QFrame* videoframe)
{
QMutexLocker lck(&mtx);
try
{
tracker_client = create_tracker_instance("127.0.0.1", 12010);
}
catch (...)
{
return error("Eyeware Beam initialization has failed");
}
return status_ok();
}
void eyeware_beam_tracker::extract_translation(const eyeware::Vector3D& t,
double& translation_x_cm,
double& translation_y_cm,
double& translation_z_cm)
{
translation_x_cm = +t.x * m_to_cm;
translation_y_cm = -t.y * m_to_cm;
translation_z_cm = +t.z * m_to_cm;
}
void eyeware_beam_tracker::extract_rotation_angles(const eyeware::Matrix3x3& R,
double& pitch_deg,
double& roll_deg,
double& yaw_deg)
{
double r00 = static_cast<double>(R[0][0]);
double r01 = static_cast<double>(R[0][1]);
double r02 = static_cast<double>(R[0][2]);
double r10 = static_cast<double>(R[1][0]);
double r11 = static_cast<double>(R[1][1]);
double r12 = static_cast<double>(R[1][2]);
double r20 = static_cast<double>(R[2][0]);
double r21 = static_cast<double>(R[2][1]);
double r22 = static_cast<double>(R[2][2]);
double dy = std::sqrt(r00 * r00 + r10 * r10);
last_yaw_deg = -std::atan2(-r20, dy) * rad_to_deg;
last_roll_deg = 0.0;
if (dy > epsilon)
{
last_pitch_deg = -std::atan2(r21, r22) * rad_to_deg;
last_roll_deg = +std::atan2(r10, r00) * rad_to_deg;
}
else
{
last_pitch_deg = -std::atan2(-r12, r11) * rad_to_deg;
}
}
void eyeware_beam_tracker::data(double *data)
{
QMutexLocker lck(&mtx);
if (connected(tracker_client))
{
eyeware::HeadPoseInfo head_pose_info = get_head_pose_info(tracker_client);
if (!head_pose_info.is_lost)
{
extract_translation(head_pose_info.transform.translation, last_translation_x_cm,
last_translation_y_cm, last_translation_z_cm);
extract_rotation_angles(head_pose_info.transform.rotation, last_pitch_deg, last_roll_deg, last_yaw_deg);
}
}
data[TX] = last_translation_x_cm;
data[TY] = last_translation_y_cm;
data[TZ] = last_translation_z_cm;
data[Yaw] = last_yaw_deg;
data[Pitch] = last_pitch_deg;
data[Roll] = last_roll_deg;
}
eyeware_beam_dialog::eyeware_beam_dialog()
{
ui.setupUi(this);
connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
}
void eyeware_beam_dialog::doOK()
{
close();
}
OPENTRACK_DECLARE_TRACKER(eyeware_beam_tracker, eyeware_beam_dialog, eyeware_beam_metadata)
|
