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

Auto-generate packet dataclasses with Jinja (#1374)

Browse Source
This commit is contained in:
Matt
2024-08-31 13:44:19 -04:00
committed by GitHub
parent c19d54c633
commit 169595e56e
140 changed files with 4445 additions and 2097 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View File

@@ -31,6 +31,7 @@ import edu.wpi.first.math.numbers.*;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.Optional;
import org.ejml.simple.SimpleMatrix;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.Core;
@@ -46,7 +47,7 @@ import org.opencv.core.Point3;
import org.opencv.core.Rect;
import org.opencv.core.RotatedRect;
import org.opencv.imgproc.Imgproc;
import org.photonvision.targeting.PNPResult;
import org.photonvision.targeting.PnpResult;
import org.photonvision.targeting.TargetCorner;
public final class OpenCVHelp {
@@ -402,7 +403,7 @@ public final class OpenCVHelp {
* @return The resulting transformation that maps the camera pose to the target pose and the
* ambiguity if an alternate solution is available.
*/
public static PNPResult solvePNP_SQUARE(
public static Optional<PnpResult> solvePNP_SQUARE(
Matrix<N3, N3> cameraMatrix,
Matrix<N8, N1> distCoeffs,
List<Translation3d> modelTrls,
@@ -467,14 +468,15 @@ public final class OpenCVHelp {
// check if solvePnP failed with NaN results and retrying failed
if (Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result");
if (alt != null) return new PNPResult(best, alt, errors[0] / errors[1], errors[0], errors[1]);
else return new PNPResult(best, errors[0]);
if (alt != null)
return Optional.of(new PnpResult(best, alt, errors[0] / errors[1], errors[0], errors[1]));
else return Optional.empty();
}
// solvePnP failed
catch (Exception e) {
System.err.println("SolvePNP_SQUARE failed!");
e.printStackTrace();
return new PNPResult();
return Optional.empty();
} finally {
// release our Mats from native memory
objectMat.release();
@@ -509,7 +511,7 @@ public final class OpenCVHelp {
* model points are supplied relative to the origin, this transformation brings the camera to
* the origin.
*/
public static PNPResult solvePNP_SQPNP(
public static Optional<PnpResult> solvePNP_SQPNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N8, N1> distCoeffs,
List<Translation3d> objectTrls,
@@ -558,11 +560,11 @@ public final class OpenCVHelp {
// check if solvePnP failed with NaN results
if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result");
return new PNPResult(best, error[0]);
return Optional.of(new PnpResult(best, error[0]));
} catch (Exception e) {
System.err.println("SolvePNP_SQPNP failed!");
e.printStackTrace();
return new PNPResult();
return Optional.empty();
}
}
}