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

Add constrained solvePNP strategy (#1682)

Browse Source 
Signed-off-by: Jade Turner <spacey-sooty@proton.me>
Co-authored-by: Jade Turner <spacey-sooty@proton.me>
This commit is contained in:
Matt Morley
2025-02-18 22:15:27 -08:00
committed by GitHub
parent 75c289f526
commit 533f8c97fd
55 changed files with 308948 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -97,9 +97,34 @@ public class PhotonPoseEstimator {
*
* <p>https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2025-build-thread/477314/98
*/
PNP_DISTANCE_TRIG_SOLVE
PNP_DISTANCE_TRIG_SOLVE,
/**
* Solve a constrained version of the Perspective-n-Point problem with the robot's drivebase
* flat on the floor. This computation takes place on the RoboRIO, and typically takes not more
* than 2ms. See {@link PhotonPoseEstimator.ConstrainedSolvepnpParams} and {@link
* org.photonvision.jni.ConstrainedSolvepnpJni} for details and tuning handles this strategy
* exposes. This strategy needs addHeadingData called every frame so heading data is up-to-date.
* If Multi-Tag PNP is enabled on the coprocessor, it will be used to provide an initial seed to
* the optimization algorithm -- otherwise, the multi-tag fallback strategy will be used as the
* seed.
*/
CONSTRAINED_SOLVEPNP
}
/**
* Tuning handles we have over the CONSTRAINED_SOLVEPNP {@link PhotonPoseEstimator.PoseStrategy}.
* Internally, the cost function is a sum-squared of pixel reprojection error + (optionally)
* heading error * heading scale factor.
*
* @param headingFree If true, heading is completely free to vary. If false, heading excursions
* from the provided heading measurement will be penalized
* @param headingScaleFactor If headingFree is false, this weights the cost of changing our robot
* heading estimate against the tag corner reprojection error const.
*/
public static final record ConstrainedSolvepnpParams(
boolean headingFree, double headingScaleFactor) {}
private AprilTagFieldLayout fieldTags;
private TargetModel tagModel = TargetModel.kAprilTag36h11;
private PoseStrategy primaryStrategy;
@@ -345,6 +370,8 @@ public class PhotonPoseEstimator {
* <li>The timestamp of the provided pipeline result is the same as in the previous call to
* {@code update()}.
* <li>No targets were found in the pipeline results.
* <li>The strategy is CONSTRAINED_SOLVEPNP, but no constrainedPnpParams were provided (use the
* other function overload).
* </ul>
*
* @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty
@@ -358,6 +385,32 @@ public class PhotonPoseEstimator {
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N8, N1>> distCoeffs) {
return update(cameraResult, cameraMatrix, distCoeffs, Optional.empty());
}
/**
* Updates the estimated position of the robot. Returns empty if:
*
* <ul>
* <li>The timestamp of the provided pipeline result is the same as in the previous call to
* {@code update()}.
* <li>No targets were found in the pipeline results.
* <li>The strategy is CONSTRAINED_SOLVEPNP, but the provided constrainedPnpParams are empty.
* </ul>
*
* @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @param constrainedPnpParams Constrained SolvePNP params, if needed.
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N8, N1>> distCoeffs,
Optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
// Time in the past -- give up, since the following if expects times > 0
if (cameraResult.getTimestampSeconds() < 0) {
return Optional.empty();
@@ -379,7 +432,8 @@ public class PhotonPoseEstimator {
return Optional.empty();
}
return update(cameraResult, cameraMatrix, distCoeffs, this.primaryStrategy);
return update(
cameraResult, cameraMatrix, distCoeffs, constrainedPnpParams, this.primaryStrategy);
}
/**
@@ -387,19 +441,20 @@ public class PhotonPoseEstimator {
* called after timestamp checks have been done by another update() overload.
*
* @param cameraResult The latest pipeline result from the camera
* @param strategy The pose strategy to use
* @param strategy The pose strategy to use. Can't be CONSTRAINED_SOLVEPNP.
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
private Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult, PoseStrategy strategy) {
return update(cameraResult, Optional.empty(), Optional.empty(), strategy);
return update(cameraResult, Optional.empty(), Optional.empty(), Optional.empty(), strategy);
}
private Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N8, N1>> distCoeffs,
Optional<ConstrainedSolvepnpParams> constrainedPnpParams,
PoseStrategy strategy) {
Optional<EstimatedRobotPose> estimatedPose =
switch (strategy) {
@@ -416,6 +471,8 @@ public class PhotonPoseEstimator {
multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
case MULTI_TAG_PNP_ON_COPROCESSOR -> multiTagOnCoprocStrategy(cameraResult);
case PNP_DISTANCE_TRIG_SOLVE -> pnpDistanceTrigSolveStrategy(cameraResult);
case CONSTRAINED_SOLVEPNP ->
constrainedPnpStrategy(cameraResult, cameraMatrix, distCoeffs, constrainedPnpParams);
};
if (estimatedPose.isPresent()) {
@@ -470,6 +527,87 @@ public class PhotonPoseEstimator {
PoseStrategy.PNP_DISTANCE_TRIG_SOLVE));
}
private Optional<EstimatedRobotPose> constrainedPnpStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N8, N1>> distCoeffsOpt,
Optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
// cannot run multitagPNP, use fallback strategy
if (!hasCalibData) {
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
}
if (constrainedPnpParams.isEmpty()) {
return Optional.empty();
}
// Need heading if heading fixed
if (!constrainedPnpParams.get().headingFree
&& headingBuffer.getSample(result.getTimestampSeconds()).isEmpty()) {
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
}
Pose3d fieldToRobotSeed;
// Attempt to use multi-tag to get a pose estimate seed
if (result.getMultiTagResult().isPresent()) {
fieldToRobotSeed =
Pose3d.kZero.plus(
result.getMultiTagResult().get().estimatedPose.best.plus(robotToCamera.inverse()));
} else {
// HACK - use fallback strategy to gimme a seed pose
// TODO - make sure nested update doesn't break state
var nestedUpdate =
update(
result,
cameraMatrixOpt,
distCoeffsOpt,
Optional.empty(),
this.multiTagFallbackStrategy);
if (nestedUpdate.isEmpty()) {
// best i can do is bail
return Optional.empty();
}
fieldToRobotSeed = nestedUpdate.get().estimatedPose;
}
if (!constrainedPnpParams.get().headingFree) {
// If heading fixed, force rotation component
fieldToRobotSeed =
new Pose3d(
fieldToRobotSeed.getTranslation(),
new Rotation3d(headingBuffer.getSample(result.getTimestampSeconds()).get()));
}
var pnpResult =
VisionEstimation.estimateRobotPoseConstrainedSolvepnp(
cameraMatrixOpt.get(),
distCoeffsOpt.get(),
result.getTargets(),
robotToCamera,
fieldToRobotSeed,
fieldTags,
tagModel,
constrainedPnpParams.get().headingFree,
headingBuffer.getSample(result.getTimestampSeconds()).get(),
constrainedPnpParams.get().headingScaleFactor);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResult.isPresent())
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
var best = Pose3d.kZero.plus(pnpResult.get().best); // field-to-robot
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.CONSTRAINED_SOLVEPNP));
}
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
if (result.getMultiTagResult().isEmpty()) {
return update(result, this.multiTagFallbackStrategy);
@@ -477,7 +615,7 @@ public class PhotonPoseEstimator {
var best_tf = result.getMultiTagResult().get().estimatedPose.best;
var best =
new Pose3d()
Pose3d.kZero
.plus(best_tf) // field-to-camera
.relativeTo(fieldTags.getOrigin())
.plus(robotToCamera.inverse()); // field-to-robot
@@ -508,9 +646,12 @@ public class PhotonPoseEstimator {
VisionEstimation.estimateCamPosePNP(
cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResult.isPresent()) return update(result, this.multiTagFallbackStrategy);
if (!pnpResult.isPresent())
return update(
result, cameraMatrixOpt, distCoeffsOpt, Optional.empty(), this.multiTagFallbackStrategy);
var best =
new Pose3d()
Pose3d.kZero
.plus(pnpResult.get().best) // field-to-camera
.plus(robotToCamera.inverse()); // field-to-robot

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

@@ -28,33 +28,58 @@ 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.assertTrue;
import static org.junit.jupiter.api.Assertions.fail;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
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.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.Translation3d;
import edu.wpi.first.math.util.Units;
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.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.simulation.PhotonCameraSim;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionTargetSim;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PhotonPipelineMetadata;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.PnpResult;
import org.photonvision.targeting.TargetCorner;
class PhotonPoseEstimatorTest {
static AprilTagFieldLayout aprilTags;
@BeforeAll
public static void init() {
public static void init() throws UnsatisfiedLinkError, IOException {
if (!WpilibLoader.loadLibraries()) {
fail();
}
if (!PhotonTargetingJniLoader.load()) {
fail();
}
HAL.initialize(1000, 0);
List<AprilTag> tagList = new ArrayList<>(2);
tagList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
tagList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
@@ -63,6 +88,11 @@ class PhotonPoseEstimatorTest {
aprilTags = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth);
}
@AfterAll
public static void teardown() {
HAL.shutdown();
}
@Test
void testLowestAmbiguityStrategy() {
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
@@ -778,6 +808,109 @@ class PhotonPoseEstimatorTest {
() -> assertEquals(2, pose.getZ(), 1e-9));
}
@Test
public void testConstrainedPnpOneTag() {
var distortion = VecBuilder.fill(0, 0, 0, 0, 0, 0, 0, 0);
var cameraMat =
MatBuilder.fill(
Nat.N3(),
Nat.N3(),
399.37500000000006,
0,
319.5,
0,
399.16666666666674,
239.5,
0,
0,
1);
/*
* Ground truth:
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/w,0.
* 31064262190452635
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/x,0.
* 24478552235412665
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/y,-0.
* 0836470779150917
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/rotation/q/z,0.
* 914649865171567
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/translation/x,3.
* 191446451763934
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/translation/y,4.
* 44396966389316
* 29.989279,NT:/photonvision/YOUR CAMERA
* NAME/rawBytes/multitagResult/estimatedPose/best/translation/z,0.
* 4995793771070878
*/
List<TargetCorner> corners8 =
List.of(
new TargetCorner(98.09875447066685, 331.0093220119495),
new TargetCorner(122.20226758624413, 335.50083894738486),
new TargetCorner(127.17118732489361, 313.81406314178633),
new TargetCorner(104.28543773760417, 309.6516557438994));
var result =
new PhotonPipelineResult(
new PhotonPipelineMetadata(10000, 2000, 1, 100),
List.of(
new PhotonTrackedTarget(0, 0, 0, 0, 8, 0, 0, null, null, 0, corners8, corners8)),
Optional.of(
new MultiTargetPNPResult(
new PnpResult(
new Transform3d(
// From ground truth
new Translation3d(
3.1665557336121353, 4.430673446050584, 0.48678786477534686),
new Rotation3d(
new Quaternion(
0.3132532247418243,
0.24722671090692333,
-0.08413452932300695,
0.9130568172784148))),
0.1),
new ArrayList<Short>(8))));
final double camPitch = Units.degreesToRadians(30.0);
final Transform3d kRobotToCam =
new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
PoseStrategy.CONSTRAINED_SOLVEPNP,
kRobotToCam);
estimator.addHeadingData(result.getTimestampSeconds(), Rotation2d.kZero);
Optional<EstimatedRobotPose> estimatedPose =
estimator.update(
result,
Optional.of(cameraMat),
Optional.of(distortion),
Optional.of(new ConstrainedSolvepnpParams(true, 0)));
Pose3d pose = estimatedPose.get().estimatedPose;
System.out.println(pose);
}
@Test
void testConstrainedPnpEmptyCase() {
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
PoseStrategy.CONSTRAINED_SOLVEPNP,
Transform3d.kZero);
PhotonPipelineResult result = new PhotonPipelineResult();
var estimate = estimator.update(result);
assertEquals(estimate, Optional.empty());
}
private static class PhotonCameraInjector extends PhotonCamera {
public PhotonCameraInjector() {
super("Test");