From 6594411a8a2cfc477e1b8162fe5c269b7d3b4810 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 7 Jun 2015 06:26:15 +0200 Subject: aruco: use DLS pose estimation --- ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) (limited to 'ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp') 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(x4,0)= -size + s.headpos_x; obj_points.at(x4,1)= size + s.headpos_y; obj_points.at(x4,2)= 0 + s.headpos_z; - - cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, roi_valid, cv::SOLVEPNP_ITERATIVE); - - std::vector roi_projection(4); + + std::vector 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 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 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); -- cgit v1.2.3