From 4d62adf8bc4f78f8dca8196c7f52a6a44cc5829a Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Fri, 16 Dec 2016 11:54:21 +0100 Subject: tracker/pt: refactor camera info handling - Pass `struct CamInfo' rather than several elements separately - Reformat - Return `struct CamInfo' together with the frame since then it's always valid - Move the focal length formula into `struct CamInfo' - Remove incorrect focal length formula rather than #if 0 - Pass some stuff by reference and not by pointer --- tracker-pt/camera.cpp | 49 ++++++++++++++++++++------ tracker-pt/camera.h | 75 ++++++++++++++++++++++++---------------- tracker-pt/ftnoir_tracker_pt.cpp | 57 ++++++------------------------ tracker-pt/ftnoir_tracker_pt.h | 2 -- tracker-pt/point_extractor.cpp | 8 +++-- tracker-pt/point_extractor.h | 1 + tracker-pt/point_tracker.cpp | 31 ++++++++--------- tracker-pt/point_tracker.h | 5 +-- 8 files changed, 118 insertions(+), 110 deletions(-) (limited to 'tracker-pt') diff --git a/tracker-pt/camera.cpp b/tracker-pt/camera.cpp index 60eb4bb8..220bcc8e 100644 --- a/tracker-pt/camera.cpp +++ b/tracker-pt/camera.cpp @@ -6,8 +6,8 @@ */ #include "camera.h" -#include -#include + +namespace impl { QString Camera::get_desired_name() const { @@ -19,7 +19,24 @@ QString Camera::get_active_name() const return active_name; } -DEFUN_WARN_UNUSED bool Camera::get_info(CamInfo& ret) +void CamInfo::get_focal_length(f& fx) const +{ + using std::tan; + using std::atan; + using std::sqrt; + + const double diag_len = sqrt(double(res_x*res_x + res_y*res_y)); + const double aspect_x = res_x / diag_len; + //const double aspect_y = res_y / diag_len; + const double diag_fov = fov * M_PI/180; + const double fov_x = 2*atan(tan(diag_fov*.5) * aspect_x); + //const double fov_y = 2*atan(tan(diag_fov*.5) * aspect_y); + fx = .5 / tan(fov_x * .5); + //fy = .5 / tan(fov_y * .5); + //static bool once = false; if (!once) { once = true; qDebug() << "f" << ret << "fov" << (fov * 180/M_PI); } +} + +DEFUN_WARN_UNUSED bool Camera::get_info(CamInfo& ret) const { if (cam_info.res_x == 0 || cam_info.res_y == 0) return false; @@ -27,20 +44,29 @@ DEFUN_WARN_UNUSED bool Camera::get_info(CamInfo& ret) return true; } -bool Camera::get_frame(double dt, cv::Mat* frame) +DEFUN_WARN_UNUSED bool Camera::get_frame(double dt, cv::Mat& frame, CamInfo& info) { bool new_frame = _get_frame(frame); + // measure fps of valid frames static constexpr double RC = .1; // seconds const double alpha = dt/(dt + RC); dt_valid += dt; + if (new_frame) { if (dt_mean < 2e-3) dt_mean = dt; else dt_mean = (1-alpha) * dt_mean + alpha * dt_valid; + cam_info.fps = dt_mean > 2e-3 ? int(1 / dt_mean + .65) : 0; + cam_info.res_x = frame.cols; + cam_info.res_y = frame.rows; + cam_info.fov = s.fov; + + info = cam_info; + dt_valid = 0; } else @@ -58,6 +84,8 @@ DEFUN_WARN_UNUSED bool Camera::start(int idx, int fps, int res_x, int res_y) cam_desired.res_x != res_x || cam_desired.res_y != res_y) { + qDebug() << "pt: opening camera"; + cam_desired.idx = idx; cam_desired.fps = fps; cam_desired.res_x = res_x; @@ -71,9 +99,8 @@ DEFUN_WARN_UNUSED bool Camera::start(int idx, int fps, int res_x, int res_y) if (cap->isOpened()) { + cam_info = CamInfo(); cam_info.idx = cam_desired.idx; - cam_info.res_x = 0; - cam_info.res_y = 0; active_name = desired_name; return true; @@ -93,18 +120,16 @@ void Camera::stop() cam_desired = CamInfo(); } -bool Camera::_get_frame(cv::Mat* frame) +DEFUN_WARN_UNUSED bool Camera::_get_frame(cv::Mat& frame) { if (cap && cap->isOpened()) { - for (int i = 0; i < 100 && !cap->read(*frame); i++) + for (int i = 0; i < 100 && !cap->read(frame); i++) ;; - if (frame->empty()) + if (frame.empty()) return false; - cam_info.res_x = frame->cols; - cam_info.res_y = frame->rows; return true; } return false; @@ -119,3 +144,5 @@ void Camera::camera_deleter::operator()(cv::VideoCapture* cap) std::default_delete()(cap); } } + +} // ns impl diff --git a/tracker-pt/camera.h b/tracker-pt/camera.h index 3f5a8f43..1d3b332c 100644 --- a/tracker-pt/camera.h +++ b/tracker-pt/camera.h @@ -10,17 +10,27 @@ #undef NDEBUG #include +#include "numeric.hpp" +#include "ftnoir_tracker_pt_settings.h" + #include "compat/util.hpp" #include -#include #include -#include + +#include #include -struct CamInfo +namespace impl { + +using namespace types; + +struct CamInfo final { - CamInfo() : res_x(0), res_y(0), fps(-1), idx(-1) {} + CamInfo() : fov(0), res_x(0), res_y(0), fps(-1), idx(-1) {} + void get_focal_length(f& fx) const; + + double fov; int res_x; int res_y; @@ -31,40 +41,47 @@ struct CamInfo class Camera final { public: - Camera() : dt_valid(0), dt_mean(0) {} + Camera() : dt_valid(0), dt_mean(0) {} - DEFUN_WARN_UNUSED bool start(int idx, int fps, int res_x, int res_y); - void stop(); + DEFUN_WARN_UNUSED bool start(int idx, int fps, int res_x, int res_y); + void stop(); - DEFUN_WARN_UNUSED bool get_frame(double dt, cv::Mat* frame); - DEFUN_WARN_UNUSED bool get_info(CamInfo &ret); + DEFUN_WARN_UNUSED bool get_frame(double dt, cv::Mat& frame, CamInfo& info); + DEFUN_WARN_UNUSED bool get_info(CamInfo &ret) const; - CamInfo get_desired() const { return cam_desired; } - QString get_desired_name() const; - QString get_active_name() const; + CamInfo get_desired() const { return cam_desired; } + QString get_desired_name() const; + QString get_active_name() const; - cv::VideoCapture& operator*() { assert(cap); return *cap; } - const cv::VideoCapture& operator*() const { assert(cap); return *cap; } - cv::VideoCapture* operator->() { assert(cap); return cap.get(); } - const cv::VideoCapture* operator->() const { return cap.get(); } - operator bool() const { return cap && cap->isOpened(); } + cv::VideoCapture& operator*() { assert(cap); return *cap; } + const cv::VideoCapture& operator*() const { assert(cap); return *cap; } + cv::VideoCapture* operator->() { assert(cap); return cap.get(); } + const cv::VideoCapture* operator->() const { return cap.get(); } + operator bool() const { return cap && cap->isOpened(); } private: - DEFUN_WARN_UNUSED bool _get_frame(cv::Mat* frame); + DEFUN_WARN_UNUSED bool _get_frame(cv::Mat& frame); + + settings_pt s; - double dt_valid; - double dt_mean; + double dt_valid; + double dt_mean; - CamInfo cam_info; - CamInfo cam_desired; - QString desired_name, active_name; + CamInfo cam_info; + CamInfo cam_desired; + QString desired_name, active_name; - struct camera_deleter final - { - void operator()(cv::VideoCapture* cap); - }; + struct camera_deleter final + { + void operator()(cv::VideoCapture* cap); + }; - using camera_ptr = std::unique_ptr; + using camera_ptr = std::unique_ptr; - camera_ptr cap; + camera_ptr cap; }; + +} // ns impl + +using impl::Camera; +using impl::CamInfo; diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp index 31d3cb14..33a40825 100644 --- a/tracker-pt/ftnoir_tracker_pt.cpp +++ b/tracker-pt/ftnoir_tracker_pt.cpp @@ -23,7 +23,7 @@ Tracker_PT::Tracker_PT() : commands(0), ever_success(false) { - connect(s.b.get(), SIGNAL(saving()), this, SLOT(apply_settings())); + connect(s.b.get(), SIGNAL(saving()), this, SLOT(apply_settings()), Qt::DirectConnection); } Tracker_PT::~Tracker_PT() @@ -47,35 +47,6 @@ void Tracker_PT::reset_command(Command command) commands &= ~command; } -bool Tracker_PT::get_focal_length(f& ret) -{ - QMutexLocker l(&camera_mtx); - CamInfo info; - const bool res = camera.get_info(info); - if (res) - { - using std::tan; - using std::atan; - using std::sqrt; - - const double w = info.res_x, h = info.res_y; -#if 0 - const double diag = sqrt(w/h*w/h + h/w*h/w); - const double diag_fov = static_cast(s.fov) * M_PI / 180.; - const double fov = 2.*atan(tan(diag_fov/2.)/diag); - ret = .5 / tan(.5 * fov); -#else - const double diag_fov = s.fov * M_PI/180; - const double aspect = w / sqrt(w*w + h*h); - const double fov = 2*atan(tan(diag_fov*.5) * aspect); - ret = .5 / tan(fov * .5); - //static bool once = false; if (!once) { once = true; qDebug() << "f" << ret << "fov" << (fov * 180/M_PI); } -#endif - return true; - } - return false; -} - void Tracker_PT::run() { cv::setNumThreads(0); @@ -93,30 +64,27 @@ void Tracker_PT::run() { const double dt = time.elapsed_seconds(); time.start(); + CamInfo cam_info; bool new_frame; { QMutexLocker l(&camera_mtx); - new_frame = camera.get_frame(dt, &frame); - if (frame.rows != frame_.rows || frame.cols != frame_.cols) - frame_ = cv::Mat(frame.rows, frame.cols, CV_8UC3); + new_frame = camera.get_frame(dt, frame, cam_info); + if (new_frame) + { + if (frame.rows != frame_.rows || frame.cols != frame_.cols) + frame_ = cv::Mat(frame.rows, frame.cols, CV_8UC3); + } frame.copyTo(frame_); } if (new_frame && !frame_.empty()) { - CamInfo cam_info; - - if (!camera.get_info(cam_info)) - continue; - point_extractor.extract_points(frame_, points); point_count = points.size(); f fx; - - if (!get_focal_length(fx)) - continue; + cam_info.get_focal_length(fx); const bool success = points.size() >= PointModel::N_POINTS; @@ -124,11 +92,8 @@ void Tracker_PT::run() { point_tracker.track(points, PointModel(s), - fx, - s.dynamic_pose, - s.init_phase_timeout, - cam_info.res_x, - cam_info.res_y); + cam_info, + s.dynamic_pose ? s.init_phase_timeout : 0); ever_success = true; } diff --git a/tracker-pt/ftnoir_tracker_pt.h b/tracker-pt/ftnoir_tracker_pt.h index 66928655..fcf74bff 100644 --- a/tracker-pt/ftnoir_tracker_pt.h +++ b/tracker-pt/ftnoir_tracker_pt.h @@ -63,8 +63,6 @@ private: void set_command(Command command); void reset_command(Command command); - bool get_focal_length(f& ret); - QMutex camera_mtx; QMutex data_mtx; Camera camera; diff --git a/tracker-pt/point_extractor.cpp b/tracker-pt/point_extractor.cpp index a688faad..f7f63784 100644 --- a/tracker-pt/point_extractor.cpp +++ b/tracker-pt/point_extractor.cpp @@ -83,9 +83,6 @@ void PointExtractor::extract_points(cv::Mat& frame, std::vector& points) using std::round; using std::sort; - const int W = frame.cols; - const int H = frame.rows; - if (frame_gray.rows != frame.rows || frame_gray.cols != frame.cols) { frame_gray = cv::Mat(frame.rows, frame.cols, CV_8U); @@ -219,6 +216,9 @@ end: sort(blobs.begin(), blobs.end(), [](const blob& b1, const blob& b2) -> bool { return b2.brightness < b1.brightness; }); + const int W = frame.cols; + const int H = frame.rows; + for (idx = 0; idx < std::min(PointModel::N_POINTS, unsigned(blobs.size())); ++idx) { blob &b = blobs[idx]; @@ -253,6 +253,8 @@ end: for (auto& b : blobs) { + // note: H/W is equal to fx/fy + vec2 p((b.pos[0] - W/2)/W, -(b.pos[1] - H/2)/W); points.push_back(p); } diff --git a/tracker-pt/point_extractor.h b/tracker-pt/point_extractor.h index ad350344..3ad8ed52 100644 --- a/tracker-pt/point_extractor.h +++ b/tracker-pt/point_extractor.h @@ -9,6 +9,7 @@ #pragma once #include "ftnoir_tracker_pt_settings.h" +#include "camera.h" #include "numeric.hpp" #include diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp index 7c0367d0..cae68bf3 100644 --- a/tracker-pt/point_tracker.cpp +++ b/tracker-pt/point_tracker.cpp @@ -89,16 +89,15 @@ PointTracker::PointTracker() : init_phase(true) PointTracker::PointOrder PointTracker::find_correspondences_previous(const vec2* points, const PointModel& model, - f focal_length, - int w, - int h) + const CamInfo& info) { + f fx; info.get_focal_length(fx); PointTracker::PointOrder p; - p[0] = project(vec3(0,0,0), focal_length); - p[1] = project(model.M01, focal_length); - p[2] = project(model.M02, focal_length); + p[0] = project(vec3(0,0,0), fx); + p[1] = project(model.M01, fx); + p[2] = project(model.M02, fx); - const int diagonal = int(std::sqrt(w*w + h*h)); + const int diagonal = int(std::sqrt(double(info.res_x*info.res_x + info.res_y*info.res_y))); static constexpr int div = 100; const int max_dist = diagonal / div; // 8 pixels for 640x480 @@ -140,26 +139,25 @@ PointTracker::PointOrder PointTracker::find_correspondences_previous(const vec2* void PointTracker::track(const std::vector& points, const PointModel& model, - f focal_length, - bool dynamic_pose, - int init_phase_timeout, - int w, - int h) + const CamInfo& info, + int init_phase_timeout) { + f fx; + info.get_focal_length(fx); PointOrder order; - if (t.elapsed_ms() > init_phase_timeout) + if (init_phase_timeout > 0 && t.elapsed_ms() > init_phase_timeout) { t.start(); init_phase = true; } - if (!dynamic_pose || init_phase) + if (!(init_phase_timeout > 0 && !init_phase)) order = find_correspondences(points.data(), model); else - order = find_correspondences_previous(points.data(), model, focal_length, w, h); + order = find_correspondences_previous(points.data(), model, info); - if (POSIT(model, order, focal_length) != -1) + if (POSIT(model, order, fx) != -1) { init_phase = false; t.start(); @@ -351,4 +349,3 @@ vec2 PointTracker::project(const vec3& v_M, f focal_length, const Affine& X_CM) vec3 v_C = X_CM * v_M; return vec2(focal_length*v_C[0]/v_C[2], focal_length*v_C[1]/v_C[2]); } - diff --git a/tracker-pt/point_tracker.h b/tracker-pt/point_tracker.h index 0bac05ab..63caf0dd 100644 --- a/tracker-pt/point_tracker.h +++ b/tracker-pt/point_tracker.h @@ -11,6 +11,7 @@ #include "ftnoir_tracker_pt_settings.h" #include "affine.hpp" #include "numeric.hpp" +#include "camera.h" #include #include @@ -57,7 +58,7 @@ public: // track the pose using the set of normalized point coordinates (x pos in range -0.5:0.5) // f : (focal length)/(sensor width) // dt : time since last call - void track(const std::vector& projected_points, const PointModel& model, f focal_length, bool dynamic_pose, int init_phase_timeout, int w, int h); + void track(const std::vector& projected_points, const PointModel& model, const CamInfo& info, int init_phase_timeout); Affine pose() { return X_CM; } vec2 project(const vec3& v_M, f focal_length); vec2 project(const vec3& v_M, f focal_length, const Affine& X_CM); @@ -67,7 +68,7 @@ private: using PointOrder = std::array; PointOrder find_correspondences(const vec2* projected_points, const PointModel &model); - PointOrder find_correspondences_previous(const vec2* points, const PointModel &model, f focal_length, int w, int h); + PointOrder find_correspondences_previous(const vec2* points, const PointModel &model, const CamInfo& info); int POSIT(const PointModel& point_model, const PointOrder& order, f focal_length); // The POSIT algorithm, returns the number of iterations Affine X_CM; // trafo from model to camera -- cgit v1.2.3