summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_tracker_pt
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2015-10-17 17:15:53 +0200
committerStanislaw Halik <sthalik@misaki.pl>2015-10-17 17:27:29 +0200
commit22857e786cd7462eb087cf039cefd36076784280 (patch)
treea43c1d8801511977084ab1d74be43ea622f55591 /ftnoir_tracker_pt
parent6dc31bc869c44c84429f3e52a033cbc09c1d3844 (diff)
pt: reformat more
Diffstat (limited to 'ftnoir_tracker_pt')
-rw-r--r--ftnoir_tracker_pt/ftnoir_tracker_pt.cpp42
-rw-r--r--ftnoir_tracker_pt/point_tracker.cpp68
2 files changed, 55 insertions, 55 deletions
diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
index 19d1bd7f..11fe64cc 100644
--- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
+++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp
@@ -19,8 +19,8 @@
Tracker_PT::Tracker_PT()
: mutex(QMutex::Recursive),
commands(0),
- video_widget(NULL),
- video_frame(NULL),
+ video_widget(NULL),
+ video_frame(NULL),
ever_success(false)
{
connect(s.b.get(), SIGNAL(saving()), this, SLOT(apply_settings()));
@@ -28,8 +28,8 @@ Tracker_PT::Tracker_PT()
Tracker_PT::~Tracker_PT()
{
- set_command(ABORT);
- wait();
+ set_command(ABORT);
+ wait();
delete video_widget;
video_widget = NULL;
if (video_frame->layout()) delete video_frame->layout();
@@ -39,13 +39,13 @@ Tracker_PT::~Tracker_PT()
void Tracker_PT::set_command(Command command)
{
//QMutexLocker lock(&mutex);
- commands |= command;
+ commands |= command;
}
void Tracker_PT::reset_command(Command command)
{
//QMutexLocker lock(&mutex);
- commands &= ~command;
+ commands &= ~command;
}
bool Tracker_PT::get_focal_length(float& ret)
@@ -69,13 +69,13 @@ bool Tracker_PT::get_focal_length(float& ret)
void Tracker_PT::run()
{
#ifdef PT_PERF_LOG
- QFile log_file(QCoreApplication::applicationDirPath() + "/PointTrackerPerformance.txt");
- if (!log_file.open(QIODevice::WriteOnly | QIODevice::Text)) return;
- QTextStream log_stream(&log_file);
+ QFile log_file(QCoreApplication::applicationDirPath() + "/PointTrackerPerformance.txt");
+ if (!log_file.open(QIODevice::WriteOnly | QIODevice::Text)) return;
+ QTextStream log_stream(&log_file);
#endif
apply_settings();
-
+
while((commands & ABORT) == 0)
{
const double dt = time.elapsed() * 1e-9;
@@ -97,20 +97,20 @@ void Tracker_PT::run()
// blobs are sorted in order of circularity
if (points.size() > PointModel::N_POINTS)
points.resize(PointModel::N_POINTS);
-
+
bool success = points.size() == PointModel::N_POINTS;
-
+
ever_success |= success;
float fx;
if (!get_focal_length(fx))
continue;
-
+
if (success)
{
point_tracker.track(points, PointModel(s), fx, s.dynamic_pose, s.init_phase_timeout);
}
-
+
{
Affine X_CM = pose();
Affine X_MH(cv::Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z)); // just copy pasted these lines from below
@@ -119,7 +119,7 @@ void Tracker_PT::run()
cv::Vec2f p_(p[0] / p[2] * fx, p[1] / p[2] * fx); // projected to screen
points.push_back(p_);
}
-
+
for (unsigned i = 0; i < points.size(); i++)
{
auto& p = points[i];
@@ -138,7 +138,7 @@ void Tracker_PT::run()
color,
4);
}
-
+
video_widget->update_image(frame);
}
#ifdef PT_PERF_LOG
@@ -181,26 +181,26 @@ void Tracker_PT::data(double *data)
if (ever_success)
{
Affine X_CM = pose();
-
+
Affine X_MH(cv::Matx33f::eye(), cv::Vec3f(s.t_MH_x, s.t_MH_y, s.t_MH_z));
Affine X_GH = X_CM * X_MH;
-
+
cv::Matx33f R = X_GH.R;
cv::Vec3f t = X_GH.t;
-
+
// translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame
// -z -> x, y -> z, x -> -y
cv::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;
diff --git a/ftnoir_tracker_pt/point_tracker.cpp b/ftnoir_tracker_pt/point_tracker.cpp
index 383061c5..cec107cf 100644
--- a/ftnoir_tracker_pt/point_tracker.cpp
+++ b/ftnoir_tracker_pt/point_tracker.cpp
@@ -18,16 +18,16 @@ const float PI = 3.14159265358979323846f;
// ----------------------------------------------------------------------------
static void get_row(const cv::Matx33f& m, int i, cv::Vec3f& v)
{
- v[0] = m(i,0);
- v[1] = m(i,1);
- v[2] = m(i,2);
+ v[0] = m(i,0);
+ v[1] = m(i,1);
+ v[2] = m(i,2);
}
static void set_row(cv::Matx33f& m, int i, const cv::Vec3f& v)
{
- m(i,0) = v[0];
- m(i,1) = v[1];
- m(i,2) = v[2];
+ m(i,0) = v[0];
+ m(i,1) = v[1];
+ m(i,2) = v[2];
}
static bool d_vals_sort(const std::pair<float,int> a, const std::pair<float,int> b)
@@ -67,31 +67,31 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const std::
// set correspondences by minimum distance to projected model point
bool point_taken[PointModel::N_POINTS];
for (int i=0; i<PointModel::N_POINTS; ++i)
- point_taken[i] = false;
+ point_taken[i] = false;
for (int i=0; i<PointModel::N_POINTS; ++i)
{
- float min_sdist = 0;
- int min_idx = 0;
- // find closest point to projected model point i
- for (int j=0; j<PointModel::N_POINTS; ++j)
+ float min_sdist = 0;
+ int min_idx = 0;
+ // find closest point to projected model point i
+ for (int j=0; j<PointModel::N_POINTS; ++j)
+ {
+ cv::Vec2f d = p.points[i]-points[j];
+ float sdist = d.dot(d);
+ if (sdist < min_sdist || j==0)
{
- cv::Vec2f d = p.points[i]-points[j];
- float sdist = d.dot(d);
- if (sdist < min_sdist || j==0)
- {
- min_idx = j;
- min_sdist = sdist;
- }
+ min_idx = j;
+ min_sdist = sdist;
}
- // if one point is closest to more than one model point, fallback
- if (point_taken[min_idx])
- {
- init_phase = true;
- return find_correspondences(points, model);
- }
- point_taken[min_idx] = true;
- p.points[i] = points[min_idx];
+ }
+ // if one point is closest to more than one model point, fallback
+ if (point_taken[min_idx])
+ {
+ init_phase = true;
+ return find_correspondences(points, model);
+ }
+ point_taken[min_idx] = true;
+ p.points[i] = points[min_idx];
}
return p;
}
@@ -100,19 +100,19 @@ void PointTracker::track(const std::vector<cv::Vec2f>& points, const PointModel&
{
PointOrder order;
- if (t.elapsed_ms() > init_phase_timeout)
- {
- t.start();
- init_phase = true;
- }
+ if (t.elapsed_ms() > init_phase_timeout)
+ {
+ t.start();
+ init_phase = true;
+ }
if (!dynamic_pose || init_phase)
order = find_correspondences(points, model);
- else
- order = find_correspondences_previous(points, model, f);
+ else
+ order = find_correspondences_previous(points, model, f);
POSIT(model, order, f);
- init_phase = false;
+ init_phase = false;
t.start();
}