mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
Add constrained solvePNP strategy (#1682)
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
@@ -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");
|
||||
|
||||
Reference in New Issue
Block a user