summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2015-02-17 06:09:34 +0100
committerStanislaw Halik <sthalik@misaki.pl>2015-02-17 06:09:34 +0100
commit7da215ba3abca612b30d2d66e2be35f11342a7b0 (patch)
tree1c908479f5679100b05236a3c11bbeb27b979003
parent48d1c492f738f077a89d7d0fbbfb9c8d368fefa0 (diff)
pt: calibrate only if pose received
Otherwise breaks center-on-startup logic.
-rw-r--r--ftnoir_tracker_pt/ftnoir_tracker_pt.cpp69
-rw-r--r--ftnoir_tracker_pt/ftnoir_tracker_pt.h2
2 files changed, 39 insertions, 32 deletions
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 23cc72c7..6b88884b 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -22,7 +22,8 @@ Tracker::Tracker()
: mutex(QMutex::Recursive),
commands(0),
video_widget(NULL),
- video_frame(NULL)
+ video_frame(NULL),
+ success(false)
{
connect(s.b.get(), SIGNAL(saving()), this, SLOT(apply_settings()));
}
@@ -76,7 +77,9 @@ void Tracker::run()
std::vector<cv::Vec2f> points = point_extractor.extract_points(frame);
- if (points.size() == PointModel::N_POINTS)
+ success = points.size() == PointModel::N_POINTS;
+
+ if (success)
point_tracker.track(points, PointModel(s), get_focal_length(), s.dynamic_pose);
{
@@ -158,36 +161,38 @@ void Tracker::StopTracker(bool exit)
void Tracker::data(THeadPoseData *data)
{
-
- Affine X_CM = pose();
-
- Affine X_MH(Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z));
- Affine X_GH = X_CM * X_MH;
-
- Matx33f R = X_GH.R * X_MH.R.t();
- Vec3f t = X_GH.t;
-
- // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame
- // -z -> x, y -> z, x -> -y
- Matx33f R_EG(0, 0,-1,
- -1, 0, 0,
- 0, 1, 0);
- R = R_EG * R * R_EG.t();
-
- // extract rotation angles
- float alpha, beta, gamma;
- beta = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
- alpha = atan2( R(1,0), R(0,0));
- gamma = atan2( R(2,1), R(2,2));
-
- // extract rotation angles
- data[Yaw] = rad2deg * alpha;
- data[Pitch] = -rad2deg * beta;
- data[Roll] = rad2deg * gamma;
- // get translation(s)
- data[TX] = t[0] / 10.0; // convert to cm
- data[TY] = t[1] / 10.0;
- data[TZ] = t[2] / 10.0;
+ if (success)
+ {
+ Affine X_CM = pose();
+
+ Affine X_MH(Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z));
+ Affine X_GH = X_CM * X_MH;
+
+ Matx33f R = X_GH.R * X_MH.R.t();
+ Vec3f t = X_GH.t;
+
+ // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame
+ // -z -> x, y -> z, x -> -y
+ Matx33f R_EG(0, 0,-1,
+ -1, 0, 0,
+ 0, 1, 0);
+ R = R_EG * R * R_EG.t();
+
+ // extract rotation angles
+ float alpha, beta, gamma;
+ beta = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
+ alpha = atan2( R(1,0), R(0,0));
+ gamma = atan2( R(2,1), R(2,2));
+
+ // extract rotation angles
+ data[Yaw] = rad2deg * alpha;
+ data[Pitch] = -rad2deg * beta;
+ data[Roll] = rad2deg * gamma;
+ // get translation(s)
+ data[TX] = t[0] / 10.0; // convert to cm
+ data[TY] = t[1] / 10.0;
+ data[TZ] = t[2] / 10.0;
+ }
}
//-----------------------------------------------------------------------------
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.h b/ftnoir_tracker_pt/ftnoir_tracker_pt.h
index 14be5c7f..399d0693 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.h
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.h
@@ -71,6 +71,8 @@ private:
settings s;
Timer time;
+
+ bool success;
static constexpr double rad2deg = 180.0/3.14159265;
static constexpr double deg2rad = 3.14159265/180.0;