CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

Revert "aruco_map: Use two-pass solvePnP"

Browse Source 
This reverts commit 91f6f6dd32. Additional testing revealed this "fix" to provide incorrect results.
This commit is contained in:
sfalexrog
2019-05-28 20:54:49 +03:00
parent 41e9f407fd
commit 8019712d8c
1 changed files with 1 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
aruco_pose/src/aruco_map.cpp
View File

@@ -179,13 +179,7 @@ public:
double center_x = 0, center_y = 0, center_z = 0;
alignObjPointsToCenter(obj_points, center_x, center_y, center_z);
// Step 1: Solve using EPnP
valid = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false, cv::SOLVEPNP_EPNP);
// Step 2: Use iterative method to refine results
valid &= solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, true);
// Step 3: Check tvec magnitude. Iterative method tends to diverge sometimes, and this divergence is not picked up
// by OpenCV code
valid &= norm(tvec) < 1e6;
valid = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false);
if (!valid) goto publish_debug;
fillTransform(transform_.transform, rvec, tvec);