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

aruco_map: Use two-pass solvePnP

Browse Source 
There are cases when iterative solvePnP method converges on a "wrong" camera position due to projectPoints not treating negative Z values properly.
Other methods don't seem to be affected by that, but their results differ from the iterative method slightly. By combining these two methods we
"nudge" the iterative method towards the correct camera position and get satisfactory results most of the time.

Sometimes, though, even with the "nudge" the iterative method diverges catastrophically, and this is not caught by the solver. We work around that
by assuming our camera position cannot be too far from the markers.
This commit is contained in:
sfalexrog
2019-05-20 12:10:46 +03:00
parent ec57f7d0a3
commit 91f6f6dd32
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);