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

Allow opencv8 distortion model in PhotonCamera (#1317)

Browse Source 
We previously assumed only OpenCV5 but mrcal uses opencv8
This commit is contained in:
Matt
2024-05-29 17:28:35 -04:00
committed by GitHub
parent fcca858a37
commit 19b4802094
17 changed files with 172 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -36,7 +36,7 @@ import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.math.numbers.N8;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.ArrayList;
import java.util.HashSet;
@@ -348,7 +348,7 @@ public class PhotonPoseEstimator {
public Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N5, N1>> distCoeffs) {
Optional<Matrix<N8, N1>> distCoeffs) {
// Time in the past -- give up, since the following if expects times > 0
if (cameraResult.getTimestampSeconds() < 0) {
return Optional.empty();
@@ -376,7 +376,7 @@ public class PhotonPoseEstimator {
private Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N5, N1>> distCoeffs,
Optional<Matrix<N8, N1>> distCoeffs,
PoseStrategy strat) {
Optional<EstimatedRobotPose> estimatedPose;
switch (strat) {
@@ -418,7 +418,7 @@ public class PhotonPoseEstimator {
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N5, N1>> distCoeffsOpt) {
Optional<Matrix<N8, N1>> distCoeffsOpt) {
if (result.getMultiTagResult().estimatedPose.isPresent) {
var best_tf = result.getMultiTagResult().estimatedPose.best;
var best =
@@ -440,7 +440,7 @@ public class PhotonPoseEstimator {
private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N5, N1>> distCoeffsOpt) {
Optional<Matrix<N8, N1>> distCoeffsOpt) {
boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
// cannot run multitagPNP, use fallback strategy
if (!hasCalibData || result.getTargets().size() < 2) {