2022-12-30 00:40:13 -06:00
|
|
|
/*
|
|
|
|
|
* MIT License
|
|
|
|
|
*
|
|
|
|
|
* Copyright (c) 2022 PhotonVision
|
|
|
|
|
*
|
|
|
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
|
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
|
|
|
* in the Software without restriction, including without limitation the rights
|
|
|
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
|
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
|
|
|
* furnished to do so, subject to the following conditions:
|
|
|
|
|
*
|
|
|
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
|
|
|
* copies or substantial portions of the Software.
|
|
|
|
|
*
|
|
|
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
|
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
|
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
|
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
|
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
|
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
|
|
|
* SOFTWARE.
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
package org.photonvision;
|
|
|
|
|
|
|
|
|
|
import static org.junit.jupiter.api.Assertions.assertEquals;
|
2023-02-13 18:22:22 -08:00
|
|
|
import static org.junit.jupiter.api.Assertions.assertFalse;
|
|
|
|
|
import static org.junit.jupiter.api.Assertions.assertTrue;
|
2022-12-30 00:40:13 -06:00
|
|
|
|
2023-01-01 21:40:48 -06:00
|
|
|
import edu.wpi.first.apriltag.AprilTag;
|
|
|
|
|
import edu.wpi.first.apriltag.AprilTagFieldLayout;
|
2022-12-30 00:40:13 -06:00
|
|
|
import edu.wpi.first.hal.JNIWrapper;
|
|
|
|
|
import edu.wpi.first.math.geometry.Pose3d;
|
|
|
|
|
import edu.wpi.first.math.geometry.Rotation3d;
|
|
|
|
|
import edu.wpi.first.math.geometry.Transform3d;
|
|
|
|
|
import edu.wpi.first.math.geometry.Translation3d;
|
2023-01-01 21:40:48 -06:00
|
|
|
import edu.wpi.first.math.util.Units;
|
2022-12-30 00:40:13 -06:00
|
|
|
import edu.wpi.first.net.WPINetJNI;
|
|
|
|
|
import edu.wpi.first.networktables.NetworkTablesJNI;
|
|
|
|
|
import edu.wpi.first.util.CombinedRuntimeLoader;
|
|
|
|
|
import edu.wpi.first.util.WPIUtilJNI;
|
|
|
|
|
import java.io.IOException;
|
|
|
|
|
import java.util.ArrayList;
|
|
|
|
|
import java.util.List;
|
2023-01-02 19:22:39 -05:00
|
|
|
import java.util.Optional;
|
2022-12-30 00:40:13 -06:00
|
|
|
import org.junit.jupiter.api.BeforeAll;
|
|
|
|
|
import org.junit.jupiter.api.Test;
|
2023-01-14 09:06:15 -06:00
|
|
|
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
|
2022-12-30 00:40:13 -06:00
|
|
|
import org.photonvision.targeting.PhotonPipelineResult;
|
|
|
|
|
import org.photonvision.targeting.PhotonTrackedTarget;
|
|
|
|
|
import org.photonvision.targeting.TargetCorner;
|
|
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
class PhotonPoseEstimatorTest {
|
2023-01-01 21:40:48 -06:00
|
|
|
static AprilTagFieldLayout aprilTags;
|
|
|
|
|
|
2022-12-30 00:40:13 -06:00
|
|
|
@BeforeAll
|
|
|
|
|
public static void init() {
|
|
|
|
|
JNIWrapper.Helper.setExtractOnStaticLoad(false);
|
|
|
|
|
WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
|
|
|
|
|
NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
|
|
|
|
|
WPINetJNI.Helper.setExtractOnStaticLoad(false);
|
|
|
|
|
|
|
|
|
|
try {
|
|
|
|
|
CombinedRuntimeLoader.loadLibraries(
|
2023-01-14 09:06:15 -06:00
|
|
|
PhotonPoseEstimatorTest.class, "wpiutiljni", "ntcorejni", "wpinetjni", "wpiHaljni");
|
2022-12-30 00:40:13 -06:00
|
|
|
} catch (IOException e) {
|
|
|
|
|
// TODO Auto-generated catch block
|
|
|
|
|
e.printStackTrace();
|
|
|
|
|
}
|
2023-01-01 21:40:48 -06:00
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
List<AprilTag> tagList = new ArrayList<AprilTag>(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())));
|
|
|
|
|
double fieldLength = Units.feetToMeters(54.0);
|
|
|
|
|
double fieldWidth = Units.feetToMeters(27.0);
|
|
|
|
|
aprilTags = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth);
|
2022-12-30 00:40:13 -06:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
@Test
|
|
|
|
|
void testLowestAmbiguityStrategy() {
|
|
|
|
|
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
|
|
|
|
|
cameraOne.result =
|
|
|
|
|
new PhotonPipelineResult(
|
|
|
|
|
2,
|
|
|
|
|
List.of(
|
|
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
3.0,
|
|
|
|
|
-4.0,
|
|
|
|
|
9.0,
|
|
|
|
|
4.0,
|
|
|
|
|
0,
|
|
|
|
|
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
|
|
|
|
|
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
|
|
|
|
|
0.7,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8))),
|
|
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
3.0,
|
|
|
|
|
-4.0,
|
|
|
|
|
9.1,
|
|
|
|
|
6.7,
|
|
|
|
|
1,
|
|
|
|
|
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)),
|
|
|
|
|
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
|
|
|
|
|
0.3,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
2023-01-14 09:06:15 -06:00
|
|
|
new TargetCorner(7, 8))),
|
2022-12-30 00:40:13 -06:00
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
9.0,
|
|
|
|
|
-2.0,
|
|
|
|
|
19.0,
|
|
|
|
|
3.0,
|
|
|
|
|
0,
|
|
|
|
|
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
|
|
|
|
|
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
|
|
|
|
|
0.4,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)))));
|
2023-01-14 09:06:15 -06:00
|
|
|
cameraOne.result.setTimestampSeconds(11);
|
2022-12-30 00:40:13 -06:00
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
PhotonPoseEstimator estimator =
|
|
|
|
|
new PhotonPoseEstimator(
|
|
|
|
|
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, new Transform3d());
|
2022-12-30 00:40:13 -06:00
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
|
|
|
|
|
Pose3d pose = estimatedPose.get().estimatedPose;
|
2022-12-30 00:40:13 -06:00
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
assertEquals(11, estimatedPose.get().timestampSeconds);
|
2022-12-30 00:40:13 -06:00
|
|
|
assertEquals(1, pose.getX(), .01);
|
|
|
|
|
assertEquals(3, pose.getY(), .01);
|
|
|
|
|
assertEquals(2, pose.getZ(), .01);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
@Test
|
|
|
|
|
void testClosestToCameraHeightStrategy() {
|
|
|
|
|
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
|
|
|
|
|
cameraOne.result =
|
|
|
|
|
new PhotonPipelineResult(
|
|
|
|
|
2,
|
|
|
|
|
List.of(
|
|
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
3.0,
|
|
|
|
|
-4.0,
|
|
|
|
|
9.0,
|
|
|
|
|
4.0,
|
|
|
|
|
1,
|
|
|
|
|
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
|
|
|
|
|
0.7,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8))),
|
|
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
3.0,
|
|
|
|
|
-4.0,
|
|
|
|
|
9.1,
|
|
|
|
|
6.7,
|
|
|
|
|
1,
|
|
|
|
|
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
|
|
|
|
|
0.3,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
2023-01-14 09:06:15 -06:00
|
|
|
new TargetCorner(7, 8))),
|
2022-12-30 00:40:13 -06:00
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
9.0,
|
|
|
|
|
-2.0,
|
|
|
|
|
19.0,
|
|
|
|
|
3.0,
|
|
|
|
|
0,
|
|
|
|
|
new Transform3d(new Translation3d(4, 4, 4), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(5, 5, 5), new Rotation3d()),
|
|
|
|
|
0.4,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)))));
|
|
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
cameraOne.result.setTimestampSeconds(4);
|
2022-12-30 00:40:13 -06:00
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
PhotonPoseEstimator estimator =
|
|
|
|
|
new PhotonPoseEstimator(
|
|
|
|
|
aprilTags,
|
|
|
|
|
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT,
|
|
|
|
|
cameraOne,
|
|
|
|
|
new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
|
2022-12-30 00:40:13 -06:00
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
|
|
|
|
|
Pose3d pose = estimatedPose.get().estimatedPose;
|
2022-12-30 00:40:13 -06:00
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
assertEquals(4, estimatedPose.get().timestampSeconds);
|
2022-12-30 00:40:13 -06:00
|
|
|
assertEquals(4, pose.getX(), .01);
|
|
|
|
|
assertEquals(4, pose.getY(), .01);
|
2023-01-01 21:40:48 -06:00
|
|
|
assertEquals(0, pose.getZ(), .01);
|
2022-12-30 00:40:13 -06:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
@Test
|
|
|
|
|
void closestToReferencePoseStrategy() {
|
|
|
|
|
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
|
|
|
|
|
cameraOne.result =
|
|
|
|
|
new PhotonPipelineResult(
|
|
|
|
|
2,
|
|
|
|
|
List.of(
|
|
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
3.0,
|
|
|
|
|
-4.0,
|
|
|
|
|
9.0,
|
|
|
|
|
4.0,
|
|
|
|
|
1,
|
|
|
|
|
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
|
|
|
|
|
0.7,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8))),
|
|
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
3.0,
|
|
|
|
|
-4.0,
|
|
|
|
|
9.1,
|
|
|
|
|
6.7,
|
|
|
|
|
1,
|
|
|
|
|
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
|
|
|
|
|
0.3,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
2023-01-14 09:06:15 -06:00
|
|
|
new TargetCorner(7, 8))),
|
2022-12-30 00:40:13 -06:00
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
9.0,
|
|
|
|
|
-2.0,
|
|
|
|
|
19.0,
|
|
|
|
|
3.0,
|
|
|
|
|
0,
|
|
|
|
|
new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
|
|
|
|
|
0.4,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)))));
|
2023-01-14 09:06:15 -06:00
|
|
|
cameraOne.result.setTimestampSeconds(17);
|
|
|
|
|
|
|
|
|
|
PhotonPoseEstimator estimator =
|
|
|
|
|
new PhotonPoseEstimator(
|
|
|
|
|
aprilTags,
|
|
|
|
|
PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
|
|
|
|
|
cameraOne,
|
|
|
|
|
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
|
2022-12-30 00:40:13 -06:00
|
|
|
estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
|
|
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
|
|
|
|
|
Pose3d pose = estimatedPose.get().estimatedPose;
|
2022-12-30 00:40:13 -06:00
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
assertEquals(17, estimatedPose.get().timestampSeconds);
|
2022-12-30 00:40:13 -06:00
|
|
|
assertEquals(1, pose.getX(), .01);
|
|
|
|
|
assertEquals(1.1, pose.getY(), .01);
|
|
|
|
|
assertEquals(.9, pose.getZ(), .01);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
@Test
|
|
|
|
|
void closestToLastPose() {
|
|
|
|
|
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
|
|
|
|
|
cameraOne.result =
|
|
|
|
|
new PhotonPipelineResult(
|
|
|
|
|
2,
|
|
|
|
|
List.of(
|
|
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
3.0,
|
|
|
|
|
-4.0,
|
|
|
|
|
9.0,
|
|
|
|
|
4.0,
|
|
|
|
|
1,
|
|
|
|
|
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
|
|
|
|
|
0.7,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8))),
|
|
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
3.0,
|
|
|
|
|
-4.0,
|
|
|
|
|
9.1,
|
|
|
|
|
6.7,
|
|
|
|
|
1,
|
|
|
|
|
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
|
|
|
|
|
0.3,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
2023-01-14 09:06:15 -06:00
|
|
|
new TargetCorner(7, 8))),
|
2022-12-30 00:40:13 -06:00
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
9.0,
|
|
|
|
|
-2.0,
|
|
|
|
|
19.0,
|
|
|
|
|
3.0,
|
|
|
|
|
0,
|
|
|
|
|
new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
|
|
|
|
|
0.4,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)))));
|
2023-02-13 18:22:22 -08:00
|
|
|
cameraOne.result.setTimestampSeconds(1);
|
2022-12-30 00:40:13 -06:00
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
PhotonPoseEstimator estimator =
|
|
|
|
|
new PhotonPoseEstimator(
|
|
|
|
|
aprilTags,
|
|
|
|
|
PoseStrategy.CLOSEST_TO_LAST_POSE,
|
|
|
|
|
cameraOne,
|
|
|
|
|
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
|
2022-12-30 00:40:13 -06:00
|
|
|
|
|
|
|
|
estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
|
|
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
|
|
|
|
|
Pose3d pose = estimatedPose.get().estimatedPose;
|
2022-12-30 00:40:13 -06:00
|
|
|
|
|
|
|
|
cameraOne.result =
|
|
|
|
|
new PhotonPipelineResult(
|
|
|
|
|
2,
|
|
|
|
|
List.of(
|
|
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
3.0,
|
|
|
|
|
-4.0,
|
|
|
|
|
9.0,
|
|
|
|
|
4.0,
|
|
|
|
|
1,
|
|
|
|
|
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
|
|
|
|
|
0.7,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8))),
|
|
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
3.0,
|
|
|
|
|
-4.0,
|
|
|
|
|
9.1,
|
|
|
|
|
6.7,
|
|
|
|
|
0,
|
|
|
|
|
new Transform3d(new Translation3d(2.1, 1.9, 2), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
|
|
|
|
|
0.3,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
2023-01-14 09:06:15 -06:00
|
|
|
new TargetCorner(7, 8))),
|
2022-12-30 00:40:13 -06:00
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
9.0,
|
|
|
|
|
-2.0,
|
|
|
|
|
19.0,
|
|
|
|
|
3.0,
|
|
|
|
|
0,
|
|
|
|
|
new Transform3d(new Translation3d(2.4, 2.4, 2.2), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(2, 1, 2), new Rotation3d()),
|
|
|
|
|
0.4,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)))));
|
2023-01-14 09:06:15 -06:00
|
|
|
cameraOne.result.setTimestampSeconds(7);
|
2022-12-30 00:40:13 -06:00
|
|
|
|
|
|
|
|
estimatedPose = estimator.update();
|
2023-01-14 09:06:15 -06:00
|
|
|
pose = estimatedPose.get().estimatedPose;
|
2022-12-30 00:40:13 -06:00
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
assertEquals(7, estimatedPose.get().timestampSeconds);
|
2022-12-30 00:40:13 -06:00
|
|
|
assertEquals(.9, pose.getX(), .01);
|
|
|
|
|
assertEquals(1.1, pose.getY(), .01);
|
|
|
|
|
assertEquals(1, pose.getZ(), .01);
|
|
|
|
|
}
|
|
|
|
|
|
2023-02-13 18:22:22 -08:00
|
|
|
@Test
|
|
|
|
|
void cacheIsInvalidated() {
|
|
|
|
|
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
|
|
|
|
|
var result =
|
|
|
|
|
new PhotonPipelineResult(
|
|
|
|
|
2,
|
|
|
|
|
List.of(
|
|
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
3.0,
|
|
|
|
|
-4.0,
|
|
|
|
|
9.0,
|
|
|
|
|
4.0,
|
|
|
|
|
0,
|
|
|
|
|
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
|
|
|
|
|
0.7,
|
|
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
|
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)))));
|
|
|
|
|
result.setTimestampSeconds(20);
|
|
|
|
|
|
|
|
|
|
PhotonPoseEstimator estimator =
|
|
|
|
|
new PhotonPoseEstimator(
|
|
|
|
|
aprilTags,
|
|
|
|
|
PoseStrategy.AVERAGE_BEST_TARGETS,
|
|
|
|
|
cameraOne,
|
|
|
|
|
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
|
|
|
|
|
|
|
|
|
|
// Empty result, expect empty result
|
|
|
|
|
cameraOne.result = new PhotonPipelineResult();
|
|
|
|
|
cameraOne.result.setTimestampSeconds(1);
|
|
|
|
|
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
|
|
|
|
|
assertFalse(estimatedPose.isPresent());
|
|
|
|
|
|
|
|
|
|
// Set actual result
|
|
|
|
|
cameraOne.result = result;
|
|
|
|
|
estimatedPose = estimator.update();
|
|
|
|
|
assertTrue(estimatedPose.isPresent());
|
|
|
|
|
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
|
|
|
|
|
assertEquals(20, estimator.poseCacheTimestampSeconds);
|
|
|
|
|
|
|
|
|
|
// And again -- pose cache should mean this is empty
|
|
|
|
|
cameraOne.result = result;
|
|
|
|
|
estimatedPose = estimator.update();
|
|
|
|
|
assertFalse(estimatedPose.isPresent());
|
|
|
|
|
// Expect the old timestamp to still be here
|
|
|
|
|
assertEquals(20, estimator.poseCacheTimestampSeconds);
|
|
|
|
|
|
|
|
|
|
// Set new field layout -- right after, the pose cache timestamp should be -1
|
|
|
|
|
estimator.setFieldTags(new AprilTagFieldLayout(List.of(new AprilTag(0, new Pose3d())), 0, 0));
|
|
|
|
|
assertEquals(-1, estimator.poseCacheTimestampSeconds);
|
|
|
|
|
// Update should cache the current timestamp (20) again
|
|
|
|
|
cameraOne.result = result;
|
|
|
|
|
estimatedPose = estimator.update();
|
|
|
|
|
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
|
|
|
|
|
assertEquals(20, estimator.poseCacheTimestampSeconds);
|
|
|
|
|
}
|
|
|
|
|
|
2022-12-30 00:40:13 -06:00
|
|
|
@Test
|
|
|
|
|
void averageBestPoses() {
|
|
|
|
|
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
|
|
|
|
|
cameraOne.result =
|
|
|
|
|
new PhotonPipelineResult(
|
|
|
|
|
2,
|
|
|
|
|
List.of(
|
|
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
3.0,
|
|
|
|
|
-4.0,
|
|
|
|
|
9.0,
|
|
|
|
|
4.0,
|
|
|
|
|
0,
|
|
|
|
|
new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
|
|
|
|
|
0.7,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8))), // 1 1 1 ambig: .7
|
|
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
3.0,
|
|
|
|
|
-4.0,
|
|
|
|
|
9.1,
|
|
|
|
|
6.7,
|
|
|
|
|
1,
|
|
|
|
|
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
|
|
|
|
|
0.3,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
2023-01-14 09:06:15 -06:00
|
|
|
new TargetCorner(7, 8))), // 2 2 2 ambig .3
|
2022-12-30 00:40:13 -06:00
|
|
|
new PhotonTrackedTarget(
|
|
|
|
|
9.0,
|
|
|
|
|
-2.0,
|
|
|
|
|
19.0,
|
|
|
|
|
3.0,
|
|
|
|
|
0,
|
|
|
|
|
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
|
|
|
|
|
new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
|
|
|
|
|
0.4,
|
2023-01-06 19:20:27 -08:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8)),
|
2022-12-30 00:40:13 -06:00
|
|
|
List.of(
|
|
|
|
|
new TargetCorner(1, 2),
|
|
|
|
|
new TargetCorner(3, 4),
|
|
|
|
|
new TargetCorner(5, 6),
|
|
|
|
|
new TargetCorner(7, 8))))); // 3 3 3 ambig .4
|
2023-01-14 09:06:15 -06:00
|
|
|
cameraOne.result.setTimestampSeconds(20);
|
2022-12-30 00:40:13 -06:00
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
PhotonPoseEstimator estimator =
|
|
|
|
|
new PhotonPoseEstimator(
|
|
|
|
|
aprilTags,
|
|
|
|
|
PoseStrategy.AVERAGE_BEST_TARGETS,
|
|
|
|
|
cameraOne,
|
|
|
|
|
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
|
2022-12-30 00:40:13 -06:00
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
|
|
|
|
|
Pose3d pose = estimatedPose.get().estimatedPose;
|
2022-12-30 00:40:13 -06:00
|
|
|
|
2023-01-14 09:06:15 -06:00
|
|
|
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
|
2022-12-30 00:40:13 -06:00
|
|
|
assertEquals(2.15, pose.getX(), .01);
|
|
|
|
|
assertEquals(2.15, pose.getY(), .01);
|
|
|
|
|
assertEquals(2.15, pose.getZ(), .01);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
private class PhotonCameraInjector extends PhotonCamera {
|
|
|
|
|
public PhotonCameraInjector() {
|
|
|
|
|
super("Test");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
PhotonPipelineResult result;
|
|
|
|
|
|
|
|
|
|
@Override
|
|
|
|
|
public PhotonPipelineResult getLatestResult() {
|
|
|
|
|
return result;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
