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

Merge pull request #125 from sfalexrog/WIP/aruco_fix

Browse Source 
aruco_map: Use two-pass solvePnP
This commit is contained in:
Oleg Kalachev
2019-05-22 15:19:42 +03:00
committed by GitHub
parent 78f43ad078 91f6f6dd32
commit 0af7d406bd
1 changed files with 7 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
aruco_pose/src/aruco_map.cpp
View File

@@ -179,7 +179,13 @@ public:
double center_x = 0, center_y = 0, center_z = 0;
alignObjPointsToCenter(obj_points, center_x, center_y, center_z);
valid = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false);
// 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;
if (!valid) goto publish_debug;
fillTransform(transform_.transform, rvec, tvec);