diff options
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 9 |
1 files changed, 5 insertions, 4 deletions
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp index 4fa4fffe..fc820d56 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp @@ -233,10 +233,10 @@ void Tracker::run() obj_points.at<float>(x4,0)= -size + s.headpos_x; obj_points.at<float>(x4,1)= size + s.headpos_y; obj_points.at<float>(x4,2)= 0 + s.headpos_z; - - cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, roi_valid, cv::SOLVEPNP_ITERATIVE); - - std::vector<cv::Point2f> roi_projection(4); + + std::vector<cv::Point2f> img_points = m; + cv::solvePnP(obj_points, img_points, intrinsics, dist_coeffs, rvec, tvec, false, cv::SOLVEPNP_DLS); + cv::solvePnP(obj_points, img_points, intrinsics, dist_coeffs, rvec, tvec, true, cv::SOLVEPNP_ITERATIVE); { std::vector<cv::Point2f> repr2; @@ -261,6 +261,7 @@ void Tracker::run() cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec_, tvec_, false, cv::SOLVEPNP_ITERATIVE); cv::Mat roi_points = obj_points * c_search_window; + std::vector<cv::Point2f> roi_projection(4); cv::projectPoints(roi_points, rvec_, tvec_, intrinsics, dist_coeffs, roi_projection); last_roi = cv::Rect(color.cols-1, color.rows-1, 0, 0); |