diff options
| -rw-r--r-- | ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 42 | ||||
| -rw-r--r-- | ftnoir_tracker_pt/point_tracker.cpp | 68 |
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(); } |