diff options
author | Stéphane Lenclud <github@lenclud.com> | 2019-04-13 12:59:22 +0200 |
---|---|---|
committer | Stéphane Lenclud <github@lenclud.com> | 2019-04-13 12:59:22 +0200 |
commit | 2a90f867d29f7080159a502230535fa397cb2adf (patch) | |
tree | d269bc7228f64474daf6fe5a9318696013d815f8 | |
parent | 4e79bfa83a92fd14ab7f453ac786b682079a0086 (diff) |
EasyTracker: Adding namespace. Reducing number of classes.
-rw-r--r-- | tracker-easy/cv-point-extractor.cpp | 120 | ||||
-rw-r--r-- | tracker-easy/cv-point-extractor.h | 21 | ||||
-rw-r--r-- | tracker-easy/lang/nl_NL.ts | 16 | ||||
-rw-r--r-- | tracker-easy/lang/ru_RU.ts | 16 | ||||
-rw-r--r-- | tracker-easy/lang/stub.ts | 16 | ||||
-rw-r--r-- | tracker-easy/lang/zh_CN.ts | 16 | ||||
-rw-r--r-- | tracker-easy/module.cpp | 43 | ||||
-rw-r--r-- | tracker-easy/module.hpp | 14 | ||||
-rw-r--r-- | tracker-easy/preview.cpp | 142 | ||||
-rw-r--r-- | tracker-easy/preview.h | 30 | ||||
-rw-r--r-- | tracker-easy/tracker-easy-api.cpp | 10 | ||||
-rw-r--r-- | tracker-easy/tracker-easy-dialog.cpp | 384 | ||||
-rw-r--r-- | tracker-easy/tracker-easy-dialog.h | 64 | ||||
-rw-r--r-- | tracker-easy/tracker-easy.cpp | 564 | ||||
-rw-r--r-- | tracker-easy/tracker-easy.h | 94 |
15 files changed, 765 insertions, 785 deletions
diff --git a/tracker-easy/cv-point-extractor.cpp b/tracker-easy/cv-point-extractor.cpp index 56de4c0b..9720eb63 100644 --- a/tracker-easy/cv-point-extractor.cpp +++ b/tracker-easy/cv-point-extractor.cpp @@ -8,6 +8,7 @@ #include "cv-point-extractor.h" #include "preview.h" +#include "tracker-easy.h" #include "cv/numeric.hpp" #include "compat/math.hpp" @@ -22,87 +23,86 @@ using namespace numeric_types; - - -CvPointExtractor::CvPointExtractor(const QString& module_name) : s(module_name) +namespace EasyTracker { + + CvPointExtractor::CvPointExtractor() : s(KModuleName) + { -} + } -void CvPointExtractor::extract_points(const cv::Mat& frame, cv::Mat* aPreview, std::vector<vec2>& aPoints) -{ + void CvPointExtractor::extract_points(const cv::Mat& frame, cv::Mat* aPreview, std::vector<vec2>& aPoints) + { - // Contours detection - std::vector<std::vector<cv::Point> > contours; - cv::findContours(frame, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); + // Contours detection + std::vector<std::vector<cv::Point> > contours; + cv::findContours(frame, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); - // Workout which countours are valid points - for (size_t i = 0; i < contours.size(); i++) - { - if (aPreview) + // Workout which countours are valid points + for (size_t i = 0; i < contours.size(); i++) { - cv::drawContours(*aPreview, contours, i, CV_RGB(255, 0, 0), 2); - } + if (aPreview) + { + cv::drawContours(*aPreview, contours, i, CV_RGB(255, 0, 0), 2); + } - cv::Rect bBox; - bBox = cv::boundingRect(contours[i]); + cv::Rect bBox; + bBox = cv::boundingRect(contours[i]); - float ratio = (float)bBox.width / (float)bBox.height; - if (ratio > 1.0f) - ratio = 1.0f / ratio; + float ratio = (float)bBox.width / (float)bBox.height; + if (ratio > 1.0f) + ratio = 1.0f / ratio; - // Searching for a bBox almost square - float minArea = s.min_point_size*s.min_point_size; - float maxArea = s.max_point_size*s.max_point_size; - if (bBox.width >= s.min_point_size - && bBox.height >= s.min_point_size - && bBox.width <= s.max_point_size - && bBox.height <= s.max_point_size - && bBox.area() >= minArea - && bBox.area() <= maxArea - /*&& ratio > 0.75 &&*/) - { - vec2 center; - center[0] = bBox.x + bBox.width / 2; - center[1] = bBox.y + bBox.height / 2; - aPoints.push_back(vec2(center)); - - if (aPreview) + // Searching for a bBox almost square + float minArea = s.min_point_size*s.min_point_size; + float maxArea = s.max_point_size*s.max_point_size; + if (bBox.width >= s.min_point_size + && bBox.height >= s.min_point_size + && bBox.width <= s.max_point_size + && bBox.height <= s.max_point_size + && bBox.area() >= minArea + && bBox.area() <= maxArea + /*&& ratio > 0.75 &&*/) { - cv::rectangle(*aPreview, bBox, CV_RGB(0, 255, 0), 2); + vec2 center; + center[0] = bBox.x + bBox.width / 2; + center[1] = bBox.y + bBox.height / 2; + aPoints.push_back(vec2(center)); + + if (aPreview) + { + cv::rectangle(*aPreview, bBox, CV_RGB(0, 255, 0), 2); + } } } - } - - // Keep the three points which are highest, i.e. with lowest Y coordinates - // That's most usefull to discard noise from features below your cap/head. - // Typically noise comming from zippers and metal parts on your clothing. - // With a cap tracker it also successfully discards noise glasses. - // However it may not work as good with a clip user wearing glasses. - while (aPoints.size() > 3) // Until we have no more than three points - { - int maxY = 0; - int index = -1; - // Search for the point with highest Y coordinate - for (size_t i = 0; i < aPoints.size(); i++) + // Keep the three points which are highest, i.e. with lowest Y coordinates + // That's most usefull to discard noise from features below your cap/head. + // Typically noise comming from zippers and metal parts on your clothing. + // With a cap tracker it also successfully discards noise glasses. + // However it may not work as good with a clip user wearing glasses. + while (aPoints.size() > 3) // Until we have no more than three points { - if (aPoints[i][1] > maxY) + int maxY = 0; + int index = -1; + + // Search for the point with highest Y coordinate + for (size_t i = 0; i < aPoints.size(); i++) { - maxY = aPoints[i][1]; - index = i; + if (aPoints[i][1] > maxY) + { + maxY = aPoints[i][1]; + index = i; + } } - } - // Discard it - aPoints.erase(aPoints.begin() + index); + // Discard it + aPoints.erase(aPoints.begin() + index); + } } - } - - diff --git a/tracker-easy/cv-point-extractor.h b/tracker-easy/cv-point-extractor.h index 405ca052..18229d4a 100644 --- a/tracker-easy/cv-point-extractor.h +++ b/tracker-easy/cv-point-extractor.h @@ -18,17 +18,20 @@ using namespace numeric_types; - -class CvPointExtractor final : public IPointExtractor +namespace EasyTracker { -public: - // extracts points from frame and draws some processing info into frame, if draw_output is set - // dt: time since last call in seconds - void extract_points(const cv::Mat& frame, cv::Mat* aPreview, std::vector<vec2>& aPoints) override; - CvPointExtractor(const QString& module_name); - pt_settings s; -}; + class CvPointExtractor final : public IPointExtractor + { + public: + // extracts points from frame and draws some processing info into frame, if draw_output is set + // dt: time since last call in seconds + void extract_points(const cv::Mat& frame, cv::Mat* aPreview, std::vector<vec2>& aPoints) override; + CvPointExtractor(); + + pt_settings s; + }; +} diff --git a/tracker-easy/lang/nl_NL.ts b/tracker-easy/lang/nl_NL.ts index 86022183..b10c7e14 100644 --- a/tracker-easy/lang/nl_NL.ts +++ b/tracker-easy/lang/nl_NL.ts @@ -2,7 +2,7 @@ <!DOCTYPE TS> <TS version="2.1" language="nl_NL"> <context> - <name>EasyTrackerDialog</name> + <name>EasyTracker::Dialog</name> <message> <source>Brightness %1/255</source> <translation type="unfinished"></translation> @@ -37,6 +37,13 @@ </message> </context> <context> + <name>EasyTracker::Metadata</name> + <message> + <source>Easy Tracker 0.1</source> + <translation type="unfinished"></translation> + </message> +</context> +<context> <name>UICPTClientControls</name> <message> <source>PointTracker Settings</source> @@ -268,11 +275,4 @@ Don't roll or change position.</source> <translation type="unfinished"></translation> </message> </context> -<context> - <name>pt_module::metadata_pt</name> - <message> - <source>Easy Tracker 0.1</source> - <translation type="unfinished"></translation> - </message> -</context> </TS> diff --git a/tracker-easy/lang/ru_RU.ts b/tracker-easy/lang/ru_RU.ts index 8ed38fac..7f5df2e2 100644 --- a/tracker-easy/lang/ru_RU.ts +++ b/tracker-easy/lang/ru_RU.ts @@ -2,7 +2,7 @@ <!DOCTYPE TS> <TS version="2.1" language="ru_RU"> <context> - <name>EasyTrackerDialog</name> + <name>EasyTracker::Dialog</name> <message> <source>Brightness %1/255</source> <translation type="unfinished"></translation> @@ -37,6 +37,13 @@ </message> </context> <context> + <name>EasyTracker::Metadata</name> + <message> + <source>Easy Tracker 0.1</source> + <translation type="unfinished"></translation> + </message> +</context> +<context> <name>UICPTClientControls</name> <message> <source>PointTracker Settings</source> @@ -273,11 +280,4 @@ ROLL или X/Y-смещения.</translation> <translation type="unfinished"></translation> </message> </context> -<context> - <name>pt_module::metadata_pt</name> - <message> - <source>Easy Tracker 0.1</source> - <translation type="unfinished"></translation> - </message> -</context> </TS> diff --git a/tracker-easy/lang/stub.ts b/tracker-easy/lang/stub.ts index 98e2e0f8..144b8e76 100644 --- a/tracker-easy/lang/stub.ts +++ b/tracker-easy/lang/stub.ts @@ -2,7 +2,7 @@ <!DOCTYPE TS> <TS version="2.1"> <context> - <name>EasyTrackerDialog</name> + <name>EasyTracker::Dialog</name> <message> <source>Brightness %1/255</source> <translation type="unfinished"></translation> @@ -37,6 +37,13 @@ </message> </context> <context> + <name>EasyTracker::Metadata</name> + <message> + <source>Easy Tracker 0.1</source> + <translation type="unfinished"></translation> + </message> +</context> +<context> <name>UICPTClientControls</name> <message> <source>PointTracker Settings</source> @@ -268,11 +275,4 @@ Don't roll or change position.</source> <translation type="unfinished"></translation> </message> </context> -<context> - <name>pt_module::metadata_pt</name> - <message> - <source>Easy Tracker 0.1</source> - <translation type="unfinished"></translation> - </message> -</context> </TS> diff --git a/tracker-easy/lang/zh_CN.ts b/tracker-easy/lang/zh_CN.ts index c2c3024a..43e121ac 100644 --- a/tracker-easy/lang/zh_CN.ts +++ b/tracker-easy/lang/zh_CN.ts @@ -2,7 +2,7 @@ <!DOCTYPE TS> <TS version="2.1" language="zh_CN"> <context> - <name>EasyTrackerDialog</name> + <name>EasyTracker::Dialog</name> <message> <source>Brightness %1/255</source> <translation type="unfinished">亮度 %1/255</translation> @@ -37,6 +37,13 @@ </message> </context> <context> + <name>EasyTracker::Metadata</name> + <message> + <source>Easy Tracker 0.1</source> + <translation type="unfinished"></translation> + </message> +</context> +<context> <name>UICPTClientControls</name> <message> <source>PointTracker Settings</source> @@ -268,11 +275,4 @@ Don't roll or change position.</source> <translation type="unfinished"></translation> </message> </context> -<context> - <name>pt_module::metadata_pt</name> - <message> - <source>Easy Tracker 0.1</source> - <translation type="unfinished"></translation> - </message> -</context> </TS> diff --git a/tracker-easy/module.cpp b/tracker-easy/module.cpp index 0c360e80..c5751e6e 100644 --- a/tracker-easy/module.cpp +++ b/tracker-easy/module.cpp @@ -4,52 +4,17 @@ #include "module.hpp" #include "cv-point-extractor.h" - #include <memory> -static const QString module_name = "tracker-easy"; - -#ifdef __clang__ -# pragma clang diagnostic ignored "-Wweak-vtables" -#endif - -namespace pt_module { - -struct pt_module_traits final : IEasyTrackerTraits -{ - pointer<IPointExtractor> make_point_extractor() const override - { - return pointer<IPointExtractor>(new CvPointExtractor(module_name)); - } - - QString get_module_name() const override - { - return module_name; - } - -}; -struct tracker_pt : EasyTracker -{ - tracker_pt() : EasyTracker(pointer<IEasyTrackerTraits>(new pt_module_traits)) - { - } -}; -struct dialog_pt : EasyTrackerDialog +namespace EasyTracker { - dialog_pt(); -}; -dialog_pt::dialog_pt() : EasyTrackerDialog(module_name) {} - -QString metadata_pt::name() { return tr("Easy Tracker 0.1"); } -QIcon metadata_pt::icon() { return QIcon(":/Resources/Logo_IR.png"); } + QString Metadata::name() { return tr("Easy Tracker 0.1"); } + QIcon Metadata::icon() { return QIcon(":/Resources/Logo_IR.png"); } } -// ns pt_module - -using namespace pt_module; -OPENTRACK_DECLARE_TRACKER(tracker_pt, dialog_pt, metadata_pt) +OPENTRACK_DECLARE_TRACKER(EasyTracker::Tracker, EasyTracker::Dialog, EasyTracker::Metadata) diff --git a/tracker-easy/module.hpp b/tracker-easy/module.hpp index 0b3f12cf..8a4e3920 100644 --- a/tracker-easy/module.hpp +++ b/tracker-easy/module.hpp @@ -6,15 +6,15 @@ #include "compat/linkage-macros.hpp" -namespace pt_module +namespace EasyTracker { -class OTR_GENERIC_EXPORT metadata_pt : public Metadata -{ - Q_OBJECT + class OTR_GENERIC_EXPORT Metadata : public ::Metadata + { + Q_OBJECT - QString name() override; - QIcon icon() override; -}; + QString name() override; + QIcon icon() override; + }; } // ns pt_module diff --git a/tracker-easy/preview.cpp b/tracker-easy/preview.cpp index 33758cfa..404ad299 100644 --- a/tracker-easy/preview.cpp +++ b/tracker-easy/preview.cpp @@ -13,88 +13,94 @@ #include <opencv2/imgproc.hpp> -Preview& Preview::operator=(const cv::Mat& aFrame) +namespace EasyTracker { - // Make sure our frame is RGB - // Make an extra copy if needed - int channelCount = aFrame.channels(); - if (channelCount == 1) - { - // Convert to RGB - cv::cvtColor(aFrame, iFrameRgb, cv::COLOR_GRAY2BGR); - } - else if (channelCount == 3) - { - iFrameRgb = aFrame; - } - else - { - eval_once(qDebug() << "tracker/easy: camera frame depth not supported" << aFrame.channels()); - return *this; - } - - return *this; -} + Preview& Preview::operator=(const cv::Mat& aFrame) + { -Preview::Preview(int w, int h) -{ - ensure_size(frame_out, w, h, CV_8UC4); - ensure_size(iFrameResized, w, h, CV_8UC3); + // Make sure our frame is RGB + // Make an extra copy if needed + int channelCount = aFrame.channels(); + if (channelCount == 1) + { + // Convert to RGB + cv::cvtColor(aFrame, iFrameRgb, cv::COLOR_GRAY2BGR); + } + else if (channelCount == 3) + { + iFrameRgb = aFrame; + } + else + { + eval_once(qDebug() << "tracker/easy: camera frame depth not supported" << aFrame.channels()); + return *this; + } - iFrameResized.setTo(cv::Scalar(0, 0, 0)); -} -QImage Preview::get_bitmap() -{ - int stride = frame_out.step.p[0]; + return *this; + } - if (stride < 64 || stride < frame_out.cols * 4) + Preview::Preview(int w, int h) { - eval_once(qDebug() << "bad stride" << stride - << "for bitmap size" << iFrameResized.cols << iFrameResized.rows); - return QImage(); + ensure_size(frame_out, w, h, CV_8UC4); + ensure_size(iFrameResized, w, h, CV_8UC3); + + iFrameResized.setTo(cv::Scalar(0, 0, 0)); } - // Resize if needed - const bool need_resize = iFrameRgb.cols != frame_out.cols || iFrameRgb.rows != frame_out.rows; - if (need_resize) + QImage Preview::get_bitmap() { - cv::resize(iFrameRgb, iFrameResized, cv::Size(frame_out.cols, frame_out.rows), 0, 0, cv::INTER_NEAREST); + int stride = frame_out.step.p[0]; + + if (stride < 64 || stride < frame_out.cols * 4) + { + eval_once(qDebug() << "bad stride" << stride + << "for bitmap size" << iFrameResized.cols << iFrameResized.rows); + return QImage(); + } + + // Resize if needed + const bool need_resize = iFrameRgb.cols != frame_out.cols || iFrameRgb.rows != frame_out.rows; + if (need_resize) + { + cv::resize(iFrameRgb, iFrameResized, cv::Size(frame_out.cols, frame_out.rows), 0, 0, cv::INTER_NEAREST); + } + else + { + iFrameRgb.copyTo(iFrameResized); + } + + cv::cvtColor(iFrameResized, frame_out, cv::COLOR_BGR2BGRA); + + return QImage((const unsigned char*)frame_out.data, + frame_out.cols, frame_out.rows, + stride, + QImage::Format_ARGB32); } - else + + void Preview::draw_head_center(numeric_types::f x, numeric_types::f y) { - iFrameRgb.copyTo(iFrameResized); + int px = iround(x), py = iround(y); + + constexpr int len = 9; + + static const cv::Scalar color(0, 255, 255); + cv::line(iFrameRgb, + cv::Point(px - len, py), + cv::Point(px + len, py), + color, 1); + cv::line(iFrameRgb, + cv::Point(px, py - len), + cv::Point(px, py + len), + color, 1); } - cv::cvtColor(iFrameResized, frame_out, cv::COLOR_BGR2BGRA); - - return QImage((const unsigned char*) frame_out.data, - frame_out.cols, frame_out.rows, - stride, - QImage::Format_ARGB32); -} - -void Preview::draw_head_center(numeric_types::f x, numeric_types::f y) -{ - int px = iround(x), py = iround(y); - - constexpr int len = 9; - - static const cv::Scalar color(0, 255, 255); - cv::line(iFrameRgb, - cv::Point(px - len, py), - cv::Point(px + len, py), - color, 1); - cv::line(iFrameRgb, - cv::Point(px, py - len), - cv::Point(px, py + len), - color, 1); -} + void Preview::ensure_size(cv::Mat& frame, int w, int h, int type) + { + if (frame.cols != w || frame.rows != h) + frame = cv::Mat(h, w, type); + } -void Preview::ensure_size(cv::Mat& frame, int w, int h, int type) -{ - if (frame.cols != w || frame.rows != h) - frame = cv::Mat(h, w, type); } diff --git a/tracker-easy/preview.h b/tracker-easy/preview.h index fc7ec9c9..c3ed9f6b 100644 --- a/tracker-easy/preview.h +++ b/tracker-easy/preview.h @@ -14,23 +14,27 @@ #include <opencv2/core.hpp> #include <QImage> -struct Preview +namespace EasyTracker { - Preview(int w, int h); - Preview& operator=(const cv::Mat& frame); - QImage get_bitmap(); - void draw_head_center(numeric_types::f x, numeric_types::f y); + struct Preview + { + Preview(int w, int h); - operator cv::Mat&() { return iFrameResized; } - operator cv::Mat const&() const { return iFrameResized; } + Preview& operator=(const cv::Mat& frame); + QImage get_bitmap(); + void draw_head_center(numeric_types::f x, numeric_types::f y); -private: - static void ensure_size(cv::Mat& frame, int w, int h, int type); + operator cv::Mat&() { return iFrameResized; } + operator cv::Mat const&() const { return iFrameResized; } -public: - cv::Mat iFrameResized, frame_out; - cv::Mat iFrameRgb; -}; + private: + static void ensure_size(cv::Mat& frame, int w, int h, int type); + public: + cv::Mat iFrameResized, frame_out; + cv::Mat iFrameRgb; + }; + +} diff --git a/tracker-easy/tracker-easy-api.cpp b/tracker-easy/tracker-easy-api.cpp deleted file mode 100644 index 2e3988a8..00000000 --- a/tracker-easy/tracker-easy-api.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "tracker-easy-api.h" -#include "cv/numeric.hpp" - -using namespace numeric_types; - - -IEasyTrackerTraits::IEasyTrackerTraits() = default; -IEasyTrackerTraits::~IEasyTrackerTraits() = default; - - diff --git a/tracker-easy/tracker-easy-dialog.cpp b/tracker-easy/tracker-easy-dialog.cpp index c0dd23dd..0b0dd61d 100644 --- a/tracker-easy/tracker-easy-dialog.cpp +++ b/tracker-easy/tracker-easy-dialog.cpp @@ -20,249 +20,253 @@ using namespace options; static void init_resources() { Q_INIT_RESOURCE(tracker_easy); } -EasyTrackerDialog::EasyTrackerDialog(const QString& module_name) : - s(module_name), - tracker(nullptr), - timer(this), - trans_calib(1, 2) +namespace EasyTracker { - init_resources(); - ui.setupUi(this); + Dialog::Dialog() : + s(KModuleName), + tracker(nullptr), + timer(this), + trans_calib(1, 2) + { + init_resources(); - for (const QString& str : video::camera_names()) - ui.camdevice_combo->addItem(str); + ui.setupUi(this); - tie_setting(s.camera_name, ui.camdevice_combo); - tie_setting(s.cam_res_x, ui.res_x_spin); - tie_setting(s.cam_res_y, ui.res_y_spin); - tie_setting(s.cam_fps, ui.fps_spin); + for (const QString& str : video::camera_names()) + ui.camdevice_combo->addItem(str); - tie_setting(s.threshold_slider, ui.threshold_slider); + tie_setting(s.camera_name, ui.camdevice_combo); + tie_setting(s.cam_res_x, ui.res_x_spin); + tie_setting(s.cam_res_y, ui.res_y_spin); + tie_setting(s.cam_fps, ui.fps_spin); - tie_setting(s.min_point_size, ui.mindiam_spin); - tie_setting(s.max_point_size, ui.maxdiam_spin); + tie_setting(s.threshold_slider, ui.threshold_slider); - tie_setting(s.clip_by, ui.clip_bheight_spin); - tie_setting(s.clip_bz, ui.clip_blength_spin); - tie_setting(s.clip_ty, ui.clip_theight_spin); - tie_setting(s.clip_tz, ui.clip_tlength_spin); + tie_setting(s.min_point_size, ui.mindiam_spin); + tie_setting(s.max_point_size, ui.maxdiam_spin); - tie_setting(s.cap_x, ui.cap_width_spin); - tie_setting(s.cap_y, ui.cap_height_spin); - tie_setting(s.cap_z, ui.cap_length_spin); + tie_setting(s.clip_by, ui.clip_bheight_spin); + tie_setting(s.clip_bz, ui.clip_blength_spin); + tie_setting(s.clip_ty, ui.clip_theight_spin); + tie_setting(s.clip_tz, ui.clip_tlength_spin); - tie_setting(s.m01_x, ui.m1x_spin); - tie_setting(s.m01_y, ui.m1y_spin); - tie_setting(s.m01_z, ui.m1z_spin); + tie_setting(s.cap_x, ui.cap_width_spin); + tie_setting(s.cap_y, ui.cap_height_spin); + tie_setting(s.cap_z, ui.cap_length_spin); - tie_setting(s.m02_x, ui.m2x_spin); - tie_setting(s.m02_y, ui.m2y_spin); - tie_setting(s.m02_z, ui.m2z_spin); + tie_setting(s.m01_x, ui.m1x_spin); + tie_setting(s.m01_y, ui.m1y_spin); + tie_setting(s.m01_z, ui.m1z_spin); - tie_setting(s.t_MH_x, ui.tx_spin); - tie_setting(s.t_MH_y, ui.ty_spin); - tie_setting(s.t_MH_z, ui.tz_spin); + tie_setting(s.m02_x, ui.m2x_spin); + tie_setting(s.m02_y, ui.m2y_spin); + tie_setting(s.m02_z, ui.m2z_spin); - tie_setting(s.fov, ui.fov); + tie_setting(s.t_MH_x, ui.tx_spin); + tie_setting(s.t_MH_y, ui.ty_spin); + tie_setting(s.t_MH_z, ui.tz_spin); - tie_setting(s.active_model_panel, ui.model_tabs); + tie_setting(s.fov, ui.fov); - tie_setting(s.dynamic_pose, ui.dynamic_pose); - tie_setting(s.init_phase_timeout, ui.init_phase_timeout); + tie_setting(s.active_model_panel, ui.model_tabs); - tie_setting(s.auto_threshold, ui.auto_threshold); + tie_setting(s.dynamic_pose, ui.dynamic_pose); + tie_setting(s.init_phase_timeout, ui.init_phase_timeout); - connect(ui.tcalib_button,SIGNAL(toggled(bool)), this, SLOT(startstop_trans_calib(bool))); + tie_setting(s.auto_threshold, ui.auto_threshold); - connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); - connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); + connect(ui.tcalib_button, SIGNAL(toggled(bool)), this, SLOT(startstop_trans_calib(bool))); - connect(ui.camdevice_combo, &QComboBox::currentTextChanged, this, &EasyTrackerDialog::set_camera_settings_available); - set_camera_settings_available(ui.camdevice_combo->currentText()); - connect(ui.camera_settings, &QPushButton::clicked, this, &EasyTrackerDialog::show_camera_settings); + connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); + connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); - connect(&timer, &QTimer::timeout, this, &EasyTrackerDialog::poll_tracker_info_impl); - timer.setInterval(250); + connect(ui.camdevice_combo, &QComboBox::currentTextChanged, this, &Dialog::set_camera_settings_available); + set_camera_settings_available(ui.camdevice_combo->currentText()); + connect(ui.camera_settings, &QPushButton::clicked, this, &Dialog::show_camera_settings); - connect(&calib_timer, &QTimer::timeout, this, &EasyTrackerDialog::trans_calib_step); - calib_timer.setInterval(35); + connect(&timer, &QTimer::timeout, this, &Dialog::poll_tracker_info_impl); + timer.setInterval(250); - poll_tracker_info_impl(); + connect(&calib_timer, &QTimer::timeout, this, &Dialog::trans_calib_step); + calib_timer.setInterval(35); - connect(this, &EasyTrackerDialog::poll_tracker_info, this, &EasyTrackerDialog::poll_tracker_info_impl, Qt::DirectConnection); + poll_tracker_info_impl(); - constexpr pt_color_type color_types[] = { - pt_color_average, - pt_color_natural, - pt_color_red_only, - pt_color_green_only, - pt_color_blue_only, - }; + connect(this, &Dialog::poll_tracker_info, this, &Dialog::poll_tracker_info_impl, Qt::DirectConnection); - for (unsigned k = 0; k < std::size(color_types); k++) - ui.blob_color->setItemData(k, int(color_types[k])); + constexpr pt_color_type color_types[] = { + pt_color_average, + pt_color_natural, + pt_color_red_only, + pt_color_green_only, + pt_color_blue_only, + }; - tie_setting(s.blob_color, ui.blob_color); + for (unsigned k = 0; k < std::size(color_types); k++) + ui.blob_color->setItemData(k, int(color_types[k])); - tie_setting(s.threshold_slider, ui.threshold_value_display, [this](const slider_value& val) { - return threshold_display_text(int(val)); - }); + tie_setting(s.blob_color, ui.blob_color); - // refresh threshold display on auto-threshold checkbox state change - tie_setting(s.auto_threshold, - this, - [this](bool) { s.threshold_slider.notify(); }); -} + tie_setting(s.threshold_slider, ui.threshold_value_display, [this](const slider_value& val) { + return threshold_display_text(int(val)); + }); -QString EasyTrackerDialog::threshold_display_text(int threshold_value) -{ - if (!s.auto_threshold) - return tr("Brightness %1/255").arg(threshold_value); - else - { - int w = s.cam_res_x, h = s.cam_res_y; + // refresh threshold display on auto-threshold checkbox state change + tie_setting(s.auto_threshold, + this, + [this](bool) { s.threshold_slider.notify(); }); + } - if (w * h <= 0) + QString Dialog::threshold_display_text(int threshold_value) + { + if (!s.auto_threshold) + return tr("Brightness %1/255").arg(threshold_value); + else { - w = 640; - h = 480; - } - - //SL: What are we suppose to do here? - double value = 0.0f; + int w = s.cam_res_x, h = s.cam_res_y; - return tr("LED radius %1 pixels").arg(value, 0, 'f', 2); - } -} + if (w * h <= 0) + { + w = 640; + h = 480; + } -void EasyTrackerDialog::startstop_trans_calib(bool start) -{ - QMutexLocker l(&calibrator_mutex); + //SL: What are we suppose to do here? + double value = 0.0f; - if (start) - { - qDebug() << "pt: starting translation calibration"; - calib_timer.start(); - trans_calib.reset(); - s.t_MH_x = 0; - s.t_MH_y = 0; - s.t_MH_z = 0; - - ui.sample_count_display->setText(QString()); + return tr("LED radius %1 pixels").arg(value, 0, 'f', 2); + } } - else + + void Dialog::startstop_trans_calib(bool start) { - calib_timer.stop(); - qDebug() << "pt: stopping translation calibration"; + QMutexLocker l(&calibrator_mutex); + + if (start) + { + qDebug() << "pt: starting translation calibration"; + calib_timer.start(); + trans_calib.reset(); + s.t_MH_x = 0; + s.t_MH_y = 0; + s.t_MH_z = 0; + + ui.sample_count_display->setText(QString()); + } + else { - auto [tmp, nsamples] = trans_calib.get_estimate(); - s.t_MH_x = int(tmp[0]); - s.t_MH_y = int(tmp[1]); - s.t_MH_z = int(tmp[2]); - - constexpr int min_yaw_samples = 15; - constexpr int min_pitch_samples = 15; - constexpr int min_samples = min_yaw_samples+min_pitch_samples; - - // Don't bother counting roll samples. Roll calibration is hard enough - // that it's a hidden unsupported feature anyway. - - QString sample_feedback; - if (nsamples[0] < min_yaw_samples) - sample_feedback = tr("%1 yaw samples. Yaw more to %2 samples for stable calibration.").arg(nsamples[0]).arg(min_yaw_samples); - else if (nsamples[1] < min_pitch_samples) - sample_feedback = tr("%1 pitch samples. Pitch more to %2 samples for stable calibration.").arg(nsamples[1]).arg(min_pitch_samples); - else + calib_timer.stop(); + qDebug() << "pt: stopping translation calibration"; { - const int nsamples_total = nsamples[0] + nsamples[1]; - sample_feedback = tr("%1 samples. Over %2, good!").arg(nsamples_total).arg(min_samples); + auto[tmp, nsamples] = trans_calib.get_estimate(); + s.t_MH_x = int(tmp[0]); + s.t_MH_y = int(tmp[1]); + s.t_MH_z = int(tmp[2]); + + constexpr int min_yaw_samples = 15; + constexpr int min_pitch_samples = 15; + constexpr int min_samples = min_yaw_samples + min_pitch_samples; + + // Don't bother counting roll samples. Roll calibration is hard enough + // that it's a hidden unsupported feature anyway. + + QString sample_feedback; + if (nsamples[0] < min_yaw_samples) + sample_feedback = tr("%1 yaw samples. Yaw more to %2 samples for stable calibration.").arg(nsamples[0]).arg(min_yaw_samples); + else if (nsamples[1] < min_pitch_samples) + sample_feedback = tr("%1 pitch samples. Pitch more to %2 samples for stable calibration.").arg(nsamples[1]).arg(min_pitch_samples); + else + { + const int nsamples_total = nsamples[0] + nsamples[1]; + sample_feedback = tr("%1 samples. Over %2, good!").arg(nsamples_total).arg(min_samples); + } + + ui.sample_count_display->setText(sample_feedback); } - - ui.sample_count_display->setText(sample_feedback); } + ui.tx_spin->setEnabled(!start); + ui.ty_spin->setEnabled(!start); + ui.tz_spin->setEnabled(!start); + + if (start) + ui.tcalib_button->setText(tr("Stop calibration")); + else + ui.tcalib_button->setText(tr("Start calibration")); } - ui.tx_spin->setEnabled(!start); - ui.ty_spin->setEnabled(!start); - ui.tz_spin->setEnabled(!start); - - if (start) - ui.tcalib_button->setText(tr("Stop calibration")); - else - ui.tcalib_button->setText(tr("Start calibration")); -} -void EasyTrackerDialog::poll_tracker_info_impl() -{ - //SL: sort this out - /* - pt_camera_info info; - if (tracker && tracker->get_cam_info(info)) + void Dialog::poll_tracker_info_impl() { - ui.caminfo_label->setText(tr("%1x%2 @ %3 FPS").arg(info.res_x).arg(info.res_y).arg(iround(info.fps))); + //SL: sort this out + /* + pt_camera_info info; + if (tracker && tracker->get_cam_info(info)) + { + ui.caminfo_label->setText(tr("%1x%2 @ %3 FPS").arg(info.res_x).arg(info.res_y).arg(iround(info.fps))); - // display point info - const int n_points = tracker->get_n_points(); - ui.pointinfo_label->setText((n_points == 3 ? tr("%1 OK!") : tr("%1 BAD!")).arg(n_points)); + // display point info + const int n_points = tracker->get_n_points(); + ui.pointinfo_label->setText((n_points == 3 ? tr("%1 OK!") : tr("%1 BAD!")).arg(n_points)); + } + else + */ + { + ui.caminfo_label->setText(tr("Tracker offline")); + ui.pointinfo_label->setText(QString()); + } } - else - */ + + void Dialog::set_camera_settings_available(const QString& /* camera_name */) { - ui.caminfo_label->setText(tr("Tracker offline")); - ui.pointinfo_label->setText(QString()); + ui.camera_settings->setEnabled(true); } -} - -void EasyTrackerDialog::set_camera_settings_available(const QString& /* camera_name */) -{ - ui.camera_settings->setEnabled(true); -} -void EasyTrackerDialog::show_camera_settings() -{ - if (tracker) + void Dialog::show_camera_settings() { - QMutexLocker l(&tracker->camera_mtx); - tracker->camera->show_dialog(); + if (tracker) + { + QMutexLocker l(&tracker->camera_mtx); + tracker->camera->show_dialog(); + } + else + (void)video::show_dialog(s.camera_name); } - else - (void)video::show_dialog(s.camera_name); -} -void EasyTrackerDialog::trans_calib_step() -{ - QMutexLocker l(&calibrator_mutex); - // TODO: Do we still need that function -} + void Dialog::trans_calib_step() + { + QMutexLocker l(&calibrator_mutex); + // TODO: Do we still need that function + } -void EasyTrackerDialog::save() -{ - s.b->save(); -} + void Dialog::save() + { + s.b->save(); + } -void EasyTrackerDialog::doOK() -{ - save(); - close(); -} + void Dialog::doOK() + { + save(); + close(); + } -void EasyTrackerDialog::doCancel() -{ - close(); -} + void Dialog::doCancel() + { + close(); + } -void EasyTrackerDialog::register_tracker(ITracker *t) -{ - tracker = static_cast<EasyTracker*>(t); - ui.tcalib_button->setEnabled(true); - poll_tracker_info(); - timer.start(); -} + void Dialog::register_tracker(ITracker *t) + { + tracker = static_cast<Tracker*>(t); + ui.tcalib_button->setEnabled(true); + poll_tracker_info(); + timer.start(); + } -void EasyTrackerDialog::unregister_tracker() -{ - tracker = nullptr; - ui.tcalib_button->setEnabled(false); - poll_tracker_info(); - timer.stop(); + void Dialog::unregister_tracker() + { + tracker = nullptr; + ui.tcalib_button->setEnabled(false); + poll_tracker_info(); + timer.stop(); + } } diff --git a/tracker-easy/tracker-easy-dialog.h b/tracker-easy/tracker-easy-dialog.h index 1f21f92b..8d4cffbd 100644 --- a/tracker-easy/tracker-easy-dialog.h +++ b/tracker-easy/tracker-easy-dialog.h @@ -17,35 +17,37 @@ #include <QTimer> #include <QMutex> - -class EasyTrackerDialog : public ITrackerDialog +namespace EasyTracker { - Q_OBJECT -public: - EasyTrackerDialog(const QString& module_name); - void register_tracker(ITracker *tracker) override; - void unregister_tracker() override; - void save(); -public slots: - void doOK(); - void doCancel(); - - void startstop_trans_calib(bool start); - void trans_calib_step(); - void poll_tracker_info_impl(); - void set_camera_settings_available(const QString& camera_name); - void show_camera_settings(); -signals: - void poll_tracker_info(); -protected: - QString threshold_display_text(int threshold_value); - - pt_settings s; - EasyTracker* tracker; - QTimer timer, calib_timer; - TranslationCalibrator trans_calib; - QMutex calibrator_mutex; - - Ui::UICPTClientControls ui; -}; - + class Dialog : public ITrackerDialog + { + Q_OBJECT + public: + Dialog(); + void register_tracker(ITracker *tracker) override; + void unregister_tracker() override; + void save(); + public slots: + void doOK(); + void doCancel(); + + void startstop_trans_calib(bool start); + void trans_calib_step(); + void poll_tracker_info_impl(); + void set_camera_settings_available(const QString& camera_name); + void show_camera_settings(); + signals: + void poll_tracker_info(); + protected: + QString threshold_display_text(int threshold_value); + + pt_settings s; + Tracker* tracker; + QTimer timer, calib_timer; + TranslationCalibrator trans_calib; + QMutex calibrator_mutex; + + Ui::UICPTClientControls ui; + }; + +} diff --git a/tracker-easy/tracker-easy.cpp b/tracker-easy/tracker-easy.cpp index 5fd1952c..5fe93b0e 100644 --- a/tracker-easy/tracker-easy.cpp +++ b/tracker-easy/tracker-easy.cpp @@ -11,7 +11,7 @@ #include "video/video-widget.hpp" #include "compat/math-imports.hpp" #include "compat/check-visible.hpp" - +#include "cv-point-extractor.h" #include "tracker-easy-api.h" #include <QHBoxLayout> @@ -26,358 +26,360 @@ using namespace options; - -EasyTracker::EasyTracker(pointer<IEasyTrackerTraits> const& traits) : - traits { traits }, - s { traits->get_module_name() }, - point_extractor { traits->make_point_extractor() }, - iPreview{ preview_width, preview_height } +namespace EasyTracker { - cv::setBreakOnError(true); - cv::setNumThreads(1); - connect(s.b.get(), &bundle_::saving, this, &EasyTracker::maybe_reopen_camera, Qt::DirectConnection); - connect(s.b.get(), &bundle_::reloading, this, &EasyTracker::maybe_reopen_camera, Qt::DirectConnection); - - connect(&s.fov, value_::value_changed<int>(), this, &EasyTracker::set_fov, Qt::DirectConnection); - set_fov(s.fov); -} - -EasyTracker::~EasyTracker() -{ - // - cv::destroyWindow("Preview"); + Tracker::Tracker() : + s{ KModuleName }, + point_extractor{ std::make_unique<CvPointExtractor>() }, + iPreview{ preview_width, preview_height } + { + cv::setBreakOnError(true); + cv::setNumThreads(1); - requestInterruption(); - wait(); + connect(s.b.get(), &bundle_::saving, this, &Tracker::maybe_reopen_camera, Qt::DirectConnection); + connect(s.b.get(), &bundle_::reloading, this, &Tracker::maybe_reopen_camera, Qt::DirectConnection); - QMutexLocker l(&camera_mtx); - camera->stop(); -} + connect(&s.fov, value_::value_changed<int>(), this, &Tracker::set_fov, Qt::DirectConnection); + set_fov(s.fov); + } + Tracker::~Tracker() + { + // + cv::destroyWindow("Preview"); -// Compute Euler angles from ratation matrix -cv::Vec3f EulerAngles(cv::Mat &R) -{ + requestInterruption(); + wait(); - float sy = sqrt(R.at<double>(0, 0) * R.at<double>(0, 0) + R.at<double>(1, 0) * R.at<double>(1, 0)); + QMutexLocker l(&camera_mtx); + camera->stop(); + } - bool singular = sy < 1e-6; // If - float x, y, z; - if (!singular) - { - x = atan2(R.at<double>(2, 1), R.at<double>(2, 2)); - y = atan2(-R.at<double>(2, 0), sy); - z = atan2(R.at<double>(1, 0), R.at<double>(0, 0)); - } - else + // Compute Euler angles from ratation matrix + cv::Vec3f EulerAngles(cv::Mat &R) { - x = atan2(-R.at<double>(1, 2), R.at<double>(1, 1)); - y = atan2(-R.at<double>(2, 0), sy); - z = 0; - } - // Convert to degrees - return cv::Vec3f(x* 180 / CV_PI, y* 180 / CV_PI, z* 180 / CV_PI); -} + float sy = sqrt(R.at<double>(0, 0) * R.at<double>(0, 0) + R.at<double>(1, 0) * R.at<double>(1, 0)); + bool singular = sy < 1e-6; // If -void getEulerAngles(cv::Mat &rotCamerMatrix, cv::Vec3d &eulerAngles) -{ + float x, y, z; + if (!singular) + { + x = atan2(R.at<double>(2, 1), R.at<double>(2, 2)); + y = atan2(-R.at<double>(2, 0), sy); + z = atan2(R.at<double>(1, 0), R.at<double>(0, 0)); + } + else + { + x = atan2(-R.at<double>(1, 2), R.at<double>(1, 1)); + y = atan2(-R.at<double>(2, 0), sy); + z = 0; + } - cv::Mat cameraMatrix, rotMatrix, transVect, rotMatrixX, rotMatrixY, rotMatrixZ; - double* _r = rotCamerMatrix.ptr<double>(); - double projMatrix[12] = { _r[0],_r[1],_r[2],0, - _r[3],_r[4],_r[5],0, - _r[6],_r[7],_r[8],0 }; - - cv::decomposeProjectionMatrix(cv::Mat(3, 4, CV_64FC1, projMatrix), - cameraMatrix, - rotMatrix, - transVect, - rotMatrixX, - rotMatrixY, - rotMatrixZ, - eulerAngles); -} + // Convert to degrees + return cv::Vec3f(x * 180 / CV_PI, y * 180 / CV_PI, z * 180 / CV_PI); + } -void EasyTracker::run() -{ - maybe_reopen_camera(); + void getEulerAngles(cv::Mat &rotCamerMatrix, cv::Vec3d &eulerAngles) + { - while(!isInterruptionRequested()) - { - bool new_frame = false; + cv::Mat cameraMatrix, rotMatrix, transVect, rotMatrixX, rotMatrixY, rotMatrixZ; + double* _r = rotCamerMatrix.ptr<double>(); + double projMatrix[12] = { _r[0],_r[1],_r[2],0, + _r[3],_r[4],_r[5],0, + _r[6],_r[7],_r[8],0 }; + + cv::decomposeProjectionMatrix(cv::Mat(3, 4, CV_64FC1, projMatrix), + cameraMatrix, + rotMatrix, + transVect, + rotMatrixX, + rotMatrixY, + rotMatrixZ, + eulerAngles); + } - { - QMutexLocker l(&camera_mtx); - if (camera) - std::tie(iFrame, new_frame) = camera->get_frame(); - } + void Tracker::run() + { + maybe_reopen_camera(); - if (new_frame) + while (!isInterruptionRequested()) { - //TODO: We should not assume channel size of 1 byte - iMatFrame = cv::Mat(iFrame.height, iFrame.width, CV_MAKETYPE(CV_8U,iFrame.channels), iFrame.data, iFrame.stride); + bool new_frame = false; - - const bool preview_visible = check_is_visible(); - if (preview_visible) { - iPreview = iMatFrame; - } + QMutexLocker l(&camera_mtx); - iPoints.clear(); - point_extractor->extract_points(iMatFrame, (preview_visible?&iPreview.iFrameRgb:nullptr), iPoints); - point_count.store(iPoints.size(), std::memory_order_relaxed); - - - if (preview_visible) - { - //iPreview = iMatFrame; - cv::imshow("Preview", iPreview.iFrameRgb); - cv::waitKey(1); + if (camera) + std::tie(iFrame, new_frame) = camera->get_frame(); } - else + + if (new_frame) { - cv::destroyWindow("Preview"); - } + //TODO: We should not assume channel size of 1 byte + iMatFrame = cv::Mat(iFrame.height, iFrame.width, CV_MAKETYPE(CV_8U, iFrame.channels), iFrame.data, iFrame.stride); + + + const bool preview_visible = check_is_visible(); + if (preview_visible) + { + iPreview = iMatFrame; + } - const bool success = iPoints.size() >= KPointCount; + iPoints.clear(); + point_extractor->extract_points(iMatFrame, (preview_visible ? &iPreview.iFrameRgb : nullptr), iPoints); + point_count.store(iPoints.size(), std::memory_order_relaxed); - int topPointIndex = -1; - { - QMutexLocker l(¢er_lock); + if (preview_visible) + { + //iPreview = iMatFrame; + cv::imshow("Preview", iPreview.iFrameRgb); + cv::waitKey(1); + } + else + { + cv::destroyWindow("Preview"); + } + + const bool success = iPoints.size() >= KPointCount; + + int topPointIndex = -1; - if (success) { - ever_success.store(true, std::memory_order_relaxed); - - // Solve P3P problem with OpenCV - - // Construct the points defining the object we want to detect based on settings. - // We are converting them from millimeters to centimeters. - // TODO: Need to support clip too. That's cap only for now. - // s.active_model_panel != PointModel::Clip - - std::vector<cv::Point3f> objectPoints; - objectPoints.push_back(cv::Point3f(s.cap_x/10.0, s.cap_z / 10.0, -s.cap_y / 10.0)); // Right - objectPoints.push_back(cv::Point3f(-s.cap_x/10.0, s.cap_z / 10.0, -s.cap_y / 10.0)); // Left - objectPoints.push_back(cv::Point3f(0, 0, 0)); // Top - - //Bitmap origin is top left - std::vector<cv::Point2f> trackedPoints; - // Stuff bitmap point in there making sure they match the order of the object point - // Find top most point, that's the one with min Y as we assume our guy's head is not up side down - - int minY = std::numeric_limits<int>::max(); - for (int i = 0; i < 3; i++) + QMutexLocker l(¢er_lock); + + if (success) { - if (iPoints[i][1]<minY) + ever_success.store(true, std::memory_order_relaxed); + + // Solve P3P problem with OpenCV + + // Construct the points defining the object we want to detect based on settings. + // We are converting them from millimeters to centimeters. + // TODO: Need to support clip too. That's cap only for now. + // s.active_model_panel != PointModel::Clip + + std::vector<cv::Point3f> objectPoints; + objectPoints.push_back(cv::Point3f(s.cap_x / 10.0, s.cap_z / 10.0, -s.cap_y / 10.0)); // Right + objectPoints.push_back(cv::Point3f(-s.cap_x / 10.0, s.cap_z / 10.0, -s.cap_y / 10.0)); // Left + objectPoints.push_back(cv::Point3f(0, 0, 0)); // Top + + //Bitmap origin is top left + std::vector<cv::Point2f> trackedPoints; + // Stuff bitmap point in there making sure they match the order of the object point + // Find top most point, that's the one with min Y as we assume our guy's head is not up side down + + int minY = std::numeric_limits<int>::max(); + for (int i = 0; i < 3; i++) { - minY = iPoints[i][1]; - topPointIndex = i; + if (iPoints[i][1] < minY) + { + minY = iPoints[i][1]; + topPointIndex = i; + } } - } - int rightPointIndex = -1; - int maxX = 0; + int rightPointIndex = -1; + int maxX = 0; - // Find right most point - for (int i = 0; i < 3; i++) - { - // Excluding top most point - if (i!=topPointIndex && iPoints[i][0] > maxX) + // Find right most point + for (int i = 0; i < 3; i++) { - maxX = iPoints[i][0]; - rightPointIndex = i; + // Excluding top most point + if (i != topPointIndex && iPoints[i][0] > maxX) + { + maxX = iPoints[i][0]; + rightPointIndex = i; + } } - } - // Find left most point - int leftPointIndex = -1; - for (int i = 0; i < 3; i++) - { - // Excluding top most point - if (i != topPointIndex && i != rightPointIndex) + // Find left most point + int leftPointIndex = -1; + for (int i = 0; i < 3; i++) { - leftPointIndex = i; - break; + // Excluding top most point + if (i != topPointIndex && i != rightPointIndex) + { + leftPointIndex = i; + break; + } } - } - // - trackedPoints.push_back(cv::Point2f(iPoints[rightPointIndex][0], iPoints[rightPointIndex][1])); - trackedPoints.push_back(cv::Point2f(iPoints[leftPointIndex][0], iPoints[leftPointIndex][1])); - trackedPoints.push_back(cv::Point2f(iPoints[topPointIndex][0], iPoints[topPointIndex][1])); - - std::cout << "Object: " << objectPoints << "\n"; - std::cout << "Points: " << trackedPoints << "\n"; - - - // Create our camera matrix - // TODO: Just do that once, use data member instead - // Double or Float? - cv::Mat cameraMatrix; - cameraMatrix.create(3, 3, CV_64FC1); - cameraMatrix.setTo(cv::Scalar(0)); - cameraMatrix.at<double>(0, 0) = iCameraInfo.focalLengthX; - cameraMatrix.at<double>(1, 1) = iCameraInfo.focalLengthY; - cameraMatrix.at<double>(0, 2) = iCameraInfo.principalPointX; - cameraMatrix.at<double>(1, 2) = iCameraInfo.principalPointY; - cameraMatrix.at<double>(2, 2) = 1; - - // Create distortion cooefficients - cv::Mat distCoeffs = cv::Mat::zeros(8, 1, CV_64FC1); - // As per OpenCV docs they should be thus: k1, k2, p1, p2, k3, k4, k5, k6 - distCoeffs.at<double>(0, 0) = 0; // Radial first order - distCoeffs.at<double>(1, 0) = iCameraInfo.radialDistortionSecondOrder; // Radial second order - distCoeffs.at<double>(2, 0) = 0; // Tangential first order - distCoeffs.at<double>(3, 0) = 0; // Tangential second order - distCoeffs.at<double>(4, 0) = 0; // Radial third order - distCoeffs.at<double>(5, 0) = iCameraInfo.radialDistortionFourthOrder; // Radial fourth order - distCoeffs.at<double>(6, 0) = 0; // Radial fith order - distCoeffs.at<double>(7, 0) = iCameraInfo.radialDistortionSixthOrder; // Radial sixth order - - // Define our solution arrays - // They will receive up to 4 solutions for our P3P problem - - - // TODO: try SOLVEPNP_AP3P too - iAngles.clear(); - iBestSolutionIndex = -1; - int solutionCount = cv::solveP3P(objectPoints, trackedPoints, cameraMatrix, distCoeffs, iRotations, iTranslations, cv::SOLVEPNP_P3P); - - if (solutionCount > 0) - { - std::cout << "Solution count: " << solutionCount << "\n"; - int minPitch = std::numeric_limits<int>::max(); - // Find the solution we want - for (int i = 0; i < solutionCount; i++) + // + trackedPoints.push_back(cv::Point2f(iPoints[rightPointIndex][0], iPoints[rightPointIndex][1])); + trackedPoints.push_back(cv::Point2f(iPoints[leftPointIndex][0], iPoints[leftPointIndex][1])); + trackedPoints.push_back(cv::Point2f(iPoints[topPointIndex][0], iPoints[topPointIndex][1])); + + std::cout << "Object: " << objectPoints << "\n"; + std::cout << "Points: " << trackedPoints << "\n"; + + + // Create our camera matrix + // TODO: Just do that once, use data member instead + // Double or Float? + cv::Mat cameraMatrix; + cameraMatrix.create(3, 3, CV_64FC1); + cameraMatrix.setTo(cv::Scalar(0)); + cameraMatrix.at<double>(0, 0) = iCameraInfo.focalLengthX; + cameraMatrix.at<double>(1, 1) = iCameraInfo.focalLengthY; + cameraMatrix.at<double>(0, 2) = iCameraInfo.principalPointX; + cameraMatrix.at<double>(1, 2) = iCameraInfo.principalPointY; + cameraMatrix.at<double>(2, 2) = 1; + + // Create distortion cooefficients + cv::Mat distCoeffs = cv::Mat::zeros(8, 1, CV_64FC1); + // As per OpenCV docs they should be thus: k1, k2, p1, p2, k3, k4, k5, k6 + distCoeffs.at<double>(0, 0) = 0; // Radial first order + distCoeffs.at<double>(1, 0) = iCameraInfo.radialDistortionSecondOrder; // Radial second order + distCoeffs.at<double>(2, 0) = 0; // Tangential first order + distCoeffs.at<double>(3, 0) = 0; // Tangential second order + distCoeffs.at<double>(4, 0) = 0; // Radial third order + distCoeffs.at<double>(5, 0) = iCameraInfo.radialDistortionFourthOrder; // Radial fourth order + distCoeffs.at<double>(6, 0) = 0; // Radial fith order + distCoeffs.at<double>(7, 0) = iCameraInfo.radialDistortionSixthOrder; // Radial sixth order + + // Define our solution arrays + // They will receive up to 4 solutions for our P3P problem + + + // TODO: try SOLVEPNP_AP3P too + iAngles.clear(); + iBestSolutionIndex = -1; + int solutionCount = cv::solveP3P(objectPoints, trackedPoints, cameraMatrix, distCoeffs, iRotations, iTranslations, cv::SOLVEPNP_P3P); + + if (solutionCount > 0) { - std::cout << "Translation:\n"; - std::cout << iTranslations.at(i); - std::cout << "\n"; - std::cout << "Rotation:\n"; - //std::cout << rvecs.at(i); - cv::Mat rotationCameraMatrix; - cv::Rodrigues(iRotations[i], rotationCameraMatrix); - cv::Vec3d angles; - getEulerAngles(rotationCameraMatrix,angles); - iAngles.push_back(angles); - - // Check if pitch is closest to zero - int absolutePitch = std::abs(angles[0]); - if (minPitch > absolutePitch) + std::cout << "Solution count: " << solutionCount << "\n"; + int minPitch = std::numeric_limits<int>::max(); + // Find the solution we want + for (int i = 0; i < solutionCount; i++) { - minPitch = absolutePitch; - iBestSolutionIndex = i; + std::cout << "Translation:\n"; + std::cout << iTranslations.at(i); + std::cout << "\n"; + std::cout << "Rotation:\n"; + //std::cout << rvecs.at(i); + cv::Mat rotationCameraMatrix; + cv::Rodrigues(iRotations[i], rotationCameraMatrix); + cv::Vec3d angles; + getEulerAngles(rotationCameraMatrix, angles); + iAngles.push_back(angles); + + // Check if pitch is closest to zero + int absolutePitch = std::abs(angles[0]); + if (minPitch > absolutePitch) + { + minPitch = absolutePitch; + iBestSolutionIndex = i; + } + + //cv::Vec3f angles=EulerAngles(quaternion); + std::cout << angles; + std::cout << "\n"; } - //cv::Vec3f angles=EulerAngles(quaternion); - std::cout << angles; std::cout << "\n"; + } - std::cout << "\n"; - } - } + // Send solution data back to main thread + QMutexLocker l2(&data_lock); + if (iBestSolutionIndex != -1) + { + iBestAngles = iAngles[iBestSolutionIndex]; + iBestTranslation = iTranslations[iBestSolutionIndex]; + } - // Send solution data back to main thread - QMutexLocker l2(&data_lock); - if (iBestSolutionIndex != -1) - { - iBestAngles = iAngles[iBestSolutionIndex]; - iBestTranslation = iTranslations[iBestSolutionIndex]; } - } - - if (preview_visible) - { - if (topPointIndex != -1) + if (preview_visible) { - // Render a cross to indicate which point is the head - iPreview.draw_head_center(iPoints[topPointIndex][0], iPoints[topPointIndex][1]); - } - - widget->update_image(iPreview.get_bitmap()); + if (topPointIndex != -1) + { + // Render a cross to indicate which point is the head + iPreview.draw_head_center(iPoints[topPointIndex][0], iPoints[topPointIndex][1]); + } - auto [ w, h ] = widget->preview_size(); - if (w != preview_width || h != preview_height) - { - // Resize preivew if widget size has changed - preview_width = w; preview_height = h; - iPreview = Preview(w, h); + widget->update_image(iPreview.get_bitmap()); + + auto[w, h] = widget->preview_size(); + if (w != preview_width || h != preview_height) + { + // Resize preivew if widget size has changed + preview_width = w; preview_height = h; + iPreview = Preview(w, h); + } } } } } -} -bool EasyTracker::maybe_reopen_camera() -{ - QMutexLocker l(&camera_mtx); + bool Tracker::maybe_reopen_camera() + { + QMutexLocker l(&camera_mtx); - return camera->start(iCameraInfo); -} + return camera->start(iCameraInfo); + } -void EasyTracker::set_fov(int value) -{ - QMutexLocker l(&camera_mtx); + void Tracker::set_fov(int value) + { + QMutexLocker l(&camera_mtx); -} + } -module_status EasyTracker::start_tracker(QFrame* video_frame) -{ - //video_frame->setAttribute(Qt::WA_NativeWindow); + module_status Tracker::start_tracker(QFrame* video_frame) + { + //video_frame->setAttribute(Qt::WA_NativeWindow); - widget = std::make_unique<video_widget>(video_frame); - layout = std::make_unique<QHBoxLayout>(video_frame); - layout->setContentsMargins(0, 0, 0, 0); - layout->addWidget(widget.get()); - video_frame->setLayout(layout.get()); - //video_widget->resize(video_frame->width(), video_frame->height()); - video_frame->show(); + widget = std::make_unique<video_widget>(video_frame); + layout = std::make_unique<QHBoxLayout>(video_frame); + layout->setContentsMargins(0, 0, 0, 0); + layout->addWidget(widget.get()); + video_frame->setLayout(layout.get()); + //video_widget->resize(video_frame->width(), video_frame->height()); + video_frame->show(); - // Create our camera - camera = video::make_camera(s.camera_name); + // Create our camera + camera = video::make_camera(s.camera_name); - start(QThread::HighPriority); + start(QThread::HighPriority); - return {}; -} + return {}; + } -void EasyTracker::data(double *data) -{ - if (ever_success.load(std::memory_order_relaxed)) + void Tracker::data(double *data) { - // Get data back from tracker thread - QMutexLocker l(&data_lock); - data[Yaw] = iBestAngles[1]; - data[Pitch] = iBestAngles[0]; - data[Roll] = iBestAngles[2]; - data[TX] = iBestTranslation[0]; - data[TY] = iBestTranslation[1]; - data[TZ] = iBestTranslation[2]; + if (ever_success.load(std::memory_order_relaxed)) + { + // Get data back from tracker thread + QMutexLocker l(&data_lock); + data[Yaw] = iBestAngles[1]; + data[Pitch] = iBestAngles[0]; + data[Roll] = iBestAngles[2]; + data[TX] = iBestTranslation[0]; + data[TY] = iBestTranslation[1]; + data[TZ] = iBestTranslation[2]; + } } -} -bool EasyTracker::center() -{ - QMutexLocker l(¢er_lock); - //TODO: Do we need to do anything there? - return false; -} + bool Tracker::center() + { + QMutexLocker l(¢er_lock); + //TODO: Do we need to do anything there? + return false; + } -int EasyTracker::get_n_points() -{ - return (int)point_count.load(std::memory_order_relaxed); -} + int Tracker::get_n_points() + { + return (int)point_count.load(std::memory_order_relaxed); + } +} diff --git a/tracker-easy/tracker-easy.h b/tracker-easy/tracker-easy.h index 3055299b..341d676b 100644 --- a/tracker-easy/tracker-easy.h +++ b/tracker-easy/tracker-easy.h @@ -25,66 +25,70 @@ #include <QMutex> #include <QLayout> +namespace EasyTracker +{ -class EasyTrackerDialog; + static const QString KModuleName = "tracker-easy"; -using namespace numeric_types; + class Dialog; -struct EasyTracker : QThread, ITracker -{ - friend class EasyTrackerDialog; + using namespace numeric_types; + + struct Tracker : QThread, ITracker + { + friend class Dialog; - template<typename t> using pointer = pt_pointer<t>; + template<typename t> using pointer = pt_pointer<t>; - explicit EasyTracker(pointer<IEasyTrackerTraits> const& pt_runtime_traits); - ~EasyTracker() override; - module_status start_tracker(QFrame* parent_window) override; - void data(double* data) override; - bool center() override; + explicit Tracker(); + ~Tracker() override; + module_status start_tracker(QFrame* parent_window) override; + void data(double* data) override; + bool center() override; - int get_n_points(); + int get_n_points(); -private: - void run() override; + private: + void run() override; - bool maybe_reopen_camera(); - void set_fov(int value); + bool maybe_reopen_camera(); + void set_fov(int value); - pointer<IEasyTrackerTraits> traits; + QMutex camera_mtx; - QMutex camera_mtx; + pt_settings s; - pt_settings s; + std::unique_ptr<QLayout> layout; + std::vector<vec2> iPoints; - std::unique_ptr<QLayout> layout; - std::vector<vec2> iPoints; + int preview_width = 320, preview_height = 240; - int preview_width = 320, preview_height = 240; + std::unique_ptr<IPointExtractor> point_extractor; + std::unique_ptr<video::impl::camera> camera; + video::impl::camera::info iCameraInfo; + pointer<video_widget> widget; - pointer<IPointExtractor> point_extractor; - std::unique_ptr<video::impl::camera> camera; - video::impl::camera::info iCameraInfo; - pointer<video_widget> widget; + video::frame iFrame; + cv::Mat iMatFrame; + Preview iPreview; - video::frame iFrame; - cv::Mat iMatFrame; - Preview iPreview; + std::atomic<unsigned> point_count{ 0 }; + std::atomic<bool> ever_success = false; + mutable QMutex center_lock, data_lock; - std::atomic<unsigned> point_count { 0 }; - std::atomic<bool> ever_success = false; - mutable QMutex center_lock, data_lock; + // Translation solutions + std::vector<cv::Mat> iTranslations; + // Rotation solutions + std::vector<cv::Mat> iRotations; + // Angle solutions, pitch, yaw, roll, in this order + std::vector<cv::Vec3d> iAngles; + // The index of our best solution in the above arrays + int iBestSolutionIndex = -1; + // Best translation + cv::Vec3d iBestTranslation; + // Best angles + cv::Vec3d iBestAngles; + }; - // Translation solutions - std::vector<cv::Mat> iTranslations; - // Rotation solutions - std::vector<cv::Mat> iRotations; - // Angle solutions, pitch, yaw, roll, in this order - std::vector<cv::Vec3d> iAngles; - // The index of our best solution in the above arrays - int iBestSolutionIndex = -1; - // Best translation - cv::Vec3d iBestTranslation; - // Best angles - cv::Vec3d iBestAngles; -}; +} |