mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
Bump wpilib to 2024-beta-4 and report resource photon usage ids from 2024v2 image (#1042)
This commit is contained in:
Sriman Achanta
@@ -24,6 +24,8 @@
|
||||
|
||||
package org.photonvision;
|
||||
|
||||
import edu.wpi.first.hal.FRCNetComm.tResourceType;
|
||||
import edu.wpi.first.hal.HAL;
|
||||
import edu.wpi.first.math.MatBuilder;
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Nat;
|
||||
@@ -52,6 +54,7 @@ import org.photonvision.targeting.PhotonPipelineResult;
|
||||
|
||||
/** Represents a camera that is connected to PhotonVision. */
|
||||
public class PhotonCamera implements AutoCloseable {
|
||||
private static int InstanceCount = 0;
|
||||
public static final String kTableName = "photonvision";
|
||||
|
||||
private final NetworkTable cameraTable;
|
||||
@@ -152,6 +155,9 @@ public class PhotonCamera implements AutoCloseable {
|
||||
MultiSubscriber m_topicNameSubscriber =
|
||||
new MultiSubscriber(
|
||||
instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true));
|
||||
|
||||
HAL.report(tResourceType.kResourceType_PhotonCamera, InstanceCount);
|
||||
InstanceCount++;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -321,14 +327,14 @@ public class PhotonCamera implements AutoCloseable {
|
||||
public Optional<Matrix<N3, N3>> getCameraMatrix() {
|
||||
var cameraMatrix = cameraIntrinsicsSubscriber.get();
|
||||
if (cameraMatrix != null && cameraMatrix.length == 9) {
|
||||
return Optional.of(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(cameraMatrix));
|
||||
return Optional.of(MatBuilder.fill(Nat.N3(), Nat.N3(), cameraMatrix));
|
||||
} else return Optional.empty();
|
||||
}
|
||||
|
||||
public Optional<Matrix<N5, N1>> getDistCoeffs() {
|
||||
var distCoeffs = cameraDistortionSubscriber.get();
|
||||
if (distCoeffs != null && distCoeffs.length == 5) {
|
||||
return Optional.of(new MatBuilder<>(Nat.N5(), Nat.N1()).fill(distCoeffs));
|
||||
return Optional.of(MatBuilder.fill(Nat.N5(), Nat.N1(), distCoeffs));
|
||||
} else return Optional.empty();
|
||||
}
|
||||
|
||||
|
||||
@@ -25,6 +25,8 @@
|
||||
package org.photonvision;
|
||||
|
||||
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
||||
import edu.wpi.first.hal.FRCNetComm.tResourceType;
|
||||
import edu.wpi.first.hal.HAL;
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Pair;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
@@ -52,6 +54,8 @@ import org.photonvision.targeting.PhotonTrackedTarget;
|
||||
* below. Example usage can be found in our apriltagExample example project.
|
||||
*/
|
||||
public class PhotonPoseEstimator {
|
||||
private static int InstanceCount = 0;
|
||||
|
||||
/** Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. */
|
||||
public enum PoseStrategy {
|
||||
/** Choose the Pose with the lowest ambiguity. */
|
||||
@@ -118,14 +122,14 @@ public class PhotonPoseEstimator {
|
||||
this.primaryStrategy = strategy;
|
||||
this.camera = camera;
|
||||
this.robotToCamera = robotToCamera;
|
||||
|
||||
HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount);
|
||||
InstanceCount++;
|
||||
}
|
||||
|
||||
public PhotonPoseEstimator(
|
||||
AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
|
||||
this.fieldTags = fieldTags;
|
||||
this.primaryStrategy = strategy;
|
||||
this.camera = null;
|
||||
this.robotToCamera = robotToCamera;
|
||||
this(fieldTags, strategy, null, robotToCamera);
|
||||
}
|
||||
|
||||
/** Invalidates the pose cache. */
|
||||
|
||||
@@ -25,6 +25,7 @@
|
||||
package org.photonvision.simulation;
|
||||
|
||||
import com.fasterxml.jackson.databind.ObjectMapper;
|
||||
import edu.wpi.first.math.MatBuilder;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.Nat;
|
||||
@@ -146,8 +147,8 @@ public class SimCameraProperties {
|
||||
setCalibration(
|
||||
jsonWidth,
|
||||
jsonHeight,
|
||||
Matrix.mat(Nat.N3(), Nat.N3()).fill(jsonIntrinsics),
|
||||
Matrix.mat(Nat.N5(), Nat.N1()).fill(jsonDistortion));
|
||||
MatBuilder.fill(Nat.N3(), Nat.N3(), jsonIntrinsics),
|
||||
MatBuilder.fill(Nat.N5(), Nat.N1(), jsonDistortion));
|
||||
setCalibError(jsonAvgError, jsonErrorStdDev);
|
||||
success = true;
|
||||
}
|
||||
@@ -184,7 +185,7 @@ public class SimCameraProperties {
|
||||
double fy = cy / Math.tan(fovHeight.getRadians() / 2.0);
|
||||
|
||||
// create camera intrinsics matrix
|
||||
var camIntrinsics = Matrix.mat(Nat.N3(), Nat.N3()).fill(fx, 0, cx, 0, fy, cy, 0, 0, 1);
|
||||
var camIntrinsics = MatBuilder.fill(Nat.N3(), Nat.N3(), fx, 0, cx, 0, fy, cy, 0, 0, 1);
|
||||
setCalibration(resWidth, resHeight, camIntrinsics, distCoeff);
|
||||
}
|
||||
|
||||
@@ -597,17 +598,19 @@ public class SimCameraProperties {
|
||||
prop.setCalibration(
|
||||
320,
|
||||
240,
|
||||
Matrix.mat(Nat.N3(), Nat.N3())
|
||||
.fill( // intrinsic
|
||||
328.2733242048587,
|
||||
0.0,
|
||||
164.8190261141906,
|
||||
0.0,
|
||||
318.0609794305216,
|
||||
123.8633838438093,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0),
|
||||
MatBuilder.fill(
|
||||
Nat.N3(),
|
||||
Nat.N3(),
|
||||
// intrinsic
|
||||
328.2733242048587,
|
||||
0.0,
|
||||
164.8190261141906,
|
||||
0.0,
|
||||
318.0609794305216,
|
||||
123.8633838438093,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0),
|
||||
VecBuilder.fill( // distort
|
||||
0.09957946553445934,
|
||||
-0.9166265114485799,
|
||||
@@ -626,17 +629,19 @@ public class SimCameraProperties {
|
||||
prop.setCalibration(
|
||||
640,
|
||||
480,
|
||||
Matrix.mat(Nat.N3(), Nat.N3())
|
||||
.fill( // intrinsic
|
||||
669.1428078983059,
|
||||
0.0,
|
||||
322.53377249329213,
|
||||
0.0,
|
||||
646.9843137061716,
|
||||
241.26567383784163,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0),
|
||||
MatBuilder.fill(
|
||||
Nat.N3(),
|
||||
Nat.N3(),
|
||||
// intrinsic
|
||||
669.1428078983059,
|
||||
0.0,
|
||||
322.53377249329213,
|
||||
0.0,
|
||||
646.9843137061716,
|
||||
241.26567383784163,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0),
|
||||
VecBuilder.fill( // distort
|
||||
0.12788470750464645,
|
||||
-1.2350335805796528,
|
||||
@@ -655,17 +660,18 @@ public class SimCameraProperties {
|
||||
prop.setCalibration(
|
||||
640,
|
||||
480,
|
||||
Matrix.mat(Nat.N3(), Nat.N3())
|
||||
.fill( // intrinsic
|
||||
511.22843367007755,
|
||||
0.0,
|
||||
323.62049380211096,
|
||||
0.0,
|
||||
514.5452336723849,
|
||||
261.8827920543568,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0),
|
||||
MatBuilder.fill(
|
||||
Nat.N3(),
|
||||
Nat.N3(), // intrinsic
|
||||
511.22843367007755,
|
||||
0.0,
|
||||
323.62049380211096,
|
||||
0.0,
|
||||
514.5452336723849,
|
||||
261.8827920543568,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0),
|
||||
VecBuilder.fill( // distort
|
||||
0.1917469998873756,
|
||||
-0.5142936883324216,
|
||||
@@ -684,17 +690,19 @@ public class SimCameraProperties {
|
||||
prop.setCalibration(
|
||||
960,
|
||||
720,
|
||||
Matrix.mat(Nat.N3(), Nat.N3())
|
||||
.fill( // intrinsic
|
||||
769.6873145148892,
|
||||
0.0,
|
||||
486.1096609458122,
|
||||
0.0,
|
||||
773.8164483705323,
|
||||
384.66071662358354,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0),
|
||||
MatBuilder.fill(
|
||||
Nat.N3(),
|
||||
Nat.N3(),
|
||||
// intrinsic
|
||||
769.6873145148892,
|
||||
0.0,
|
||||
486.1096609458122,
|
||||
0.0,
|
||||
773.8164483705323,
|
||||
384.66071662358354,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0),
|
||||
VecBuilder.fill( // distort
|
||||
0.189462064814501,
|
||||
-0.49903003669627627,
|
||||
@@ -713,17 +721,19 @@ public class SimCameraProperties {
|
||||
prop.setCalibration(
|
||||
1280,
|
||||
720,
|
||||
Matrix.mat(Nat.N3(), Nat.N3())
|
||||
.fill( // intrinsic
|
||||
1011.3749416937393,
|
||||
0.0,
|
||||
645.4955139388737,
|
||||
0.0,
|
||||
1008.5391755084075,
|
||||
508.32877656020196,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0),
|
||||
MatBuilder.fill(
|
||||
Nat.N3(),
|
||||
Nat.N3(),
|
||||
// intrinsic
|
||||
1011.3749416937393,
|
||||
0.0,
|
||||
645.4955139388737,
|
||||
0.0,
|
||||
1008.5391755084075,
|
||||
508.32877656020196,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0),
|
||||
VecBuilder.fill( // distort
|
||||
0.13730101577061535,
|
||||
-0.2904345656989261,
|
||||
|
||||
Reference in New Issue
Block a user