diff options
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 51 |
1 files changed, 16 insertions, 35 deletions
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp index 9f85fcce..793469e2 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp @@ -119,21 +119,18 @@ void Tracker::run() auto freq = cv::getTickFrequency(); auto last_time = cv::getTickCount(); - int cur_fps = 0; - int last_fps = 0; - + double cur_fps = 0; std::vector<int> box_sizes { 5, 7, 9, 11 }; int box_idx = 0; double failed = 0; - const double max_failed = .334; + const double max_failed = 1.25; + cv::Vec3d rvec, tvec; while (!stop) { cv::Mat color; if (!camera.read(color)) continue; - auto tm = cv::getTickCount(); - cv::Mat grayscale; cv::cvtColor(color, grayscale, cv::COLOR_RGB2GRAY); @@ -156,7 +153,13 @@ void Tracker::run() const double size_max = 0.4; bool roi_valid = false; - + + auto time = cv::getTickCount(); + + const double dt = (time - last_time) / freq; + last_time = time; + cur_fps = cur_fps * 0.97 + 0.03 * (dt == 0 ? 0 : 1./dt); + if (last_roi.width > 0 && last_roi.height) { detector.setThresholdParams(box_sizes[box_idx], 5); @@ -166,7 +169,7 @@ void Tracker::run() if (detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false), markers.size() == 1 && markers[0].size() == 4) { - failed = 0; + failed = std::max(0., failed - dt); auto& m = markers.at(0); for (int i = 0; i < 4; i++) { @@ -177,17 +180,6 @@ void Tracker::run() roi_valid = true; } } - - auto time = cv::getTickCount(); - - if ((long) (time / freq) != (long) (last_time / freq)) - { - last_fps = cur_fps; - cur_fps = 0; - last_time = time; - } - - const double dt = (time - last_time) / freq; if (!roi_valid) { @@ -209,16 +201,12 @@ void Tracker::run() cv::line(color, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), scale, 8); } - cur_fps++; - char buf[128]; frame = color.clone(); - ::sprintf(buf, "Hz: %d", last_fps); + ::sprintf(buf, "Hz: %d", (int)cur_fps); cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(0, 255, 0), scale*2); - ::sprintf(buf, "Jiffies: %ld", (long) (10000 * (time - tm) / freq)); - cv::putText(frame, buf, cv::Point(10, 54), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(80, 255, 0), scale*2); if (markers.size() == 1 && markers[0].size() == 4) { const auto& m = markers.at(0); @@ -242,8 +230,6 @@ void Tracker::run() obj_points.at<float>(x4,1)= size + s.headpos_y; obj_points.at<float>(x4,2)= 0 + s.headpos_z; - cv::Vec3d rvec, tvec; - cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, CV_ITERATIVE); std::vector<cv::Point2f> roi_projection(4); @@ -267,14 +253,11 @@ void Tracker::run() obj_points.at<float>(i, 2) -= s.headpos_z; } - { - cv::Mat rvec_, tvec_; - cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec_, tvec_, false, CV_ITERATIVE); - tvec = tvec_; - } + cv::Mat rvec_, tvec_; + cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec_, tvec_, false, CV_ITERATIVE); cv::Mat roi_points = obj_points * c_search_window; - cv::projectPoints(roi_points, rvec, tvec, intrinsics, dist_coeffs, roi_projection); + cv::projectPoints(roi_points, rvec_, tvec_, intrinsics, dist_coeffs, roi_projection); last_roi = cv::Rect(color.cols-1, color.rows-1, 0, 0); @@ -317,8 +300,6 @@ void Tracker::run() QMutexLocker lck(&mtx); - tvec[1] *= -1; - for (int i = 0; i < 3; i++) pose[i] = tvec(i); pose[Yaw] = euler[1]; @@ -326,7 +307,7 @@ void Tracker::run() pose[Roll] = euler[2]; r = rmat; - t = tvec; + t = cv::Vec3d(tvec[0], -tvec[1], tvec[2]); } if (roi_valid) |