From 0f577dbe8adb8f7e210241c6bee0be73349f8d45 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 6 Dec 2015 00:41:59 +0100 Subject: tracker/aruco, tracker/pt: sleep before releasing camera Really fast toggling tracking crashed with my PS3 Eye. --- tracker-pt/ftnoir_tracker_pt.cpp | 3 +++ 1 file changed, 3 insertions(+) (limited to 'tracker-pt/ftnoir_tracker_pt.cpp') diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp index 15a60962..2dbf9068 100644 --- a/tracker-pt/ftnoir_tracker_pt.cpp +++ b/tracker-pt/ftnoir_tracker_pt.cpp @@ -13,6 +13,7 @@ #include #include #include "opentrack/camera-names.hpp" +#include "opentrack-compat/sleep.hpp" //#define PT_PERF_LOG //log performance @@ -34,6 +35,8 @@ Tracker_PT::~Tracker_PT() delete video_widget; video_widget = NULL; if (video_frame->layout()) delete video_frame->layout(); + // fast start/stop causes breakage + portable::sleep(1000); camera.stop(); } -- cgit v1.2.3 From 211a17996e3ffc0d84c985c43c612da32923b42d Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 6 Dec 2015 02:24:44 +0100 Subject: api/camera-names: move to compat/ --- opentrack-compat/CMakeLists.txt | 1 + opentrack-compat/camera-names.cpp | 113 ++++++++++++++++++++++++++++++++ opentrack-compat/camera-names.hpp | 22 +++++++ opentrack/CMakeLists.txt | 1 - opentrack/camera-names.cpp | 113 -------------------------------- opentrack/camera-names.hpp | 16 ----- opentrack/opencv-camera-dialog.hpp | 2 +- tracker-aruco/ftnoir_tracker_aruco.cpp | 2 +- tracker-pt/ftnoir_tracker_pt.cpp | 2 +- tracker-pt/ftnoir_tracker_pt_dialog.cpp | 2 +- 10 files changed, 140 insertions(+), 134 deletions(-) create mode 100644 opentrack-compat/camera-names.cpp create mode 100644 opentrack-compat/camera-names.hpp delete mode 100644 opentrack/camera-names.cpp delete mode 100644 opentrack/camera-names.hpp (limited to 'tracker-pt/ftnoir_tracker_pt.cpp') diff --git a/opentrack-compat/CMakeLists.txt b/opentrack-compat/CMakeLists.txt index 37d53e6c..8dba67e2 100644 --- a/opentrack-compat/CMakeLists.txt +++ b/opentrack-compat/CMakeLists.txt @@ -2,3 +2,4 @@ opentrack_boilerplate(opentrack-compat NO-COMPAT NO-LINKER-SCRIPT LINKAGE) if(NOT WIN32 AND NOT APPLE) target_link_libraries(opentrack-compat rt) endif() +link_with_dinput8(opentrack-compat) diff --git a/opentrack-compat/camera-names.cpp b/opentrack-compat/camera-names.cpp new file mode 100644 index 00000000..72bcf41a --- /dev/null +++ b/opentrack-compat/camera-names.cpp @@ -0,0 +1,113 @@ +#include "camera-names.hpp" + +#if defined(OPENTRACK_API) && defined(_WIN32) +# define NO_DSHOW_STRSAFE +# include +# include +#endif + +#if defined(OPENTRACK_API) && (defined(__unix) || defined(__linux) || defined(__APPLE__)) +# include +#endif + +#ifdef __linux +#include +#include +#include +#include +#endif + +#include + +OPENTRACK_COMPAT_EXPORT int camera_name_to_index(const QString &name) +{ + auto list = get_camera_names(); + int ret = list.indexOf(name); + if (ret < 0) + ret = 0; + return ret; +} + +OPENTRACK_COMPAT_EXPORT QList get_camera_names() { + QList ret; +#if defined(_WIN32) + // Create the System Device Enumerator. + HRESULT hr; + hr = CoInitializeEx(NULL, COINIT_MULTITHREADED); + if (FAILED(hr)) + qDebug() << "failed CoInitializeEx" << hr; + ICreateDevEnum *pSysDevEnum = NULL; + hr = CoCreateInstance(CLSID_SystemDeviceEnum, NULL, CLSCTX_INPROC_SERVER, IID_ICreateDevEnum, (void **)&pSysDevEnum); + if (FAILED(hr)) + { + qDebug() << "failed CLSID_SystemDeviceEnum" << hr; + return ret; + } + // Obtain a class enumerator for the video compressor category. + IEnumMoniker *pEnumCat = NULL; + hr = pSysDevEnum->CreateClassEnumerator(CLSID_VideoInputDeviceCategory, &pEnumCat, 0); + + if (hr == S_OK) { + // Enumerate the monikers. + IMoniker *pMoniker = NULL; + ULONG cFetched; + while (pEnumCat->Next(1, &pMoniker, &cFetched) == S_OK) { + IPropertyBag *pPropBag; + hr = pMoniker->BindToStorage(0, 0, IID_IPropertyBag, (void **)&pPropBag); + if (SUCCEEDED(hr)) { + // To retrieve the filter's friendly name, do the following: + VARIANT varName; + VariantInit(&varName); + hr = pPropBag->Read(L"FriendlyName", &varName, 0); + if (SUCCEEDED(hr)) + { + // Display the name in your UI somehow. + QString str((QChar*)varName.bstrVal, wcslen(varName.bstrVal)); + ret.append(str); + } + VariantClear(&varName); + + ////// To create an instance of the filter, do the following: + ////IBaseFilter *pFilter; + ////hr = pMoniker->BindToObject(NULL, NULL, IID_IBaseFilter, + //// (void**)&pFilter); + // Now add the filter to the graph. + //Remember to release pFilter later. + pPropBag->Release(); + } + pMoniker->Release(); + } + pEnumCat->Release(); + } + else + qDebug() << "failed CLSID_VideoInputDeviceCategory" << hr; + + pSysDevEnum->Release(); +#endif +#ifdef __linux + for (int i = 0; i < 16; i++) { + char buf[128]; + sprintf(buf, "/dev/video%d", i); + if (access(buf, F_OK) == 0) + ret.append(buf); + else + continue; + + if (access(buf, R_OK | W_OK) == 0) { + int fd = open(buf, O_RDONLY); + if (fd == -1) + continue; + struct v4l2_capability video_cap; + if(ioctl(fd, VIDIOC_QUERYCAP, &video_cap) == -1) + { + qDebug() << "VIDIOC_QUERYCAP" << errno; + close(fd); + continue; + } + ret[ret.size()-1] = reinterpret_cast(video_cap.card); + close(fd); + } + } +#endif + return ret; +} \ No newline at end of file diff --git a/opentrack-compat/camera-names.hpp b/opentrack-compat/camera-names.hpp new file mode 100644 index 00000000..c2879000 --- /dev/null +++ b/opentrack-compat/camera-names.hpp @@ -0,0 +1,22 @@ +/* Copyright (c) 2014-2015, Stanislaw Halik + + * 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. + */ + +#pragma once + +#include +#include + +#ifdef BUILD_compat +# include "compat-export.hpp" +#else +# include "compat-import.hpp" +#endif + +OPENTRACK_COMPAT_EXPORT QList get_camera_names(); +OPENTRACK_COMPAT_EXPORT int camera_name_to_index(const QString &name); + diff --git a/opentrack/CMakeLists.txt b/opentrack/CMakeLists.txt index de5975e2..08003a5c 100644 --- a/opentrack/CMakeLists.txt +++ b/opentrack/CMakeLists.txt @@ -4,5 +4,4 @@ if(NOT WIN32) else() target_link_libraries(opentrack-api winmm) endif() -link_with_dinput8(opentrack-api) target_link_libraries(opentrack-api opentrack-spline-widget) diff --git a/opentrack/camera-names.cpp b/opentrack/camera-names.cpp deleted file mode 100644 index 74e998ca..00000000 --- a/opentrack/camera-names.cpp +++ /dev/null @@ -1,113 +0,0 @@ -#include "camera-names.hpp" - -#if defined(OPENTRACK_API) && defined(_WIN32) -# define NO_DSHOW_STRSAFE -# include -# include -#endif - -#if defined(OPENTRACK_API) && (defined(__unix) || defined(__linux) || defined(__APPLE__)) -# include -#endif - -#ifdef __linux -#include -#include -#include -#include -#endif - -#include - -int camera_name_to_index(const QString &name) -{ - auto list = get_camera_names(); - int ret = list.indexOf(name); - if (ret < 0) - ret = 0; - return ret; -} - -QList get_camera_names() { - QList ret; -#if defined(_WIN32) - // Create the System Device Enumerator. - HRESULT hr; - hr = CoInitializeEx(NULL, COINIT_MULTITHREADED); - if (FAILED(hr)) - qDebug() << "failed CoInitializeEx" << hr; - ICreateDevEnum *pSysDevEnum = NULL; - hr = CoCreateInstance(CLSID_SystemDeviceEnum, NULL, CLSCTX_INPROC_SERVER, IID_ICreateDevEnum, (void **)&pSysDevEnum); - if (FAILED(hr)) - { - qDebug() << "failed CLSID_SystemDeviceEnum" << hr; - return ret; - } - // Obtain a class enumerator for the video compressor category. - IEnumMoniker *pEnumCat = NULL; - hr = pSysDevEnum->CreateClassEnumerator(CLSID_VideoInputDeviceCategory, &pEnumCat, 0); - - if (hr == S_OK) { - // Enumerate the monikers. - IMoniker *pMoniker = NULL; - ULONG cFetched; - while (pEnumCat->Next(1, &pMoniker, &cFetched) == S_OK) { - IPropertyBag *pPropBag; - hr = pMoniker->BindToStorage(0, 0, IID_IPropertyBag, (void **)&pPropBag); - if (SUCCEEDED(hr)) { - // To retrieve the filter's friendly name, do the following: - VARIANT varName; - VariantInit(&varName); - hr = pPropBag->Read(L"FriendlyName", &varName, 0); - if (SUCCEEDED(hr)) - { - // Display the name in your UI somehow. - QString str((QChar*)varName.bstrVal, wcslen(varName.bstrVal)); - ret.append(str); - } - VariantClear(&varName); - - ////// To create an instance of the filter, do the following: - ////IBaseFilter *pFilter; - ////hr = pMoniker->BindToObject(NULL, NULL, IID_IBaseFilter, - //// (void**)&pFilter); - // Now add the filter to the graph. - //Remember to release pFilter later. - pPropBag->Release(); - } - pMoniker->Release(); - } - pEnumCat->Release(); - } - else - qDebug() << "failed CLSID_VideoInputDeviceCategory" << hr; - - pSysDevEnum->Release(); -#endif -#ifdef __linux - for (int i = 0; i < 16; i++) { - char buf[128]; - sprintf(buf, "/dev/video%d", i); - if (access(buf, F_OK) == 0) - ret.append(buf); - else - continue; - - if (access(buf, R_OK | W_OK) == 0) { - int fd = open(buf, O_RDONLY); - if (fd == -1) - continue; - struct v4l2_capability video_cap; - if(ioctl(fd, VIDIOC_QUERYCAP, &video_cap) == -1) - { - qDebug() << "VIDIOC_QUERYCAP" << errno; - close(fd); - continue; - } - ret[ret.size()-1] = reinterpret_cast(video_cap.card); - close(fd); - } - } -#endif - return ret; -} \ No newline at end of file diff --git a/opentrack/camera-names.hpp b/opentrack/camera-names.hpp deleted file mode 100644 index ef914458..00000000 --- a/opentrack/camera-names.hpp +++ /dev/null @@ -1,16 +0,0 @@ -/* Copyright (c) 2014-2015, Stanislaw Halik - - * 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. - */ - -#pragma once - -#include -#include - -QList get_camera_names(); -int camera_name_to_index(const QString &name); - diff --git a/opentrack/opencv-camera-dialog.hpp b/opentrack/opencv-camera-dialog.hpp index 0d4a51af..96c7a643 100644 --- a/opentrack/opencv-camera-dialog.hpp +++ b/opentrack/opencv-camera-dialog.hpp @@ -12,7 +12,7 @@ #include #include #include -#include "opentrack/camera-names.hpp" +#include "opentrack-compat/camera-names.hpp" #ifdef __linux #include diff --git a/tracker-aruco/ftnoir_tracker_aruco.cpp b/tracker-aruco/ftnoir_tracker_aruco.cpp index 1da5b8df..316c7e13 100644 --- a/tracker-aruco/ftnoir_tracker_aruco.cpp +++ b/tracker-aruco/ftnoir_tracker_aruco.cpp @@ -16,7 +16,7 @@ #include #include #include -#include "opentrack/camera-names.hpp" +#include "opentrack-compat/camera-names.hpp" #include "opentrack-compat/sleep.hpp" typedef struct { diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp index 2dbf9068..3dd91a45 100644 --- a/tracker-pt/ftnoir_tracker_pt.cpp +++ b/tracker-pt/ftnoir_tracker_pt.cpp @@ -12,7 +12,7 @@ #include #include #include -#include "opentrack/camera-names.hpp" +#include "opentrack-compat/camera-names.hpp" #include "opentrack-compat/sleep.hpp" //#define PT_PERF_LOG //log performance diff --git a/tracker-pt/ftnoir_tracker_pt_dialog.cpp b/tracker-pt/ftnoir_tracker_pt_dialog.cpp index b1ae2238..551910f7 100644 --- a/tracker-pt/ftnoir_tracker_pt_dialog.cpp +++ b/tracker-pt/ftnoir_tracker_pt_dialog.cpp @@ -12,7 +12,7 @@ #include #include #include -#include "opentrack/camera-names.hpp" +#include "opentrack-compat/camera-names.hpp" #include //----------------------------------------------------------------------------- -- cgit v1.2.3 From fd99faebde3361e5acf1dfc4ebebfe4727a47124 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Fri, 18 Dec 2015 19:45:29 +0100 Subject: tracker/pt: move ctor out of the loop --- tracker-pt/camera.cpp | 3 +++ tracker-pt/ftnoir_tracker_pt.cpp | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) (limited to 'tracker-pt/ftnoir_tracker_pt.cpp') diff --git a/tracker-pt/camera.cpp b/tracker-pt/camera.cpp index 63b401a8..ff721557 100644 --- a/tracker-pt/camera.cpp +++ b/tracker-pt/camera.cpp @@ -117,6 +117,9 @@ bool CVCamera::_get_frame(cv::Mat* frame) if (img.empty()) return false; + if (frame->rows != img.rows || frame->cols != img.cols) + *frame = cv::Mat(); + *frame = img; cam_info.res_x = img.cols; cam_info.res_y = img.rows; diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp index 3dd91a45..877b58fd 100644 --- a/tracker-pt/ftnoir_tracker_pt.cpp +++ b/tracker-pt/ftnoir_tracker_pt.cpp @@ -79,12 +79,12 @@ void Tracker_PT::run() #endif apply_settings(); + cv::Mat frame; while((commands & ABORT) == 0) { const double dt = time.elapsed() * 1e-9; time.start(); - cv::Mat frame; bool new_frame; { -- cgit v1.2.3 From 8a60f1c9613d396b50aa6adec4b1f5ffdb9e9806 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Fri, 18 Dec 2015 19:57:42 +0100 Subject: tracker/pt: reduce mutex contention --- tracker-pt/ftnoir_tracker_pt.cpp | 12 ++++-------- tracker-pt/ftnoir_tracker_pt.h | 5 ++--- tracker-pt/point_extractor.cpp | 7 ++++--- tracker-pt/point_extractor.h | 7 ++++--- tracker-pt/point_tracker.cpp | 1 + tracker-pt/point_tracker.h | 6 +++--- 6 files changed, 18 insertions(+), 20 deletions(-) (limited to 'tracker-pt/ftnoir_tracker_pt.cpp') diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp index 877b58fd..80227a39 100644 --- a/tracker-pt/ftnoir_tracker_pt.cpp +++ b/tracker-pt/ftnoir_tracker_pt.cpp @@ -19,8 +19,7 @@ //----------------------------------------------------------------------------- Tracker_PT::Tracker_PT() - : mutex(QMutex::Recursive), - commands(0), + : commands(0), video_widget(NULL), video_frame(NULL), ever_success(false) @@ -94,29 +93,26 @@ void Tracker_PT::run() if (new_frame && !frame.empty()) { - QMutexLocker lock(&mutex); - std::vector points = point_extractor.extract_points(frame); // 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; - float fx; if (!get_focal_length(fx)) continue; + + const bool success = points.size() == PointModel::N_POINTS; if (success) { point_tracker.track(points, PointModel(s), fx, s.dynamic_pose, s.init_phase_timeout); + ever_success = true; } Affine X_CM = pose(); - ever_success |= success; - { 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 Affine X_GH = X_CM * X_MH; diff --git a/tracker-pt/ftnoir_tracker_pt.h b/tracker-pt/ftnoir_tracker_pt.h index f73d106b..5ec7ea91 100644 --- a/tracker-pt/ftnoir_tracker_pt.h +++ b/tracker-pt/ftnoir_tracker_pt.h @@ -40,15 +40,14 @@ public: void start_tracker(QFrame* parent_window) override; void data(double* data) override; - Affine pose() { QMutexLocker lock(&mutex); return point_tracker.pose(); } - int get_n_points() { QMutexLocker lock(&mutex); return point_extractor.get_points().size(); } + Affine pose() { return point_tracker.pose(); } + int get_n_points() { return point_extractor.get_points().size(); } bool get_cam_info(CamInfo* info) { QMutexLocker lock(&camera_mtx); return camera.get_info(*info); } public slots: void apply_settings(); protected: void run() override; private: - QMutex mutex; // thread commands enum Command { ABORT = 1<<0 diff --git a/tracker-pt/point_extractor.cpp b/tracker-pt/point_extractor.cpp index dd36c57f..6eb255a5 100644 --- a/tracker-pt/point_extractor.cpp +++ b/tracker-pt/point_extractor.cpp @@ -147,12 +147,13 @@ std::vector PointExtractor::extract_points(cv::Mat& frame) blobs.push_back(blob(radius, pos, confid, area)); } - // clear old points - points.clear(); - using b = const blob; std::sort(blobs.begin(), blobs.end(), [](b& b1, b& b2) {return b1.confid > b2.confid;}); + QMutexLocker l(&mtx); + + points.clear(); + for (auto& b : blobs) { cv::Vec2f p((b.pos[0] - W/2)/W, -(b.pos[1] - H/2)/W); diff --git a/tracker-pt/point_extractor.h b/tracker-pt/point_extractor.h index b9368ab6..80c1897d 100644 --- a/tracker-pt/point_extractor.h +++ b/tracker-pt/point_extractor.h @@ -13,8 +13,8 @@ #include "ftnoir_tracker_pt_settings.h" -// ---------------------------------------------------------------------------- -// Extracts points from an opencv image +#include + class PointExtractor { public: @@ -22,12 +22,13 @@ public: // dt: time since last call in seconds // WARNING: returned reference is valid as long as object std::vector extract_points(cv::Mat &frame); - const std::vector& get_points() { return points; } + const std::vector& get_points() { QMutexLocker l(&mtx); return points; } PointExtractor(); settings_pt s; private: std::vector points; + QMutex mtx; }; #endif //POINTEXTRACTOR_H diff --git a/tracker-pt/point_tracker.cpp b/tracker-pt/point_tracker.cpp index 924b75de..aa6feb5b 100644 --- a/tracker-pt/point_tracker.cpp +++ b/tracker-pt/point_tracker.cpp @@ -249,6 +249,7 @@ int PointTracker::POSIT(const PointModel& model, const PointOrder& order_, float old_epsilon_2 = epsilon_2; } + QMutexLocker l(&mtx); // apply results X_CM.R = *R_current; X_CM.t[0] = order[0][0] * Z0/focal_length; diff --git a/tracker-pt/point_tracker.h b/tracker-pt/point_tracker.h index cdcf2998..2757f22c 100644 --- a/tracker-pt/point_tracker.h +++ b/tracker-pt/point_tracker.h @@ -15,9 +15,8 @@ #include "ftnoir_tracker_pt_settings.h" #include +#include -// ---------------------------------------------------------------------------- -// Affine frame trafo class Affine { public: @@ -116,7 +115,7 @@ public: // f : (focal length)/(sensor width) // dt : time since last call void track(const std::vector& projected_points, const PointModel& model, float f, bool dynamic_pose, int init_phase_timeout); - Affine pose() const { return X_CM; } + Affine pose() { QMutexLocker l(&mtx); return X_CM; } cv::Vec2f project(const cv::Vec3f& v_M, float f); private: // the points in model order @@ -138,6 +137,7 @@ private: Timer t; bool init_phase; + QMutex mtx; }; #endif //POINTTRACKER_H -- cgit v1.2.3 From 90138a999b4c95afeb9a49d355b0234b6145e221 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sat, 19 Dec 2015 20:43:24 +0100 Subject: tracker/pt: don't allocate temporary frame --- tracker-pt/camera.cpp | 13 ++++--------- tracker-pt/ftnoir_tracker_pt.cpp | 14 +++++++------- tracker-pt/ftnoir_tracker_pt.h | 1 + 3 files changed, 12 insertions(+), 16 deletions(-) (limited to 'tracker-pt/ftnoir_tracker_pt.cpp') diff --git a/tracker-pt/camera.cpp b/tracker-pt/camera.cpp index ff721557..600ab26a 100644 --- a/tracker-pt/camera.cpp +++ b/tracker-pt/camera.cpp @@ -110,19 +110,14 @@ bool CVCamera::_get_frame(cv::Mat* frame) { if (cap && cap->isOpened()) { - cv::Mat img; - for (int i = 0; i < 100 && !cap->read(img); i++) + for (int i = 0; i < 100 && !cap->read(*frame); i++) ;; - if (img.empty()) + if (frame->empty()) return false; - if (frame->rows != img.rows || frame->cols != img.cols) - *frame = cv::Mat(); - - *frame = img; - cam_info.res_x = img.cols; - cam_info.res_y = img.rows; + cam_info.res_x = frame->cols; + cam_info.res_y = frame->rows; return true; } return false; diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp index 80227a39..9a4fa037 100644 --- a/tracker-pt/ftnoir_tracker_pt.cpp +++ b/tracker-pt/ftnoir_tracker_pt.cpp @@ -14,6 +14,7 @@ #include #include "opentrack-compat/camera-names.hpp" #include "opentrack-compat/sleep.hpp" +#include //#define PT_PERF_LOG //log performance @@ -78,7 +79,7 @@ void Tracker_PT::run() #endif apply_settings(); - cv::Mat frame; + cv::Mat frame_; while((commands & ABORT) == 0) { @@ -89,9 +90,12 @@ void Tracker_PT::run() { 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); + frame.copyTo(frame_); } - if (new_frame && !frame.empty()) + if (new_frame && !frame_.empty()) { std::vector points = point_extractor.extract_points(frame); @@ -142,11 +146,6 @@ void Tracker_PT::run() video_widget->update_image(frame); } -#ifdef PT_PERF_LOG - log_stream<<"dt: "< Date: Sat, 19 Dec 2015 20:44:41 +0100 Subject: tracker/pt: don't copy points array needlessly --- tracker-pt/ftnoir_tracker_pt.cpp | 47 +++++++++++++++++++--------------------- tracker-pt/point_extractor.cpp | 13 +++++++++-- tracker-pt/point_extractor.h | 2 +- 3 files changed, 34 insertions(+), 28 deletions(-) (limited to 'tracker-pt/ftnoir_tracker_pt.cpp') diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp index 9a4fa037..88356771 100644 --- a/tracker-pt/ftnoir_tracker_pt.cpp +++ b/tracker-pt/ftnoir_tracker_pt.cpp @@ -97,17 +97,13 @@ void Tracker_PT::run() if (new_frame && !frame_.empty()) { - std::vector points = point_extractor.extract_points(frame); - - // blobs are sorted in order of circularity - if (points.size() > PointModel::N_POINTS) - points.resize(PointModel::N_POINTS); + const auto& points = point_extractor.extract_points(frame_); float fx; if (!get_focal_length(fx)) continue; - const bool success = points.size() == PointModel::N_POINTS; + const bool success = points.size() >= PointModel::N_POINTS; if (success) { @@ -116,35 +112,36 @@ void Tracker_PT::run() } 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 - Affine X_GH = X_CM * X_MH; - cv::Vec3f p = X_GH.t; // head (center?) position in global space - 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++) + + std::function fun = [&](const cv::Vec2f& p, const cv::Scalar color) { - auto& p = points[i]; - auto p2 = cv::Point(p[0] * frame.cols + frame.cols/2, -p[1] * frame.cols + frame.rows/2); - cv::Scalar color(0, 255, 0); - if (i == points.size()-1) - color = cv::Scalar(0, 0, 255); - cv::line(frame, + auto p2 = cv::Point(p[0] * frame_.cols + frame_.cols/2, -p[1] * frame_.cols + frame_.rows/2); + cv::line(frame_, cv::Point(p2.x - 20, p2.y), cv::Point(p2.x + 20, p2.y), color, 4); - cv::line(frame, + cv::line(frame_, cv::Point(p2.x, p2.y - 20), cv::Point(p2.x, p2.y + 20), color, - 4); + 4); + }; + + for (unsigned i = 0; i < points.size(); i++) + { + fun(points[i], cv::Scalar(0, 255, 0)); + } + + { + 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 + Affine X_GH = X_CM * X_MH; + cv::Vec3f p = X_GH.t; // head (center?) position in global space + cv::Vec2f p_(p[0] / p[2] * fx, p[1] / p[2] * fx); // projected to screen + fun(p_, cv::Scalar(0, 0, 255)); } - video_widget->update_image(frame); + video_widget->update_image(frame_); } } qDebug()<<"Tracker:: Thread stopping"; diff --git a/tracker-pt/point_extractor.cpp b/tracker-pt/point_extractor.cpp index 6118f3d2..3808c408 100644 --- a/tracker-pt/point_extractor.cpp +++ b/tracker-pt/point_extractor.cpp @@ -16,10 +16,17 @@ PointExtractor::PointExtractor() { } -std::vector PointExtractor::extract_points(cv::Mat& frame) + +const std::vector& PointExtractor::extract_points(cv::Mat& frame) { 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); + frame_bin = cv::Mat(frame.rows, frame.cols, CV_8U);; + } // convert to grayscale cv::Mat frame_gray; @@ -151,9 +158,11 @@ std::vector PointExtractor::extract_points(cv::Mat& frame) using b = const blob; std::sort(blobs.begin(), blobs.end(), [](b& b1, b& b2) {return b1.confid > b2.confid;}); + points.reserve(blobs.size()); + QMutexLocker l(&mtx); - points.clear(); + points.clear(); for (auto& b : blobs) { diff --git a/tracker-pt/point_extractor.h b/tracker-pt/point_extractor.h index 80c1897d..979cc8b6 100644 --- a/tracker-pt/point_extractor.h +++ b/tracker-pt/point_extractor.h @@ -21,7 +21,7 @@ public: // extracts points from frame and draws some processing info into frame, if draw_output is set // dt: time since last call in seconds // WARNING: returned reference is valid as long as object - std::vector extract_points(cv::Mat &frame); + const std::vector &extract_points(cv::Mat &frame); const std::vector& get_points() { QMutexLocker l(&mtx); return points; } PointExtractor(); -- cgit v1.2.3