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

Merge branch 'main' into py-docs

Browse Source
This commit is contained in:
Sam Freund
2025-10-23 16:14:46 -05:00
committed by GitHub
parent 46e71703ef 7cb3b7a37b
commit 99ca8228a1
223 changed files with 12559 additions and 11613 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
photon-lib/build.gradle
View File

@@ -282,7 +282,7 @@ if (!project.hasProperty('copyOfflineArtifacts')) {
artifactId = "${nativeName}-json"
groupId = "org.photonvision"
version "1.0"
version = "1.0"
}
}
}
@@ -344,8 +344,8 @@ publishing {
artifact combinedHeadersZip
artifactId = "${nativeName}-combinedcpp"
groupId artifactGroupId
version pubVersion
groupId = artifactGroupId
version = pubVersion
}
}
}
@@ -363,6 +363,9 @@ def nativeTasks = wpilibTools.createExtractionTasks {
nativeTasks.addToSourceSetResources(sourceSets.test)
dependencies {
wpilibNatives project(":photon-targeting")
}
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
nativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")

6
photon-lib/py/buildAndTest.sh
View File

@@ -1,7 +1,13 @@
#!/usr/bin/env bash
set -euo pipefail
cd -- "$(dirname -- "$0")"
# Uninstall if it already was installed
python3 -m pip uninstall -y photonlibpy
# Build wheel
python3 -m pip install wheel
python3 setup.py bdist_wheel
# Install whatever wheel was made

10
photon-lib/py/photonlibpy/photonCamera.py
View File

@@ -18,6 +18,7 @@
from enum import Enum
from typing import List
import hal
import ntcore
# magical import to make serde stuff work
@@ -48,6 +49,8 @@ def setVersionCheckEnabled(enabled: bool):
class PhotonCamera:
instance_count = 1
def __init__(self, cameraName: str):
"""Constructs a PhotonCamera from the name of the camera.
@@ -108,6 +111,13 @@ class PhotonCamera:
# Start the time sync server
inst.start()
# Usage reporting
hal.report(
hal.tResourceType.kResourceType_PhotonCamera.value,
PhotonCamera.instance_count,
)
PhotonCamera.instance_count += 1
def getAllUnreadResults(self) -> List[PhotonPipelineResult]:
"""
The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults().

110
photon-lib/py/photonlibpy/photonPoseEstimator.py
View File

@@ -18,9 +18,20 @@
import enum
from typing import Optional
import hal
import wpilib
import wpimath.units
from robotpy_apriltag import AprilTagFieldLayout
from wpimath.geometry import Pose2d, Pose3d, Transform3d
from wpimath.geometry import (
Pose2d,
Pose3d,
Rotation2d,
Rotation3d,
Transform3d,
Translation2d,
Translation3d,
)
from wpimath.interpolation import TimeInterpolatableRotation2dBuffer
from .estimatedRobotPose import EstimatedRobotPose
from .photonCamera import PhotonCamera
@@ -59,8 +70,21 @@ class PoseStrategy(enum.Enum):
This runs on the RoboRIO, and can take a lot of time.
"""
PNP_DISTANCE_TRIG_SOLVE = enum.auto()
"""
Use distance data from best visible tag to compute a Pose. This runs on
the RoboRIO in order to access the robot's yaw heading, and MUST have
addHeadingData called every frame so heading data is up-to-date.
Produces a Pose2d in estimatedRobotPose (0 for z, roll, pitch).
See https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
"""
class PhotonPoseEstimator:
instance_count = 1
"""
The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a
given timestamp on the field to produce a single robot in field pose, using the strategy set
@@ -95,8 +119,14 @@ class PhotonPoseEstimator:
self._poseCacheTimestampSeconds = -1.0
self._lastPose: Optional[Pose3d] = None
self._referencePose: Optional[Pose3d] = None
self._headingBuffer = TimeInterpolatableRotation2dBuffer(1)
# TODO: Implement HAL reporting
# Usage reporting
hal.report(
hal.tResourceType.kResourceType_PhotonPoseEstimator.value,
PhotonPoseEstimator.instance_count,
)
PhotonPoseEstimator.instance_count += 1
@property
def fieldTags(self) -> AprilTagFieldLayout:
@@ -199,9 +229,35 @@ class PhotonPoseEstimator:
self._poseCacheTimestampSeconds = -1.0
def _checkUpdate(self, oldObj, newObj) -> None:
if oldObj != newObj and oldObj is not None and oldObj is not newObj:
if oldObj != newObj:
self._invalidatePoseCache()
def addHeadingData(
self, timestampSeconds: wpimath.units.seconds, heading: Rotation2d | Rotation3d
) -> None:
"""
Add robot heading data to buffer. Must be called periodically for the **PNP_DISTANCE_TRIG_SOLVE** strategy.
:param timestampSeconds :timestamp of the robot heading data
:param heading: field-relative robot heading at given timestamp
"""
if isinstance(heading, Rotation3d):
heading = heading.toRotation2d()
self._headingBuffer.addSample(timestampSeconds, heading)
def resetHeadingData(
self, timestampSeconds: wpimath.units.seconds, heading: Rotation2d | Rotation3d
) -> None:
"""
Clears all heading data in the buffer, and adds a new seed. Useful for preventing estimates
from utilizing heading data provided prior to a pose or rotation reset.
:param timestampSeconds: timestamp of the robot heading data
:param heading: field-relative robot heading at given timestamp
"""
self._headingBuffer.clear()
self.addHeadingData(timestampSeconds, heading)
def update(
self, cameraResult: Optional[PhotonPipelineResult] = None
) -> Optional[EstimatedRobotPose]:
@@ -255,6 +311,8 @@ class PhotonPoseEstimator:
estimatedPose = self._lowestAmbiguityStrategy(cameraResult)
elif strat is PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR:
estimatedPose = self._multiTagOnCoprocStrategy(cameraResult)
elif strat is PoseStrategy.PNP_DISTANCE_TRIG_SOLVE:
estimatedPose = self._pnpDistanceTrigSolveStrategy(cameraResult)
else:
wpilib.reportError(
"[PhotonPoseEstimator] Unknown Position Estimation Strategy!", False
@@ -266,6 +324,52 @@ class PhotonPoseEstimator:
return estimatedPose
def _pnpDistanceTrigSolveStrategy(
self, result: PhotonPipelineResult
) -> Optional[EstimatedRobotPose]:
if (bestTarget := result.getBestTarget()) is None:
return None
if (
headingSample := self._headingBuffer.sample(result.getTimestampSeconds())
) is None:
return None
if (tagPose := self._fieldTags.getTagPose(bestTarget.fiducialId)) is None:
return None
camToTagTranslation = (
Translation3d(
bestTarget.getBestCameraToTarget().translation().norm(),
Rotation3d(
0,
-wpimath.units.degreesToRadians(bestTarget.getPitch()),
-wpimath.units.degreesToRadians(bestTarget.getYaw()),
),
)
.rotateBy(self.robotToCamera.rotation())
.toTranslation2d()
.rotateBy(headingSample)
)
fieldToCameraTranslation = (
tagPose.toPose2d().translation() - camToTagTranslation
)
camToRobotTranslation: Translation2d = -(
self.robotToCamera.translation().toTranslation2d()
)
camToRobotTranslation = camToRobotTranslation.rotateBy(headingSample)
robotPose = Pose2d(
fieldToCameraTranslation + camToRobotTranslation, headingSample
)
return EstimatedRobotPose(
Pose3d(robotPose),
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.PNP_DISTANCE_TRIG_SOLVE,
)
def _multiTagOnCoprocStrategy(
self, result: PhotonPipelineResult
) -> Optional[EstimatedRobotPose]:

11
photon-lib/py/photonlibpy/simulation/photonCameraSim.py
View File

@@ -420,14 +420,15 @@ class PhotonCameraSim:
# put this simulated data to NT
self.heartbeatCounter += 1
now_micros = wpilib.Timer.getFPGATimestamp() * 1e6
publishTimestampMicros = wpilib.Timer.getFPGATimestamp() * 1e6
return PhotonPipelineResult(
ntReceiveTimestampMicros=int(publishTimestampMicros + 10),
metadata=PhotonPipelineMetadata(
int(now_micros - latency * 1e6),
int(now_micros),
self.heartbeatCounter,
captureTimestampMicros=int(publishTimestampMicros - latency * 1e6),
publishTimestampMicros=int(publishTimestampMicros),
sequenceID=self.heartbeatCounter,
# Pretend like we heard a pong recently
int(np.random.uniform(950, 1050)),
timeSinceLastPong=int(np.random.uniform(950, 1050)),
),
targets=detectableTgts,
multitagResult=multiTagResults,

11
photon-lib/py/photonlibpy/targeting/photonPipelineResult.py
View File

@@ -47,13 +47,10 @@ class PhotonPipelineResult:
timestamp, coproc timebase))
"""
# TODO - we don't trust NT4 to correctly latency-compensate ntReceiveTimestampMicros
return (
self.ntReceiveTimestampMicros
- (
self.metadata.publishTimestampMicros
- self.metadata.captureTimestampMicros
)
) / 1e6
latency = (
self.metadata.publishTimestampMicros - self.metadata.captureTimestampMicros
)
return (self.ntReceiveTimestampMicros - latency) / 1e6
def getTargets(self) -> list[PhotonTrackedTarget]:
return self.targets

0
photon-lib/py/test/__init__.py Normal file
View File

160
photon-lib/py/test/photonPoseEstimator_test.py
View File

@@ -15,7 +15,12 @@
## along with this program. If not, see <https://www.gnu.org/licenses/>.
###############################################################################
from photonlibpy import PhotonPoseEstimator, PoseStrategy
from test import testUtil
import wpimath.units
from photonlibpy import PhotonCamera, PhotonPoseEstimator, PoseStrategy
from photonlibpy.estimation import TargetModel
from photonlibpy.simulation import PhotonCameraSim, SimCameraProperties, VisionTargetSim
from photonlibpy.targeting import (
PhotonPipelineMetadata,
PhotonTrackedTarget,
@@ -27,14 +32,17 @@ from robotpy_apriltag import AprilTag, AprilTagFieldLayout
from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
class PhotonCameraInjector:
class PhotonCameraInjector(PhotonCamera):
result: PhotonPipelineResult
def __init__(self, cameraName="camera"):
super().__init__(cameraName)
def getLatestResult(self) -> PhotonPipelineResult:
return self.result
def setupCommon() -> AprilTagFieldLayout:
def fakeAprilTagFieldLayout() -> AprilTagFieldLayout:
tagList = []
tagPoses = (
Pose3d(3, 3, 3, Rotation3d()),
@@ -53,8 +61,7 @@ def setupCommon() -> AprilTagFieldLayout:
def test_lowestAmbiguityStrategy():
aprilTags = setupCommon()
aprilTags = fakeAprilTagFieldLayout()
cameraOne = PhotonCameraInjector()
cameraOne.result = PhotonPipelineResult(
int(11 * 1e6),
@@ -146,6 +153,86 @@ def test_lowestAmbiguityStrategy():
assertEquals(2, pose.z, 0.01)
def test_pnpDistanceTrigSolve():
aprilTags = fakeAprilTagFieldLayout()
cameraOne = PhotonCameraInjector()
latencySecs: wpimath.units.seconds = 1
fakeTimestampSecs: wpimath.units.seconds = 9 + latencySecs
cameraOneSim = PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG())
simTargets = [
VisionTargetSim(tag.pose, TargetModel.AprilTag36h11(), tag.ID)
for tag in aprilTags.getTags()
]
# Compound Rolled + Pitched + Yaw
compoundTestTransform = Transform3d(
-wpimath.units.inchesToMeters(12),
-wpimath.units.inchesToMeters(11),
3,
Rotation3d(
wpimath.units.degreesToRadians(37),
wpimath.units.degreesToRadians(6),
wpimath.units.degreesToRadians(60),
),
)
estimator = PhotonPoseEstimator(
aprilTags,
PoseStrategy.PNP_DISTANCE_TRIG_SOLVE,
cameraOne,
compoundTestTransform,
)
realPose = Pose3d(7.3, 4.42, 0, Rotation3d(0, 0, 2.197)) # Pose to compare with
result = cameraOneSim.process(
latencySecs, realPose.transformBy(estimator.robotToCamera), simTargets
)
bestTarget = result.getBestTarget()
assert bestTarget is not None
assert bestTarget.fiducialId == 0
assert result.ntReceiveTimestampMicros > 0
# Make test independent of the FPGA time.
result.ntReceiveTimestampMicros = int(fakeTimestampSecs * 1e6)
estimator.addHeadingData(
result.getTimestampSeconds(), realPose.rotation().toRotation2d()
)
estimatedRobotPose = estimator.update(result)
assert estimatedRobotPose is not None
pose = estimatedRobotPose.estimatedPose
assertEquals(realPose.x, pose.x, 0.01)
assertEquals(realPose.y, pose.y, 0.01)
assertEquals(0.0, pose.z, 0.01)
# Straight on
fakeTimestampSecs += 60
straightOnTestTransform = Transform3d(0, 0, 3, Rotation3d())
estimator.robotToCamera = straightOnTestTransform
realPose = Pose3d(4.81, 2.38, 0, Rotation3d(0, 0, 2.818)) # Pose to compare with
result = cameraOneSim.process(
latencySecs, realPose.transformBy(estimator.robotToCamera), simTargets
)
bestTarget = result.getBestTarget()
assert bestTarget is not None
assert bestTarget.fiducialId == 0
assert result.ntReceiveTimestampMicros > 0
# Make test independent of the FPGA time.
result.ntReceiveTimestampMicros = int(fakeTimestampSecs * 1e6)
estimator.addHeadingData(
result.getTimestampSeconds(), realPose.rotation().toRotation2d()
)
estimatedRobotPose = estimator.update(result)
assert estimatedRobotPose is not None
pose = estimatedRobotPose.estimatedPose
assertEquals(realPose.x, pose.x, 0.01)
assertEquals(realPose.y, pose.y, 0.01)
assertEquals(0.0, pose.z, 0.01)
def test_multiTagOnCoprocStrategy():
cameraOne = PhotonCameraInjector()
cameraOne.result = PhotonPipelineResult(
@@ -202,11 +289,38 @@ def test_multiTagOnCoprocStrategy():
def test_cacheIsInvalidated():
aprilTags = setupCommon()
aprilTags = fakeAprilTagFieldLayout()
cameraOne = PhotonCameraInjector()
estimator = PhotonPoseEstimator(
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
)
# Initial state, expect no timestamp.
assertEquals(-1, estimator._poseCacheTimestampSeconds)
# First result is 17s after epoch start.
timestamps = testUtil.PipelineTimestamps(captureTimestampMicros=17_000_000)
latencySecs = timestamps.pipelineLatencySecs()
# No targets, expect empty result
cameraOne.result = PhotonPipelineResult(
timestamps.receiveTimestampMicros(),
metadata=timestamps.toPhotonPipelineMetadata(),
)
estimatedPose = estimator.update()
assert estimatedPose is None
assertEquals(
timestamps.receiveTimestampMicros() * 1e-6 - latencySecs,
estimator._poseCacheTimestampSeconds,
1e-3,
)
# Set actual result
timestamps.incrementTimeMicros(2_500_000)
result = PhotonPipelineResult(
int(20 * 1e6),
timestamps.receiveTimestampMicros(),
[
PhotonTrackedTarget(
3.0,
@@ -231,31 +345,21 @@ def test_cacheIsInvalidated():
0.7,
)
],
metadata=PhotonPipelineMetadata(0, int(2 * 1e3), 0),
metadata=timestamps.toPhotonPipelineMetadata(),
)
estimator = PhotonPoseEstimator(
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, Transform3d()
)
# Empty result, expect empty result
cameraOne.result = PhotonPipelineResult(0)
estimatedPose = estimator.update()
assert estimatedPose is None
# Set actual result
cameraOne.result = result
estimatedPose = estimator.update()
assert estimatedPose is not None
assertEquals(20, estimatedPose.timestampSeconds, 0.01)
assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
expectedTimestamp = timestamps.receiveTimestampMicros() * 1e-6 - latencySecs
assertEquals(expectedTimestamp, estimatedPose.timestampSeconds, 1e-3)
assertEquals(expectedTimestamp, estimator._poseCacheTimestampSeconds, 1e-3)
# And again -- pose cache should mean this is empty
cameraOne.result = result
estimatedPose = estimator.update()
assert estimatedPose is None
# Expect the old timestamp to still be here
assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
assertEquals(expectedTimestamp, estimator._poseCacheTimestampSeconds, 1e-3)
# Set new field layout -- right after, the pose cache timestamp should be -1
estimator.fieldTags = AprilTagFieldLayout([AprilTag()], 0, 0)
@@ -266,8 +370,14 @@ def test_cacheIsInvalidated():
assert estimatedPose is not None
assertEquals(20, estimatedPose.timestampSeconds, 0.01)
assertEquals(20 - 2e-3, estimator._poseCacheTimestampSeconds, 1e-3)
assertEquals(expectedTimestamp, estimatedPose.timestampSeconds, 1e-3)
assertEquals(expectedTimestamp, estimator._poseCacheTimestampSeconds, 1e-3)
# Setting a value from None to a non-None should invalidate the cache.
assert estimator.referencePose is None
estimator.referencePose = Pose3d(3, 3, 3, Rotation3d())
assertEquals(-1, estimator._poseCacheTimestampSeconds)
def assertEquals(expected, actual, epsilon=0.0):

65
photon-lib/py/test/testUtil.py Normal file
View File

@@ -0,0 +1,65 @@
"""Test utilities."""
from photonlibpy.targeting import PhotonPipelineMetadata
class InvalidTestDataException(ValueError):
pass
class PipelineTimestamps:
"""Helper class to ensure timestamps are positive."""
def __init__(
self,
*,
captureTimestampMicros: int,
pipelineLatencyMicros=2_000,
receiveLatencyMicros=1_000,
):
if captureTimestampMicros < 0:
raise InvalidTestDataException("captureTimestampMicros cannot be negative")
if pipelineLatencyMicros <= 0:
raise InvalidTestDataException("pipelineLatencyMicros must be positive")
if receiveLatencyMicros < 0:
raise InvalidTestDataException("receiveLatencyMicros cannot be negative")
self._captureTimestampMicros = captureTimestampMicros
self._pipelineLatencyMicros = pipelineLatencyMicros
self._receiveLatencyMicros = receiveLatencyMicros
self._sequenceID = 0
@property
def captureTimestampMicros(self) -> int:
return self._captureTimestampMicros
@captureTimestampMicros.setter
def captureTimestampMicros(self, micros: int) -> None:
if micros < 0:
raise InvalidTestDataException("captureTimestampMicros cannot be negative")
if micros < self._captureTimestampMicros:
raise InvalidTestDataException("time cannot go backwards")
self._captureTimestampMicros = micros
self._sequenceID += 1
@property
def pipelineLatencyMicros(self) -> int:
return self._pipelineLatencyMicros
def pipelineLatencySecs(self) -> float:
return self.pipelineLatencyMicros * 1e-6
def incrementTimeMicros(self, micros: int) -> None:
self.captureTimestampMicros += micros
def publishTimestampMicros(self) -> int:
return self._captureTimestampMicros + self.pipelineLatencyMicros
def receiveTimestampMicros(self) -> int:
return self.publishTimestampMicros() + self._receiveLatencyMicros
def toPhotonPipelineMetadata(self) -> PhotonPipelineMetadata:
return PhotonPipelineMetadata(
captureTimestampMicros=self.captureTimestampMicros,
publishTimestampMicros=self.publishTimestampMicros(),
sequenceID=self._sequenceID,
)

35
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View File

@@ -58,7 +58,7 @@ import org.photonvision.timesync.TimeSyncSingleton;
/** Represents a camera that is connected to PhotonVision. */
public class PhotonCamera implements AutoCloseable {
private static int InstanceCount = 0;
private static int InstanceCount = 1;
public static final String kTableName = "photonvision";
private static final String PHOTON_ALERT_GROUP = "PhotonAlerts";
@@ -195,11 +195,18 @@ public class PhotonCamera implements AutoCloseable {
+ ">>> but you are using WPILib "
+ WPILibVersion.Version
+ """
>>> \s
\n>>> \s
>>> This is neither tested nor supported. \s
>>> You MUST update PhotonVision, \s
>>> PhotonLib, or both. \s
>>> Verify the output of `./gradlew dependencies`
>>> You MUST update WPILib, PhotonLib, or both.
>>> Check `./gradlew dependencies` and ensure\s
>>> all mentions of OpenCV match the version \s
>>> that PhotonLib was built for. If you find a
>>> a mismatched version in a dependency, you\s
>>> must take steps to update the version of \s
>>> OpenCV used in that dependency. If you do\s
>>> not control that dependency and an updated\s
>>> version is not available, contact the \s
>>> developers of that dependency. \s
>>> \s
>>> Your code will now crash. \s
>>> We hope your day gets better. \s
@@ -232,11 +239,18 @@ public class PhotonCamera implements AutoCloseable {
+ ">>> but you are using OpenCV "
+ Core.VERSION
+ """
>>> \s
\n>>> \s
>>> This is neither tested nor supported. \s
>>> You MUST update PhotonVision, \s
>>> PhotonLib, or both. \s
>>> Verify the output of `./gradlew dependencies`
>>> You MUST update WPILib, PhotonLib, or both.
>>> Check `./gradlew dependencies` and ensure\s
>>> all mentions of OpenCV match the version \s
>>> that PhotonLib was built for. If you find a
>>> a mismatched version in a dependency, you\s
>>> must take steps to update the version of \s
>>> OpenCV used in that dependency. If you do\s
>>> not control that dependency and an updated\s
>>> version is not available, contact the \s
>>> developers of that dependency. \s
>>> \s
>>> Your code will now crash. \s
>>> We hope your day gets better. \s
@@ -272,11 +286,10 @@ public class PhotonCamera implements AutoCloseable {
verifyVersion();
updateDisconnectAlert();
List<PhotonPipelineResult> ret = new ArrayList<>();
// Grab the latest results. We don't care about the timestamps from NT - the metadata header has
// this, latency compensated by the Time Sync Client
var changes = resultSubscriber.getAllChanges();
List<PhotonPipelineResult> ret = new ArrayList<>(changes.size());
for (var c : changes) {
var result = c.value;
checkTimeSyncOrWarn(result);

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

@@ -42,11 +42,7 @@ import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N8;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Optional;
import java.util.Set;
import java.util.*;
import org.photonvision.estimation.TargetModel;
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.targeting.PhotonPipelineResult;
@@ -58,7 +54,7 @@ import org.photonvision.targeting.PhotonTrackedTarget;
* below. Example usage can be found in our apriltagExample example project.
*/
public class PhotonPoseEstimator {
private static int InstanceCount = 0;
private static int InstanceCount = 1;
/** Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. */
public enum PoseStrategy {
@@ -175,7 +171,7 @@ public class PhotonPoseEstimator {
}
private void checkUpdate(Object oldObj, Object newObj) {
if (oldObj != newObj && oldObj != null && !oldObj.equals(newObj)) {
if (!Objects.equals(oldObj, newObj)) {
invalidatePoseCache();
}
}
@@ -316,7 +312,7 @@ public class PhotonPoseEstimator {
* Add robot heading data to buffer. Must be called periodically for the
* <b>PNP_DISTANCE_TRIG_SOLVE</b> strategy.
*
* @param timestampSeconds timestamp of the robot heading data.
* @param timestampSeconds Timestamp of the robot heading data.
* @param heading Field-relative robot heading at given timestamp. Standard WPILIB field
* coordinates.
*/
@@ -328,7 +324,7 @@ public class PhotonPoseEstimator {
* Add robot heading data to buffer. Must be called periodically for the
* <b>PNP_DISTANCE_TRIG_SOLVE</b> strategy.
*
* @param timestampSeconds timestamp of the robot heading data.
* @param timestampSeconds Timestamp of the robot heading data.
* @param heading Field-relative robot heading at given timestamp. Standard WPILIB field
* coordinates.
*/
@@ -340,7 +336,20 @@ public class PhotonPoseEstimator {
* Clears all heading data in the buffer, and adds a new seed. Useful for preventing estimates
* from utilizing heading data provided prior to a pose or rotation reset.
*
* @param timestampSeconds timestamp of the robot heading data.
* @param timestampSeconds Timestamp of the robot heading data.
* @param heading Field-relative robot heading at given timestamp. Standard WPILIB field
* coordinates.
*/
public void resetHeadingData(double timestampSeconds, Rotation3d heading) {
headingBuffer.clear();
addHeadingData(timestampSeconds, heading);
}
/**
* Clears all heading data in the buffer, and adds a new seed. Useful for preventing estimates
* from utilizing heading data provided prior to a pose or rotation reset.
*
* @param timestampSeconds Timestamp of the robot heading data.
* @param heading Field-relative robot heading at given timestamp. Standard WPILIB field
* coordinates.
*/

8
photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java
View File

@@ -100,6 +100,14 @@ public class VideoSimUtil {
/**
* Gets the 10x10 (grayscale) image of a specific 36h11 AprilTag.
*
* <p>WARNING: This creates a {@link RawFrame} instance but does not close it, which would result
* in a resource leak if the {@link Mat} is garbage-collected. Unfortunately, closing the {@code
* RawFrame} inside this function would delete the underlying data that backs the {@code
* ByteBuffer} that is passed to the {@code Mat} constructor (see comments on <a
* href="https://github.com/PhotonVision/photonvision/pull/2023">PR 2023</a> for details).
* Luckily, this method is private and is (as of Aug 2025) only used to populate the {@link
* #kTag36h11Images} static map at static-initialization time.
*
* @param id The fiducial id of the desired tag
*/
private static Mat get36h11TagImage(int id) {

1
photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java
View File

@@ -122,6 +122,7 @@ public class VisionSystemSim {
* @return If the camera was present and removed
*/
public boolean removeCamera(PhotonCameraSim cameraSim) {
@SuppressWarnings("resource")
boolean success = camSimMap.remove(cameraSim.getCamera().getName()) != null;
camTrfMap.remove(cameraSim);
return success;

11
photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java
View File

@@ -24,8 +24,8 @@
package org.photonvision.timesync;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import org.photonvision.jni.PhotonTargetingJniLoader;
import org.photonvision.jni.TimeSyncServer;
/** Helper to hold a single TimeSyncServer instance with some default config */
@@ -35,12 +35,11 @@ public class TimeSyncSingleton {
public static boolean load() {
if (INSTANCE == null) {
try {
if (!PhotonTargetingJniLoader.load()) {
return false;
}
} catch (UnsatisfiedLinkError | IOException e) {
RuntimeLoader.loadLibrary("photontargetingJNI");
} catch (IOException e) {
// Don't want to return early. We want to create the TimeSyncServer so the program crashes
// because we need it in order to function.
e.printStackTrace();
return false;
}
INSTANCE = new TimeSyncServer(5810);

26
photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
View File

@@ -69,9 +69,16 @@ inline void verifyDependencies() {
bfw +=
"\n>>> \n"
">>> This is neither tested nor supported. \n"
">>> You MUST update PhotonVision, \n"
">>> PhotonLib, or both. \n"
">>> Verify the output of `./gradlew dependencies` \n"
">>> You MUST update WPILib, PhotonLib, or both.\n"
">>> Check `./gradlew dependencies` and ensure\n"
">>> all mentions of WPILib match the version \n"
">>> that PhotonLib was built for. If you find a"
">>> a mismatched version in a dependency, you\n"
">>> must take steps to update the version of \n"
">>> WPILib used in that dependency. If you do\n"
">>> not control that dependency and an updated\n"
">>> version is not available, contact the \n"
">>> developers of that dependency. \n"
">>> \n"
">>> Your code will now crash. \n"
">>> We hope your day gets better. \n"
@@ -104,9 +111,16 @@ inline void verifyDependencies() {
bfw +=
"\n>>> \n"
">>> This is neither tested nor supported. \n"
">>> You MUST update PhotonVision, \n"
">>> PhotonLib, or both. \n"
">>> Verify the output of `./gradlew dependencies` \n"
">>> You MUST update WPILib, PhotonLib, or both.\n"
">>> Check `./gradlew dependencies` and ensure\n"
">>> all mentions of OpenCV match the version \n"
">>> that PhotonLib was built for. If you find a"
">>> a mismatched version in a dependency, you\n"
">>> must take steps to update the version of \n"
">>> OpenCV used in that dependency. If you do\n"
">>> not control that dependency and an updated\n"
">>> version is not available, contact the \n"
">>> developers of that dependency. \n"
">>> \n"
">>> Your code will now crash. \n"
">>> We hope your day gets better. \n"

137
photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
View File

@@ -44,10 +44,13 @@
#include <opencv2/calib3d.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/types.hpp>
#include <units/angle.h>
#include <units/math.h>
#include <units/time.h>
#include "photon/PhotonCamera.h"
#include "photon/estimation/TargetModel.h"
#include "photon/estimation/VisionEstimation.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
@@ -73,7 +76,8 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()),
poseCacheTimestamp(-1_s) {
poseCacheTimestamp(-1_s),
headingBuffer(frc::TimeInterpolatableBuffer<frc::Rotation2d>(1_s)) {
HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
InstanceCount);
InstanceCount++;
@@ -97,7 +101,8 @@ void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
const PhotonPipelineResult& result,
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs) {
std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
FRC_ReportError(frc::warn::Warning,
@@ -120,13 +125,15 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
return std::nullopt;
}
return Update(result, cameraMatrixData, cameraDistCoeffs, this->strategy);
return Update(result, cameraMatrixData, cameraDistCoeffs,
constrainedPnpParams, this->strategy);
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
const PhotonPipelineResult& result,
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
std::optional<PhotonCamera::DistortionMatrix> cameraDistCoeffs,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams,
PoseStrategy strategy) {
std::optional<EstimatedRobotPose> ret = std::nullopt;
@@ -159,6 +166,13 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
"");
}
break;
case CONSTRAINED_SOLVEPNP:
ret = ConstrainedPnpStrategy(result, cameraMatrixData, cameraDistCoeffs,
constrainedPnpParams);
break;
case PNP_DISTANCE_TRIG_SOLVE:
ret = PnpDistanceTrigSolveStrategy(result);
break;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
"");
@@ -429,6 +443,53 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
MULTI_TAG_PNP_ON_RIO);
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::PnpDistanceTrigSolveStrategy(PhotonPipelineResult result) {
PhotonTrackedTarget bestTarget = result.GetBestTarget();
std::optional<frc::Rotation2d> headingSampleOpt =
headingBuffer.Sample(result.GetTimestamp());
if (!headingSampleOpt) {
FRC_ReportError(frc::warn::Warning,
"There was no heading data! Use AddHeadingData to add it!");
return std::nullopt;
}
frc::Rotation2d headingSample = headingSampleOpt.value();
frc::Translation2d camToTagTranslation =
frc::Translation3d(
bestTarget.GetBestCameraToTarget().Translation().Norm(),
frc::Rotation3d(0_rad, -units::degree_t(bestTarget.GetPitch()),
-units::degree_t(bestTarget.GetYaw())))
.RotateBy(m_robotToCamera.Rotation())
.ToTranslation2d()
.RotateBy(headingSample);
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(bestTarget.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
bestTarget.GetFiducialId());
return std::nullopt;
}
frc::Pose2d tagPose = fiducialPose.value().ToPose2d();
frc::Translation2d fieldToCameraTranslation =
tagPose.Translation() - camToTagTranslation;
frc::Translation2d camToRobotTranslation =
(-m_robotToCamera.Translation().ToTranslation2d())
.RotateBy(headingSample);
frc::Pose2d robotPose = frc::Pose2d(
fieldToCameraTranslation + camToRobotTranslation, headingSample);
return EstimatedRobotPose{frc::Pose3d(robotPose), result.GetTimestamp(),
result.GetTargets(), PNP_DISTANCE_TRIG_SOLVE};
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
@@ -475,4 +536,74 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
result.GetTimestamp(), result.GetTargets(),
AVERAGE_BEST_TARGETS};
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::ConstrainedPnpStrategy(
photon::PhotonPipelineResult result,
std::optional<photon::PhotonCamera::CameraMatrix> camMat,
std::optional<photon::PhotonCamera::DistortionMatrix> distCoeffs,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
using namespace frc;
if (!camMat || !distCoeffs) {
FRC_ReportError(frc::warn::Warning,
"No camera calibration data provided to "
"StrPoseEstimator::MultiTagOnRioStrategy!",
"");
return Update(result, this->multiTagFallbackStrategy);
}
if (!constrainedPnpParams) {
return {};
}
if (!constrainedPnpParams->headingFree &&
!headingBuffer.Sample(result.GetTimestamp()).has_value()) {
return Update(result, camMat, distCoeffs, {},
this->multiTagFallbackStrategy);
}
frc::Pose3d fieldToRobotSeed;
if (result.MultiTagResult().has_value()) {
fieldToRobotSeed =
frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
m_robotToCamera.Inverse());
} else {
std::optional<EstimatedRobotPose> nestedUpdate =
Update(result, camMat, distCoeffs, {}, this->multiTagFallbackStrategy);
if (!nestedUpdate.has_value()) {
return {};
}
fieldToRobotSeed = nestedUpdate->estimatedPose;
}
if (!constrainedPnpParams.value().headingFree) {
fieldToRobotSeed = frc::Pose3d{
fieldToRobotSeed.Translation(),
frc::Rotation3d{headingBuffer.Sample(result.GetTimestamp()).value()}};
}
std::vector<photon::PhotonTrackedTarget> targets{result.GetTargets().begin(),
result.GetTargets().end()};
std::optional<photon::PnpResult> pnpResult =
VisionEstimation::EstimateRobotPoseConstrainedSolvePNP(
camMat.value(), distCoeffs.value(), targets, m_robotToCamera,
fieldToRobotSeed, aprilTags, photon::kAprilTag36h11,
constrainedPnpParams->headingFree,
frc::Rotation2d{headingBuffer.Sample(result.GetTimestamp()).value()},
constrainedPnpParams->headingScalingFactor);
if (!pnpResult) {
return Update(result, camMat, distCoeffs, {},
this->multiTagFallbackStrategy);
}
frc::Pose3d best = frc::Pose3d{} + pnpResult->best;
return EstimatedRobotPose{best, result.GetTimestamp(), result.GetTargets(),
PoseStrategy::CONSTRAINED_SOLVEPNP};
}
} // namespace photon

2
photon-lib/src/main/native/include/photon/PhotonCamera.h
View File

@@ -225,7 +225,7 @@ class PhotonCamera {
private:
units::second_t lastVersionCheckTime = 0_s;
static bool VERSION_CHECK_ENABLED;
inline static int InstanceCount = 0;
inline static int InstanceCount = 1;
units::second_t prevTimeSyncWarnTime = 0_s;

98
photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
View File

@@ -28,7 +28,9 @@
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <frc/interpolation/TimeInterpolatableBuffer.h>
#include <opencv2/core/mat.hpp>
#include "photon/PhotonCamera.h"
@@ -47,6 +49,13 @@ enum PoseStrategy {
AVERAGE_BEST_TARGETS,
MULTI_TAG_PNP_ON_COPROCESSOR,
MULTI_TAG_PNP_ON_RIO,
CONSTRAINED_SOLVEPNP,
PNP_DISTANCE_TRIG_SOLVE
};
struct ConstrainedSolvepnpParams {
bool headingFree{false};
double headingScalingFactor{0.0};
};
struct EstimatedRobotPose {
@@ -172,6 +181,61 @@ class PhotonPoseEstimator {
*/
inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
/**
* Add robot heading data to the buffer. Must be called periodically for the
* PNP_DISTANCE_TRIG_SOLVE strategy.
*
* @param timestamp Timestamp of the robot heading data.
* @param heading Field-relative heading at the given timestamp. Standard
* WPILIB field coordinates.
*/
inline void AddHeadingData(units::second_t timestamp,
frc::Rotation2d heading) {
this->headingBuffer.AddSample(timestamp, heading);
}
/**
* Add robot heading data to the buffer. Must be called periodically for the
* PNP_DISTANCE_TRIG_SOLVE strategy.
*
* @param timestamp Timestamp of the robot heading data.
* @param heading Field-relative heading at the given timestamp. Standard
* WPILIB coordinates.
*/
inline void AddHeadingData(units::second_t timestamp,
frc::Rotation3d heading) {
AddHeadingData(timestamp, heading.ToRotation2d());
}
/**
* Clears all heading data in the buffer, and adds a new seed. Useful for
* preventing estimates from utilizing heading data provided prior to a pose
* or rotation reset.
*
* @param timestamp Timestamp of the robot heading data.
* @param heading Field-relative robot heading at given timestamp. Standard
* WPILIB field coordinates.
*/
inline void ResetHeadingData(units::second_t timestamp,
frc::Rotation2d heading) {
headingBuffer.Clear();
AddHeadingData(timestamp, heading);
}
/**
* Clears all heading data in the buffer, and adds a new seed. Useful for
* preventing estimates from utilizing heading data provided prior to a pose
* or rotation reset.
*
* @param timestamp Timestamp of the robot heading data.
* @param heading Field-relative robot heading at given timestamp. Standard
* WPILIB field coordinates.
*/
inline void ResetHeadingData(units::second_t timestamp,
frc::Rotation3d heading) {
ResetHeadingData(timestamp, heading.ToRotation2d());
}
/**
* Update the pose estimator. If updating multiple times per loop, you should
* call this exactly once per new result, in order of increasing result
@@ -182,11 +246,16 @@ class PhotonPoseEstimator {
* Only required if doing multitag-on-rio, and may be nullopt otherwise.
* @param distCoeffsData The camera calibration distortion coefficients. Only
* required if doing multitag-on-rio, and may be nullopt otherwise.
* @param constrainedPnpParams Constrained SolvePNP params, if needed.
*/
std::optional<EstimatedRobotPose> Update(
const PhotonPipelineResult& result,
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData = std::nullopt,
std::optional<PhotonCamera::DistortionMatrix> coeffsData = std::nullopt);
const photon::PhotonPipelineResult& result,
std::optional<photon::PhotonCamera::CameraMatrix> cameraMatrixData =
std::nullopt,
std::optional<photon::PhotonCamera::DistortionMatrix> coeffsData =
std::nullopt,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams =
std::nullopt);
private:
frc::AprilTagFieldLayout aprilTags;
@@ -200,7 +269,9 @@ class PhotonPoseEstimator {
units::second_t poseCacheTimestamp;
inline static int InstanceCount = 0;
frc::TimeInterpolatableBuffer<frc::Rotation2d> headingBuffer;
inline static int InstanceCount = 1;
inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }
@@ -216,13 +287,14 @@ class PhotonPoseEstimator {
*/
std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result,
PoseStrategy strategy) {
return Update(result, std::nullopt, std::nullopt, strategy);
return Update(result, std::nullopt, std::nullopt, std::nullopt, strategy);
}
std::optional<EstimatedRobotPose> Update(
const PhotonPipelineResult& result,
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
std::optional<PhotonCamera::DistortionMatrix> coeffsData,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams,
PoseStrategy strategy);
/**
@@ -278,6 +350,16 @@ class PhotonPoseEstimator {
std::optional<PhotonCamera::CameraMatrix> camMat,
std::optional<PhotonCamera::DistortionMatrix> distCoeffs);
/**
* Return the pose calculation using the best visible tag and the robot's
* heading
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation
*/
std::optional<EstimatedRobotPose> PnpDistanceTrigSolveStrategy(
PhotonPipelineResult result);
/**
* Return the average of the best target poses using ambiguity as weight.
@@ -286,6 +368,12 @@ class PhotonPoseEstimator {
*/
std::optional<EstimatedRobotPose> AverageBestTargetsStrategy(
PhotonPipelineResult result);
std::optional<EstimatedRobotPose> ConstrainedPnpStrategy(
photon::PhotonPipelineResult result,
std::optional<photon::PhotonCamera::CameraMatrix> camMat,
std::optional<photon::PhotonCamera::DistortionMatrix> distCoeffs,
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams);
};
} // namespace photon

164
photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java
View File

@@ -28,7 +28,6 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertNull;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.Assumptions.assumeTrue;
import static org.photonvision.UnitTestUtils.waitForCondition;
import static org.photonvision.UnitTestUtils.waitForSequenceNumber;
@@ -36,6 +35,7 @@ import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.NetworkTablesJNI;
import edu.wpi.first.util.RuntimeLoader;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.simulation.SimHooks;
@@ -48,25 +48,29 @@ import java.util.stream.Stream;
import org.junit.jupiter.api.AfterEach;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.MethodOrderer;
import org.junit.jupiter.api.Order;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.api.TestMethodOrder;
import org.junit.jupiter.params.ParameterizedTest;
import org.junit.jupiter.params.provider.Arguments;
import org.junit.jupiter.params.provider.MethodSource;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.jni.PhotonTargetingJniLoader;
import org.photonvision.jni.LibraryLoader;
import org.photonvision.jni.TimeSyncClient;
import org.photonvision.jni.WpilibLoader;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.targeting.PhotonPipelineMetadata;
import org.photonvision.targeting.PhotonPipelineResult;
@TestMethodOrder(MethodOrderer.OrderAnnotation.class)
class PhotonCameraTest {
// A test-scoped, local-only NT instance
NetworkTableInstance inst = null;
@BeforeAll
public static void load_wpilib() {
WpilibLoader.loadLibraries();
public static void load() throws IOException {
LibraryLoader.loadWpiLibraries();
RuntimeLoader.loadLibrary("photontargetingJNI");
}
@BeforeEach
@@ -76,6 +80,7 @@ class PhotonCameraTest {
HAL.initialize(500, 0);
inst = NetworkTableInstance.create();
assertTrue(inst.isValid());
inst.stopClient();
inst.stopServer();
inst.startLocal();
@@ -105,38 +110,36 @@ class PhotonCameraTest {
// Just a smoketest for dev use -- don't run by default
@Test
@Order(3)
public void testTimeSyncServerWithPhotonCamera() throws InterruptedException, IOException {
load_wpilib();
PhotonTargetingJniLoader.load();
inst.stopClient();
inst.startServer();
var camera = new PhotonCamera(inst, "Arducam_OV2311_USB_Camera");
PhotonCamera.setVersionCheckEnabled(false);
try (PhotonCamera camera = new PhotonCamera(inst, "Arducam_OV2311_USB_Camera")) {
PhotonCamera.setVersionCheckEnabled(false);
for (int i = 0; i < 5; i++) {
Thread.sleep(500);
for (int i = 0; i < 5; i++) {
Thread.sleep(500);
var res = camera.getLatestResult();
var captureTime = res.getTimestampSeconds();
var now = Timer.getFPGATimestamp();
var res = camera.getLatestResult();
var captureTime = res.getTimestampSeconds();
var now = Timer.getFPGATimestamp();
// expectTrue(captureTime < now);
// expectTrue(captureTime < now);
System.out.println(
"sequence "
+ res.metadata.sequenceID
+ " image capture "
+ captureTime
+ " received at "
+ res.getTimestampSeconds()
+ " now: "
+ NetworkTablesJNI.now() / 1e6
+ " time since last pong: "
+ res.metadata.timeSinceLastPong / 1e6);
System.out.println(
"sequence "
+ res.metadata.sequenceID
+ " image capture "
+ captureTime
+ " received at "
+ res.getTimestampSeconds()
+ " now: "
+ NetworkTablesJNI.now() / 1e6
+ " time since last pong: "
+ res.metadata.timeSinceLastPong / 1e6);
}
}
HAL.shutdown();
}
@@ -189,12 +192,10 @@ class PhotonCameraTest {
* check
*/
@ParameterizedTest
@Order(2)
@MethodSource("testNtOffsets")
public void testRestartingRobotAndCoproc(
int robotStart, int coprocStart, int robotRestart, int coprocRestart) throws Throwable {
// See #1574 - test flakey, disabled until we address this
assumeTrue(false);
var robotNt = NetworkTableInstance.create();
var coprocNt = NetworkTableInstance.create();
@@ -304,6 +305,7 @@ class PhotonCameraTest {
}
@Test
@Order(1) // Alerts can't be reset, need to run this test first to have a clean slate
public void testAlerts() throws InterruptedException {
// GIVEN a fresh NT instance
@@ -331,62 +333,62 @@ class PhotonCameraTest {
Thread.sleep(20);
}
// GIVEN a simulated camera
var sim = new PhotonCameraSim(camera);
// AND a result with a timeSinceLastPong in the past
PhotonPipelineResult noPongResult =
new PhotonPipelineResult(
new PhotonPipelineMetadata(
1, 2, 3, 10 * 1000000 // 10 seconds -> us since last pong
),
List.of(),
Optional.empty());
// GIVEN a simulated camera AND a result with a timeSinceLastPong in the past
try (PhotonCameraSim sim = new PhotonCameraSim(camera)) {
PhotonPipelineResult noPongResult =
new PhotonPipelineResult(
new PhotonPipelineMetadata(
1, 2, 3, 10 * 1000000 // 10 seconds -> us since last pong
),
List.of(),
Optional.empty());
// Loop to hit cases past first iteration
for (int i = 0; i < 10; i++) {
// AND a PhotonCamera with a "new" result
// Loop to hit cases past first iteration
for (int i = 0; i < 10; i++) {
// AND a PhotonCamera with a "new" result
sim.submitProcessedFrame(noPongResult);
// WHEN we update the camera
camera.getAllUnreadResults();
// AND we tick SmartDashboard
SmartDashboard.updateValues();
// THEN the camera isn't disconnected
assertTrue(
Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0]))
.noneMatch(it -> it.equals(disconnectedCameraString)));
// AND the alert string looks like a timesync warning
assertTrue(
Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0]))
.filter(it -> it.contains("is not connected to the TimeSyncServer"))
.count()
== 1);
Thread.sleep(20);
}
final double HEARTBEAT_TIMEOUT = 0.5;
// GIVEN a PhotonCamera provided new results
SimHooks.pauseTiming();
sim.submitProcessedFrame(noPongResult);
// WHEN we update the camera
camera.getAllUnreadResults();
// AND in a connected state
assertTrue(camera.isConnected());
// AND we tick SmartDashboard
SmartDashboard.updateValues();
// WHEN we wait the timeout
SimHooks.stepTiming(HEARTBEAT_TIMEOUT * 1.5);
// THEN the camera isn't disconnected
assertTrue(
Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0]))
.noneMatch(it -> it.equals(disconnectedCameraString)));
// AND the alert string looks like a timesync warning
assertTrue(
Arrays.stream(SmartDashboard.getStringArray("PhotonAlerts/warnings", new String[0]))
.filter(it -> it.contains("is not connected to the TimeSyncServer"))
.count()
== 1);
// THEN the camera will not be connected
assertFalse(camera.isConnected());
Thread.sleep(20);
// WHEN we then provide new results
SimHooks.stepTiming(0.02);
sim.submitProcessedFrame(noPongResult);
camera.getAllUnreadResults();
// THEN the camera will not be connected
assertTrue(camera.isConnected());
}
final double HEARTBEAT_TIMEOUT = 0.5;
// GIVEN a PhotonCamera provided new results
SimHooks.pauseTiming();
sim.submitProcessedFrame(noPongResult);
camera.getAllUnreadResults();
// AND in a connected state
assertTrue(camera.isConnected());
// WHEN we wait the timeout
SimHooks.stepTiming(HEARTBEAT_TIMEOUT * 1.5);
// THEN the camera will not be connected
assertFalse(camera.isConnected());
// WHEN we then provide new results
SimHooks.stepTiming(0.02);
sim.submitProcessedFrame(noPongResult);
camera.getAllUnreadResults();
// THEN the camera will not be connected
assertTrue(camera.isConnected());
}
}

131
photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View File

@@ -27,6 +27,8 @@ package org.photonvision;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertNotNull;
import static org.junit.jupiter.api.Assertions.assertNull;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.Assertions.fail;
@@ -37,25 +39,28 @@ import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.junit.jupiter.api.AfterAll;
import org.junit.jupiter.api.AutoClose;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.photonvision.PhotonPoseEstimator.ConstrainedSolvepnpParams;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.estimation.TargetModel;
import org.photonvision.jni.PhotonTargetingJniLoader;
import org.photonvision.jni.WpilibLoader;
import org.photonvision.jni.LibraryLoader;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionTargetSim;
@@ -68,15 +73,14 @@ import org.photonvision.targeting.TargetCorner;
class PhotonPoseEstimatorTest {
static AprilTagFieldLayout aprilTags;
@AutoClose final PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@BeforeAll
public static void init() throws UnsatisfiedLinkError, IOException {
if (!WpilibLoader.loadLibraries()) {
fail();
}
if (!PhotonTargetingJniLoader.load()) {
public static void init() throws IOException {
if (!LibraryLoader.loadWpiLibraries()) {
fail();
}
RuntimeLoader.loadLibrary("photontargetingJNI");
HAL.initialize(1000, 0);
@@ -95,7 +99,6 @@ class PhotonPoseEstimatorTest {
@Test
void testLowestAmbiguityStrategy() {
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
cameraOne.result =
new PhotonPipelineResult(
0,
@@ -181,7 +184,6 @@ class PhotonPoseEstimatorTest {
@Test
void testClosestToCameraHeightStrategy() {
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
cameraOne.result =
new PhotonPipelineResult(
0,
@@ -270,7 +272,6 @@ class PhotonPoseEstimatorTest {
@Test
void closestToReferencePoseStrategy() {
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
cameraOne.result =
new PhotonPipelineResult(
0,
@@ -360,7 +361,6 @@ class PhotonPoseEstimatorTest {
@Test
void closestToLastPose() {
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
cameraOne.result =
new PhotonPipelineResult(
0,
@@ -525,74 +525,72 @@ class PhotonPoseEstimatorTest {
@Test
void pnpDistanceTrigSolve() {
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
PhotonCameraSim cameraOneSim =
new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG());
List<VisionTargetSim> simTargets =
aprilTags.getTags().stream()
.map((AprilTag x) -> new VisionTargetSim(x.pose, TargetModel.kAprilTag36h11, x.ID))
.toList();
try (PhotonCameraSim cameraOneSim =
new PhotonCameraSim(cameraOne, SimCameraProperties.PERFECT_90DEG())) {
/* Compound Rolled + Pitched + Yaw */
Transform3d compoundTestTransform =
new Transform3d(
-Units.inchesToMeters(12),
-Units.inchesToMeters(11),
3,
new Rotation3d(
Units.degreesToRadians(37),
Units.degreesToRadians(6),
Units.degreesToRadians(60)));
/* Compound Rolled + Pitched + Yaw */
var estimator =
new PhotonPoseEstimator(
aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
Transform3d compoundTestTransform =
new Transform3d(
-Units.inchesToMeters(12),
-Units.inchesToMeters(11),
3,
new Rotation3d(
Units.degreesToRadians(37), Units.degreesToRadians(6), Units.degreesToRadians(60)));
/* this is the real pose of the robot base we test against */
var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197));
PhotonPipelineResult result =
cameraOneSim.process(
1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
var bestTarget = result.getBestTarget();
assertNotNull(bestTarget);
assertEquals(0, bestTarget.fiducialId);
var estimator =
new PhotonPoseEstimator(
aprilTags, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
var estimatedPose = estimator.update(result);
/* this is the real pose of the robot base we test against */
var realPose = new Pose3d(7.3, 4.42, 0, new Rotation3d(0, 0, 2.197));
PhotonPipelineResult result =
cameraOneSim.process(
1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
var pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
assertEquals(realPose.getY(), pose.getY(), .01);
assertEquals(0.0, pose.getZ(), .01);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
/* Straight on */
Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, Rotation3d.kZero);
var estimatedPose = estimator.update(result);
var pose = estimatedPose.get().estimatedPose;
estimator.setRobotToCameraTransform(straightOnTestTransform);
assertEquals(realPose.getX(), pose.getX(), .01);
assertEquals(realPose.getY(), pose.getY(), .01);
assertEquals(0.0, pose.getZ(), .01);
/* Pose to compare with */
realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818));
result =
cameraOneSim.process(
1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
/* Straight on */
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
estimatedPose = estimator.update(result);
Transform3d straightOnTestTransform = new Transform3d(0, 0, 3, new Rotation3d(0, 0, 0));
estimator.setRobotToCameraTransform(straightOnTestTransform);
/* Pose to compare with */
realPose = new Pose3d(4.81, 2.38, 0, new Rotation3d(0, 0, 2.818));
result =
cameraOneSim.process(
1, realPose.transformBy(estimator.getRobotToCameraTransform()), simTargets);
estimator.addHeadingData(result.getTimestampSeconds(), realPose.getRotation().toRotation2d());
estimatedPose = estimator.update(result);
pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
assertEquals(realPose.getY(), pose.getY(), .01);
assertEquals(0.0, pose.getZ(), .01);
pose = estimatedPose.get().estimatedPose;
assertEquals(realPose.getX(), pose.getX(), .01);
assertEquals(realPose.getY(), pose.getY(), .01);
assertEquals(0.0, pose.getZ(), .01);
}
}
@Test
void cacheIsInvalidated() {
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
var result =
new PhotonPipelineResult(
0,
20000000,
1100000,
20_000_000,
1_100_000,
1024,
List.of(
new PhotonTrackedTarget(
@@ -623,6 +621,9 @@ class PhotonPoseEstimatorTest {
PoseStrategy.AVERAGE_BEST_TARGETS,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
// Initial state, expect no timestamp
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Empty result, expect empty result
cameraOne.result = new PhotonPipelineResult();
cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6);
@@ -651,11 +652,16 @@ class PhotonPoseEstimatorTest {
estimatedPose = estimator.update(cameraOne.result);
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// Setting a value from None to a non-None should invalidate the cache
assertNull(estimator.getReferencePose());
assertEquals(20, estimator.poseCacheTimestampSeconds);
estimator.setReferencePose(new Pose2d(new Translation2d(1, 2), Rotation2d.kZero));
assertEquals(-1, estimator.poseCacheTimestampSeconds, "wtf");
}
@Test
void averageBestPoses() {
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
cameraOne.result =
new PhotonPipelineResult(
0,
@@ -744,8 +750,7 @@ class PhotonPoseEstimatorTest {
@Test
void testMultiTagOnRioFallback() {
PhotonCameraInjector camera = new PhotonCameraInjector();
camera.result =
cameraOne.result =
new PhotonPipelineResult(
0,
11 * 1_000_000,
@@ -798,7 +803,7 @@ class PhotonPoseEstimatorTest {
new PhotonPoseEstimator(aprilTags, PoseStrategy.MULTI_TAG_PNP_ON_RIO, Transform3d.kZero);
estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
Optional<EstimatedRobotPose> estimatedPose = estimator.update(camera.result);
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
assertAll(

32
photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
View File

@@ -28,8 +28,6 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.Assertions.fail;
import static org.junit.jupiter.api.Assumptions.assumeTrue;
import static org.photonvision.UnitTestUtils.waitForSequenceNumber;
import edu.wpi.first.apriltag.AprilTag;
@@ -45,6 +43,7 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.util.RuntimeLoader;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.io.IOException;
import java.util.ArrayList;
@@ -60,34 +59,23 @@ import org.junit.jupiter.params.provider.MethodSource;
import org.junit.jupiter.params.provider.ValueSource;
import org.photonvision.estimation.TargetModel;
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.jni.PhotonTargetingJniLoader;
import org.photonvision.jni.WpilibLoader;
import org.photonvision.jni.LibraryLoader;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.VisionSystemSim;
import org.photonvision.simulation.VisionTargetSim;
import org.photonvision.targeting.PhotonTrackedTarget;
// See #1574 - flakey on windows and also linux, so commenting out until we bump wpilib
class VisionSystemSimTest {
private static final double kRotDeltaDeg = 0.25;
NetworkTableInstance inst;
@BeforeAll
public static void setUp() {
assertTrue(WpilibLoader.loadLibraries());
try {
assertTrue(PhotonTargetingJniLoader.load());
} catch (UnsatisfiedLinkError | IOException e) {
e.printStackTrace();
fail(e);
}
public static void setUp() throws IOException {
assertTrue(LibraryLoader.loadWpiLibraries());
RuntimeLoader.loadLibrary("photontargetingJNI");
OpenCvLoader.forceStaticLoad();
// See #1574 - test flakey, disabled until we address this
assumeTrue(false);
}
@BeforeEach
@@ -200,7 +188,7 @@ class VisionSystemSimTest {
var cameraSim = new PhotonCameraSim(camera);
visionSysSim.addCamera(cameraSim, new Transform3d());
cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80));
visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(1.0, 3.0), 3));
visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(3.0, 3.0), 3));
var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(5));
visionSysSim.update(robotPose);
@@ -225,7 +213,7 @@ class VisionSystemSimTest {
var cameraSim = new PhotonCameraSim(camera);
visionSysSim.addCamera(cameraSim, robotToCamera);
cameraSim.prop.setCalibration(1234, 1234, Rotation2d.fromDegrees(80));
visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(1.0, 0.5), 1736));
visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.5, 0.5), 1736));
var robotPose = new Pose2d(new Translation2d(13.98, 0), Rotation2d.fromDegrees(5));
visionSysSim.update(robotPose);
@@ -250,7 +238,7 @@ class VisionSystemSimTest {
visionSysSim.addCamera(cameraSim, new Transform3d());
cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80));
cameraSim.setMinTargetAreaPixels(20.0);
visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.1, 0.025), 24));
visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.1, 0.1), 24));
var robotPose = new Pose2d(new Translation2d(12, 0), Rotation2d.fromDegrees(5));
visionSysSim.update(robotPose);
@@ -274,7 +262,7 @@ class VisionSystemSimTest {
cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80));
cameraSim.setMaxSightRange(10);
cameraSim.setMinTargetAreaPixels(1.0);
visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(1.0, 0.25), 78));
visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(1.0, 1), 78));
var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(5));
visionSysSim.update(robotPose);
@@ -322,7 +310,7 @@ class VisionSystemSimTest {
visionSysSim.addCamera(cameraSim, new Transform3d());
cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(120));
cameraSim.setMinTargetAreaPixels(0.0);
visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.5, 0.5), 23));
visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.5, 0.5), 3));
// Transform is now robot -> camera
visionSysSim.adjustCamera(

185
photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View File

@@ -38,6 +38,9 @@
#include "photon/PhotonCamera.h"
#include "photon/PhotonPoseEstimator.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/simulation/PhotonCameraSim.h"
#include "photon/simulation/SimCameraProperties.h"
#include "photon/simulation/VisionTargetSim.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
@@ -306,6 +309,84 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
cameraOne.test = true;
std::vector<photon::VisionTargetSim> targets;
targets.reserve(tags.size());
for (const auto& tag : tags) {
targets.push_back(
photon::VisionTargetSim(tag.pose, photon::kAprilTag36h11, tag.ID));
}
photon::PhotonCameraSim cameraOneSim = photon::PhotonCameraSim(
&cameraOne, photon::SimCameraProperties::PERFECT_90DEG());
/* Compound Rolled + Pitched + Yaw */
frc::Transform3d compoundTestTransform = frc::Transform3d(
-12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg));
photon::PhotonPoseEstimator estimator(
aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
/* real pose of the robot base to test against */
frc::Pose3d realPose =
frc::Pose3d(7.3_m, 4.42_m, 0_m, frc::Rotation3d(0_rad, 0_rad, 2.197_rad));
photon::PhotonPipelineResult result = cameraOneSim.Process(
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
targets);
cameraOne.testResult = {result};
cameraOne.testResult[0].SetReceiveTimestamp(17_s);
estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Y()),
units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Z()),
units::unit_cast<double>(pose.Z()), .01);
/* Straight on */
frc::Transform3d straightOnTestTransform =
frc::Transform3d(0_m, 0_m, 3_m, frc::Rotation3d(0_rad, 0_rad, 0_rad));
estimator.SetRobotToCameraTransform(straightOnTestTransform);
realPose = frc::Pose3d(4.81_m, 2.38_m, 0_m,
frc::Rotation3d(0_rad, 0_rad, 2.818_rad));
result = cameraOneSim.Process(
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
targets);
cameraOne.testResult = {result};
cameraOne.testResult[0].SetReceiveTimestamp(18_s);
estimator.AddHeadingData(result.GetTimestamp(), realPose.Rotation());
estimatedPose = std::nullopt;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Y()),
units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Z()),
units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
@@ -412,12 +493,41 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
EXPECT_NEAR((15_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- now pose cache should be empty
// And again -- pose cache should result in returning std::nullopt
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// If the camera produces a result that is > 1 micro second later,
// the pose cache should not be hit.
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(16));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_NEAR((16_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- pose cache should result in returning std::nullopt
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// Setting ReferencePose should also clear the cache
estimator.SetReferencePose(frc::Pose3d(units::meter_t(1), units::meter_t(2),
units::meter_t(3), frc::Rotation3d()));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
EXPECT_NEAR((16_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
}
TEST(PhotonPoseEstimatorTest, MultiTagOnRioFallback) {
@@ -474,3 +584,76 @@ TEST(PhotonPoseEstimatorTest, CopyResult) {
EXPECT_NEAR(testResult.GetTimestamp().to<double>(),
test2.GetTimestamp().to<double>(), 0.001);
}
TEST(PhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) {
photon::PhotonPoseEstimator estimator(
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, frc::Transform3d());
photon::PhotonPipelineResult result;
auto estimate = estimator.Update(result);
EXPECT_FALSE(estimate.has_value());
}
TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
auto distortion = Eigen::VectorXd::Zero(8);
auto cameraMat = Eigen::Matrix3d{{399.37500000000006, 0, 319.5},
{0, 399.16666666666674, 239.5},
{0, 0, 1}};
// Create corners data matching the Java test
std::vector<photon::TargetCorner> corners8{
photon::TargetCorner{98.09875447066685, 331.0093220119495},
photon::TargetCorner{122.20226758624413, 335.50083894738486},
photon::TargetCorner{127.17118732489361, 313.81406314178633},
photon::TargetCorner{104.28543773760417, 309.6516557438994}};
frc::Transform3d poseTransform(
frc::Translation3d(3.1665557336121353_m, 4.430673446050584_m,
0.48678786477534686_m),
frc::Rotation3d(frc::Quaternion(0.3132532247418243, 0.24722671090692333,
-0.08413452932300695,
0.9130568172784148)));
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{0.0, 0.0, 0.0, 0.0, 8, 0, 0.0f, poseTransform,
poseTransform, 0.0, corners8, corners8}};
auto multiTagResult = std::make_optional<photon::MultiTargetPNPResult>(
photon::PnpResult{poseTransform, poseTransform, 0.1, 0.1, 0.0},
std::vector<int16_t>{8});
photon::PhotonPipelineResult result{
photon::PhotonPipelineMetadata{1, 10000, 2000, 100}, targets,
multiTagResult};
cameraOne.test = true;
cameraOne.testResult = {result};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
const units::radian_t camPitch = 30_deg;
const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m),
frc::Rotation3d(0_rad, -camPitch, 0_rad)};
photon::PhotonPoseEstimator estimator(
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, kRobotToCam);
estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(),
frc::Rotation2d());
auto estimatedPose =
estimator.Update(cameraOne.testResult[0], cameraMat, distortion,
photon::ConstrainedSolvepnpParams{true, 0});
ASSERT_TRUE(estimatedPose.has_value());
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(3.58, units::unit_cast<double>(pose.X()), 0.01);
EXPECT_NEAR(4.13, units::unit_cast<double>(pose.Y()), 0.01);
EXPECT_NEAR(0.0, units::unit_cast<double>(pose.Z()), 0.01);
EXPECT_EQ(photon::CONSTRAINED_SOLVEPNP, estimatedPose.value().strategy);
}

5
photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
View File

@@ -220,7 +220,6 @@ TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) {
const frc::Pose3d targetPose{
{15.98_m, 0_m, 0_m},
frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}};
frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
photon::PhotonCameraSim cameraSim{&camera};
@@ -231,8 +230,8 @@ TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) {
targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}});
// If the robot is rotated x deg (CCW+), the target yaw should be x deg (CW+)
robotPose =
frc::Pose2d{frc::Translation2d{10_m, 0_m}, frc::Rotation2d{GetParam()}};
frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m},
frc::Rotation2d{GetParam()}};
visionSysSim.Update(robotPose);
const auto result = camera.GetLatestResult();