Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Don't add bad corners to multi-tag solution (#817)

Browse Source 
Closes #837


---------

Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com>
This commit is contained in:
Matt
2023-06-17 22:16:10 -07:00
committed by GitHub
parent b1546b8038
commit f6756bdb9a
3 changed files with 49 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java
View File

@@ -45,6 +45,7 @@ public class VisionEstimation {
* transformation. If only one tag is visible, the result may have an alternate solution.
*
* <p><b>Note:</b> The returned transformation is from the field origin to the camera pose!
* (Unless you only feed this one tag??)
*
* @param cameraMatrix the camera intrinsics matrix in standard opencv form
* @param distCoeffs the camera distortion matrix in standard opencv form
@@ -52,6 +53,7 @@ public class VisionEstimation {
* @param knownTags The known tag field poses corresponding to the visible tag IDs
* @return The transformation that maps the field origin to the camera pose
*/
@Deprecated
public static PNPResults estimateCamPosePNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
@@ -109,43 +111,40 @@ public class VisionEstimation {
}
}
// /**
// * The best estimated transformation to the target, and possibly an alternate
// * transformation
// * depending on the solvePNP method. If an alternate solution is present, the
// * ambiguity value
// * represents the ratio of reprojection error in the best solution to the
// * alternate (best / alternate).
// */
// public static class PNPResults {
// public final Transform3d best;
// public final double bestReprojErr;
// /**
// * Alternate, ambiguous solution from solvepnp. This may be empty
// * if no alternate solution is found.
// */
// public final Transform3d alt;
// /** If no alternate solution is found, this is 0 */
// public final double altReprojErr;
// /** If no alternate solution is found, this is 0 */
// public final double ambiguity;
// public PNPResults() {
// this(new Transform3d(), new Transform3d(), 0, 0, 0);
// }
// public PNPResults(
// Transform3d best, Transform3d alt,
// double ambiguity, double bestReprojErr, double altReprojErr) {
// this.best = best;
// this.alt = alt;
// this.ambiguity = ambiguity;
// this.bestReprojErr = bestReprojErr;
// this.altReprojErr = altReprojErr;
// }
// }
/**
* Performs solvePNP using 3d-2d point correspondences to estimate the field-to-camera
* transformation. If only one tag is visible, the result may have an alternate solution.
*
* <p><b>Note:</b> The returned transformation is from the field origin to the camera pose!
*
* @param cameraMatrix the camera intrinsics matrix in standard opencv form
* @param distCoeffs the camera distortion matrix in standard opencv form
* @param corners The visible tag corners in the 2d image
* @param knownTags The known tag field poses corresponding to the visible tag IDs
* @return The transformation that maps the field origin to the camera pose
*/
public static PNPResults estimateCamPoseSqpnp(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<TargetCorner> corners,
List<AprilTag> knownTags) {
if (knownTags == null
|| corners == null
|| corners.size() != knownTags.size() * 4
|| knownTags.size() == 0) {
return new PNPResults();
}
var objectTrls = new ArrayList<Translation3d>();
for (var tag : knownTags) objectTrls.addAll(kTagModel.getFieldVertices(tag.pose));
var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners);
// var camToOrigin = OpenCVHelp.solveTagsPNPRansac(prop, objectTrls, corners);
return new PNPResults(
camToOrigin.best.inverse(),
camToOrigin.alt.inverse(),
camToOrigin.ambiguity,
camToOrigin.bestReprojErr,
camToOrigin.altReprojErr);
}
/**
* The best estimated transformation (Rotation-translation composition) that maps a set of