From 14f59d9fe5f1926a8562efc8a51a785365bc0131 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Tue, 14 Oct 2014 23:02:15 +0200 Subject: aruco: fix fixating on the prev marker location --- ftnoir_tracker_aruco/aruco-trackercontrols.ui | 117 +++++++++++--------------- ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 79 +++++++---------- ftnoir_tracker_aruco/ftnoir_tracker_aruco.h | 6 +- 3 files changed, 78 insertions(+), 124 deletions(-) (limited to 'ftnoir_tracker_aruco') diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui index 1898d15b..099dec02 100644 --- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui +++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui @@ -10,7 +10,7 @@ 0 0 562 - 214 + 178 @@ -33,6 +33,42 @@ + + + + + 0 + 0 + + + + + + + 35.000000000000000 + + + 180.000000000000000 + + + 52.000000000000000 + + + + + + + Frames per second + + + + + + + Resolution + + + @@ -75,59 +111,6 @@ - - - - Resolution - - - - - - - - 0 - 0 - - - - - - - 35.000000000000000 - - - 180.000000000000000 - - - 52.000000000000000 - - - - - - - Frames per second - - - - - - - - 0 - 0 - - - - - - - - Camera name - - - @@ -158,26 +141,20 @@ - - + + - Sun glare removal + Camera name - - - - 100 - - - 10 - - - 20 - - - Qt::Horizontal + + + + + 0 + 0 + diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp index 0974f0f0..fd8a8ce1 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp @@ -194,7 +194,6 @@ void Tracker::run() auto last_time = cv::getTickCount(); int cur_fps = 0; int last_fps = 0; - cv::Point2f last_centroid; while (!stop) { @@ -203,42 +202,9 @@ void Tracker::run() continue; auto tm = cv::getTickCount(); - const double c = s.desaturate * 16. / 100.; - - if (std::abs(c) > 1e-3) - { - const int w=color.cols, h=color.rows; - - cv::Mat hsv; - cv::cvtColor(color, hsv, cv::COLOR_BGR2HSV); - vector channels; - cv::split(hsv, channels); - cv::Mat sat = channels[1]; - cv::Mat val = channels[2]; - - struct ops { - static double sig(double x) - { - double x_ = -6 + x * 2 * 6; - return 1./(1.+exp(-x_)); - } - }; - - for (int i = 0; i < h; i++) - for (int j = 0; j < w; j++) - { - const double sat_ij = sat.at(i, j)/255.; - val.at(i, j) *= std::max(0., 1. - c*ops::sig(sat_ij)); - } - - channels[1] = sat; - channels[2] = val; - cv::merge(channels, hsv); - cv::cvtColor(hsv, color, cv::COLOR_HSV2BGR); - } cv::Mat grayscale; cv::cvtColor(color, grayscale, cv::COLOR_BGR2GRAY); - + const int scale = frame.cols > 480 ? 2 : 1; detector.setThresholdParams(scale > 1 ? 11 : 7, 4); @@ -333,9 +299,35 @@ void Tracker::run() cv::Vec3d rvec, tvec; - cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::ITERATIVE); + cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::P3P); std::vector roi_projection(4); + + { + std::vector repr2; + std::vector centroid; + centroid.push_back(cv::Point3f(0, 0, 0)); + cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, repr2); + + { + auto s = cv::Scalar(255, 0, 255); + cv::circle(frame, repr2.at(0), 4, s, -1); + } + } + + for (int i = 0; i < 4; i++) + { + obj_points.at(i, 0) -= s.headpos_x; + obj_points.at(i, 1) -= s.headpos_y; + obj_points.at(i, 2) -= s.headpos_z; + } + + { + cv::Mat rvec_, tvec_; + cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec_, tvec_, false, cv::P3P); + tvec = tvec_; + } + cv::Mat roi_points = obj_points * c_search_window; cv::projectPoints(roi_points, rvec, tvec, intrinsics, dist_coeffs, roi_projection); @@ -389,21 +381,9 @@ void Tracker::run() r = rmat; t = tvec; } - - std::vector repr2; - std::vector centroid; - centroid.push_back(cv::Point3f(0, 0, 0)); - cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, repr2); - - { - auto s = cv::Scalar(255, 0, 255); - cv::circle(frame, repr2.at(0), 4, s, -1); - } if (roi_valid) cv::rectangle(frame, last_roi, cv::Scalar(255, 0, 255), 1); - - last_centroid = repr2[0]; } else last_roi = cv::Rect(65535, 65535, 0, 0); @@ -490,7 +470,6 @@ TrackerControls::TrackerControls() tie_setting(s.headpos_x, ui.cx); tie_setting(s.headpos_y, ui.cy); tie_setting(s.headpos_z, ui.cz); - tie_setting(s.desaturate, ui.desaturate_slider); connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); connect(ui.btn_calibrate, SIGNAL(clicked()), this, SLOT(toggleCalibrate())); @@ -540,6 +519,6 @@ void TrackerControls::doOK() void TrackerControls::doCancel() { - s.b->revert(); + s.b->reload(); this->close(); } diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h index 03fff844..3d37dacd 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h @@ -28,7 +28,6 @@ struct settings { pbundle b; value fov, headpos_x, headpos_y, headpos_z; value camera_index, force_fps, resolution; - value desaturate; settings() : b(bundle("aruco-tracker")), fov(b, "field-of-view", 56), @@ -37,15 +36,14 @@ struct settings { headpos_z(b, "headpos-z", 0), camera_index(b, "camera-index", 0), force_fps(b, "force-fps", 0), - resolution(b, "force-resolution", 0), - desaturate(b, "desaturate", 0) + resolution(b, "force-resolution", 0) {} }; class Tracker : protected QThread, public ITracker { Q_OBJECT - static constexpr double c_search_window = 2.9; + static constexpr double c_search_window = 2.2; public: Tracker(); ~Tracker() override; -- cgit v1.2.3