diff options
| -rw-r--r-- | tracker-aruco/ftnoir_tracker_aruco.cpp | 62 | ||||
| -rw-r--r-- | tracker-aruco/ftnoir_tracker_aruco.h | 7 |
2 files changed, 36 insertions, 33 deletions
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<cv::Point3f> 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<aruco::Marker> markers; cv::Vec3d t; - cv::Vec3d rvec, tvec, rvec_, tvec_; + cv::Vec3d rvec, tvec; std::vector<cv::Point2f> roi_projection; std::vector<cv::Point2f> repr2; - std::vector<cv::Point3f> centroid; cv::Matx33d m_r, m_q, rmat; cv::Vec3d euler; std::vector<cv::Point3f> 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 |