From 10c47b159d5ea0e3888318d801be065b6d921b9d Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Mon, 27 Jun 2016 14:44:35 +0200 Subject: tracker/aruco: fix #375 The height was zero on the test video. The ROI check only saw if width is at least zero. Check for both to be greater than one. Video provided by: @kblomster Issue: #375 Also, fix minor issues: - nix vars that can be const static in function scope - don't call solvepnp twice where obj_points shift will do - don't do bounds checking on vector elt access - respect sprintf varargs type size; change to snprintf for no reason - fix clamp-to-image logic - set proper alpha for fps estimation --- tracker-aruco/ftnoir_tracker_aruco.cpp | 62 ++++++++++++++++++---------------- tracker-aruco/ftnoir_tracker_aruco.h | 7 ++-- 2 files changed, 36 insertions(+), 33 deletions(-) (limited to 'tracker-aruco') diff --git a/tracker-aruco/ftnoir_tracker_aruco.cpp b/tracker-aruco/ftnoir_tracker_aruco.cpp index 65056a63..d9579743 100644 --- a/tracker-aruco/ftnoir_tracker_aruco.cpp +++ b/tracker-aruco/ftnoir_tracker_aruco.cpp @@ -43,7 +43,6 @@ Tracker::Tracker() : obj_points(4), intrinsics(decltype(intrinsics)::eye()), dist_coeffs(decltype(dist_coeffs)::zeros()), - centroid { cv::Point3f(0, 0, 0) }, rmat(decltype(rmat)::eye()), roi_points(4), last_roi(65535, 65535, 0, 0), @@ -98,25 +97,29 @@ void Tracker::getRT(cv::Matx33d& r_, cv::Vec3d& t_) bool Tracker::detect_with_roi() { - if (last_roi.width > 0) + if (last_roi.width > 1 && last_roi.height > 1) { - detector.setMinMaxSize(std::min(1.f, std::max(0.01f, size_min * grayscale.cols / last_roi.width)), - std::max(.01f, std::min(1.0f, size_max * grayscale.cols / last_roi.width))); + float min = std::min(1.f, std::max(.01f, size_min * grayscale.cols / last_roi.width)); + float max = std::max(.01f, std::min(1.f, size_max * grayscale.cols / last_roi.width)); + detector.setMinMaxSize(min, max); - detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false); + cv::Mat grayscale_ = grayscale(last_roi); + + detector.detect(grayscale_, markers, cv::Mat(), cv::Mat(), -1, false); if (markers.size() == 1 && markers[0].size() == 4) { - auto& m = markers.at(0); + auto& m = markers[0]; for (unsigned i = 0; i < 4; i++) { - auto& p = m.at(i); + auto& p = m[i]; p.x += last_roi.x; p.y += last_roi.y; } return true; } } + last_roi = cv::Rect(65535, 65535, 0, 0); return false; } @@ -203,13 +206,14 @@ void Tracker::draw_ar(bool ok) { if (ok) { - const auto& m = markers.at(0); + const auto& m = markers[0]; for (unsigned i = 0; i < 4; i++) cv::line(frame, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), 2, 8); } char buf[32]; - ::sprintf(buf, "Hz: %d", (unsigned short)cur_fps); + ::snprintf(buf, sizeof(buf)-1, "Hz: %d", (int)(unsigned short)cur_fps); + buf[sizeof(buf)-1] = '\0'; cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 255, 0), 1); } @@ -219,10 +223,10 @@ void Tracker::clamp_last_roi() last_roi.x = 0; if (last_roi.y < 0) last_roi.y = 0; - if (last_roi.width < 0) - last_roi.width = 0; - if (last_roi.height < 0) - last_roi.height = 0; + if (last_roi.width < 1) + last_roi.width = 1; + if (last_roi.height < 1) + last_roi.height = 1; if (last_roi.x >= color.cols-1) last_roi.x = color.cols-1; if (last_roi.width >= color.cols-1) @@ -231,6 +235,9 @@ void Tracker::clamp_last_roi() last_roi.y = color.rows-1; if (last_roi.height >= color.rows-1) last_roi.height = color.rows-1; + + last_roi.width -= last_roi.x; + last_roi.height -= last_roi.y; } void Tracker::set_points() @@ -251,30 +258,30 @@ void Tracker::set_points() void Tracker::draw_centroid() { repr2.clear(); + + static const std::vector centroid { 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); + cv::circle(frame, repr2[0], 4, s, -1); } -bool Tracker::set_last_roi() +void Tracker::set_last_roi() { roi_projection.clear(); - if (!cv::solvePnP(obj_points, markers[0], intrinsics, dist_coeffs, rvec_, tvec_, false, cv::SOLVEPNP_ITERATIVE)) - return false; - + using f = float; + cv::Point3f h(f(s.headpos_x), f(s.headpos_y), f(s.headpos_z)); for (unsigned i = 0; i < 4; i++) { - using f = float; - cv::Point3f pt(obj_points[i] - cv::Point3f(f(s.headpos_x), f(s.headpos_y), f(s.headpos_z))); + cv::Point3f pt(obj_points[i] - h); roi_points[i] = pt * c_search_window; } - cv::projectPoints(roi_points, rvec_, tvec_, intrinsics, dist_coeffs, roi_projection); - set_roi_from_projection(); + cv::projectPoints(roi_points, rvec, tvec, intrinsics, dist_coeffs, roi_projection); - return true; + set_roi_from_projection(); } void Tracker::set_rmat() @@ -316,9 +323,6 @@ void Tracker::set_roi_from_projection() last_roi.height = max_y; } - last_roi.width -= last_roi.x; - last_roi.height -= last_roi.y; - clamp_last_roi(); } @@ -346,9 +350,11 @@ void Tracker::run() } cv::cvtColor(color, grayscale, cv::COLOR_RGB2GRAY); + color.copyTo(frame); set_intrinsics(); + update_fps(alpha_); markers.clear(); @@ -362,9 +368,7 @@ void Tracker::run() if (!cv::solvePnP(obj_points, markers[0], intrinsics, dist_coeffs, rvec, tvec, false, cv::SOLVEPNP_ITERATIVE)) goto fail; - if (!set_last_roi()) - goto fail; - + set_last_roi(); draw_centroid(); set_rmat(); } diff --git a/tracker-aruco/ftnoir_tracker_aruco.h b/tracker-aruco/ftnoir_tracker_aruco.h index fb207f6f..94a96e4c 100644 --- a/tracker-aruco/ftnoir_tracker_aruco.h +++ b/tracker-aruco/ftnoir_tracker_aruco.h @@ -70,7 +70,7 @@ private: void clamp_last_roi(); void set_points(); void draw_centroid(); - bool set_last_roi(); + void set_last_roi(); void set_rmat(); void set_roi_from_projection(); @@ -90,10 +90,9 @@ private: aruco::MarkerDetector detector; std::vector markers; cv::Vec3d t; - cv::Vec3d rvec, tvec, rvec_, tvec_; + cv::Vec3d rvec, tvec; std::vector roi_projection; std::vector repr2; - std::vector centroid; cv::Matx33d m_r, m_q, rmat; cv::Vec3d euler; std::vector roi_points; @@ -104,7 +103,7 @@ private: static constexpr float size_min = 0.05f; static constexpr float size_max = 0.3f; - static constexpr double alpha_ = .985; + static constexpr double alpha_ = .95; }; class TrackerControls : public ITrackerDialog, protected camera_dialog -- cgit v1.2.3