From 2db91e4c0df65960fcc5c28e64c7a34598561450 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Thu, 14 Mar 2024 20:36:15 -0700 Subject: [PATCH 1/8] 3/14 testing Update to 2024.2.9 Fix pitch filter Add debug prints --- .../robot/sensors/AprilTagsProcessor.java | 54 ++++++++++++++++--- vendordeps/photonlib.json | 10 ++-- 2 files changed, 53 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index 01975444..b6fe959c 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -62,10 +63,15 @@ private static PhotonPipelineResult copy(PhotonPipelineResult result) { private static final double MAX_POSE_AMBIGUITY = 0.1; // Both of these are in radians - // Negate pitch to go from robot/target to cam pitch to cam to robot/target pitch - private static final double EXPECTED_CAM_TO_TARGET_PITCH = -ROBOT_TO_CAM.getRotation().getY(); + // Negate to convert robot to cam to cam to robot, and negate again because tag is flipped, + // leading to no net effect + private static final double EXPECTED_CAM_TO_TARGET_PITCH = ROBOT_TO_CAM.getRotation().getY(); private static final double CAM_TO_TARGET_PITCH_TOLERANCE = 0.1; + static { + System.out.println("Expected pitch: " + EXPECTED_CAM_TO_TARGET_PITCH); + } + private static boolean hasCorrectPitch(Transform3d camToTarget) { return Math.abs(camToTarget.getRotation().getY() - EXPECTED_CAM_TO_TARGET_PITCH) < CAM_TO_TARGET_PITCH_TOLERANCE; @@ -89,16 +95,40 @@ private static PhotonPipelineResult filteredPipelineResult(PhotonPipelineResult var copy = copy(result); for (int i = copy.targets.size() - 1; i >= 0; --i) { var target = copy.targets.get(i); - if (target.getPoseAmbiguity() > MAX_POSE_AMBIGUITY) { - copy.targets.remove(i); - continue; - } if (!hasCorrectPitch(target.getBestCameraToTarget())) { if (hasCorrectPitch(target.getAlternateCameraToTarget())) { target = swapBestAndAltTransforms(target); copy.targets.set(i, target); + if (canPrint) { + System.out.println( + "Swapped best and alt transform for target " + + i + + " (best pitch=" + + target.getBestCameraToTarget().getRotation().getY() + + ", alt pitch=" + + target.getAlternateCameraToTarget().getRotation().getY() + + ")"); + } } else { copy.targets.remove(i); + if (canPrint) { + System.out.println( + "Removed target " + + i + + " (best pitch=" + + target.getBestCameraToTarget().getRotation().getY() + + ", alt pitch=" + + target.getAlternateCameraToTarget().getRotation().getY() + + ") because of bad pitch "); + } + continue; + } + if (target.getPoseAmbiguity() > MAX_POSE_AMBIGUITY) { + copy.targets.remove(i); + if (canPrint) { + System.out.println( + "Excluded target " + i + " with ambiguity " + target.getPoseAmbiguity()); + } continue; } } @@ -120,6 +150,9 @@ private static PhotonPipelineResult filteredPipelineResult(PhotonPipelineResult private double lastTimestampSeconds = 0; private Pose2d lastFieldPose = new Pose2d(-1, -1, new Rotation2d()); + private static double lastPrintTimeSeconds = -1; + private static boolean canPrint = false; + private static final AprilTagFieldLayout fieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); @@ -166,10 +199,19 @@ public AprilTagsProcessor(DrivebaseWrapper aprilTagsHelper) { } public void update() { + double now = Timer.getFPGATimestamp(); + canPrint = now - lastPrintTimeSeconds >= 1; + if (canPrint) { + lastPrintTimeSeconds = now; + System.out.println("Got update at " + now); + } latestResult = photonCamera.getLatestResult(); latestFilteredResult = filteredPipelineResult(latestResult); latestPose = photonPoseEstimator.update(latestFilteredResult); if (latestPose.isPresent()) { + if (canPrint) { + System.out.println("Have result"); + } lastTimestampSeconds = latestPose.get().timestampSeconds; lastFieldPose = latestPose.get().estimatedPose.toPose2d(); rawVisionFieldObject.setPose(lastFieldPose); diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 46211fc1..77641e45 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.4", + "version": "v2024.2.9", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.4", + "version": "v2024.2.9", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.4", + "version": "v2024.2.9", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.4" + "version": "v2024.2.9" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.4" + "version": "v2024.2.9" } ] } \ No newline at end of file From cca5f4f1ee53bbff2f15e05fc79ccc1ec01d614c Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Thu, 14 Mar 2024 21:20:56 -0700 Subject: [PATCH 2/8] Remove prints --- .../robot/sensors/AprilTagsProcessor.java | 41 ------------------- 1 file changed, 41 deletions(-) diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index b6fe959c..f2f223fb 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -13,7 +13,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTableEvent; import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; @@ -68,10 +67,6 @@ private static PhotonPipelineResult copy(PhotonPipelineResult result) { private static final double EXPECTED_CAM_TO_TARGET_PITCH = ROBOT_TO_CAM.getRotation().getY(); private static final double CAM_TO_TARGET_PITCH_TOLERANCE = 0.1; - static { - System.out.println("Expected pitch: " + EXPECTED_CAM_TO_TARGET_PITCH); - } - private static boolean hasCorrectPitch(Transform3d camToTarget) { return Math.abs(camToTarget.getRotation().getY() - EXPECTED_CAM_TO_TARGET_PITCH) < CAM_TO_TARGET_PITCH_TOLERANCE; @@ -99,36 +94,12 @@ private static PhotonPipelineResult filteredPipelineResult(PhotonPipelineResult if (hasCorrectPitch(target.getAlternateCameraToTarget())) { target = swapBestAndAltTransforms(target); copy.targets.set(i, target); - if (canPrint) { - System.out.println( - "Swapped best and alt transform for target " - + i - + " (best pitch=" - + target.getBestCameraToTarget().getRotation().getY() - + ", alt pitch=" - + target.getAlternateCameraToTarget().getRotation().getY() - + ")"); - } } else { copy.targets.remove(i); - if (canPrint) { - System.out.println( - "Removed target " - + i - + " (best pitch=" - + target.getBestCameraToTarget().getRotation().getY() - + ", alt pitch=" - + target.getAlternateCameraToTarget().getRotation().getY() - + ") because of bad pitch "); - } continue; } if (target.getPoseAmbiguity() > MAX_POSE_AMBIGUITY) { copy.targets.remove(i); - if (canPrint) { - System.out.println( - "Excluded target " + i + " with ambiguity " + target.getPoseAmbiguity()); - } continue; } } @@ -150,9 +121,6 @@ private static PhotonPipelineResult filteredPipelineResult(PhotonPipelineResult private double lastTimestampSeconds = 0; private Pose2d lastFieldPose = new Pose2d(-1, -1, new Rotation2d()); - private static double lastPrintTimeSeconds = -1; - private static boolean canPrint = false; - private static final AprilTagFieldLayout fieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); @@ -199,19 +167,10 @@ public AprilTagsProcessor(DrivebaseWrapper aprilTagsHelper) { } public void update() { - double now = Timer.getFPGATimestamp(); - canPrint = now - lastPrintTimeSeconds >= 1; - if (canPrint) { - lastPrintTimeSeconds = now; - System.out.println("Got update at " + now); - } latestResult = photonCamera.getLatestResult(); latestFilteredResult = filteredPipelineResult(latestResult); latestPose = photonPoseEstimator.update(latestFilteredResult); if (latestPose.isPresent()) { - if (canPrint) { - System.out.println("Have result"); - } lastTimestampSeconds = latestPose.get().timestampSeconds; lastFieldPose = latestPose.get().estimatedPose.toPose2d(); rawVisionFieldObject.setPose(lastFieldPose); From 4f96d34a7d6251950bbdbfc9fe8e5987af3157bf Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Thu, 14 Mar 2024 21:22:58 -0700 Subject: [PATCH 3/8] Move ambiguity filter back This was supposed to have been moved after the pitch check for logging purposes, but I moved it to the wrong level --- .../frc/team2412/robot/sensors/AprilTagsProcessor.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index f2f223fb..66d8eb03 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -90,6 +90,10 @@ private static PhotonPipelineResult filteredPipelineResult(PhotonPipelineResult var copy = copy(result); for (int i = copy.targets.size() - 1; i >= 0; --i) { var target = copy.targets.get(i); + if (target.getPoseAmbiguity() > MAX_POSE_AMBIGUITY) { + copy.targets.remove(i); + continue; + } if (!hasCorrectPitch(target.getBestCameraToTarget())) { if (hasCorrectPitch(target.getAlternateCameraToTarget())) { target = swapBestAndAltTransforms(target); @@ -98,10 +102,6 @@ private static PhotonPipelineResult filteredPipelineResult(PhotonPipelineResult copy.targets.remove(i); continue; } - if (target.getPoseAmbiguity() > MAX_POSE_AMBIGUITY) { - copy.targets.remove(i); - continue; - } } } return copy; From d4b91a986b2c605a3006f8c9680e2d6e6240a4be Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Thu, 14 Mar 2024 21:29:01 -0700 Subject: [PATCH 4/8] Adjust ROBOT_TO_CAM Not tested, roll may need to be negated --- .../java/frc/team2412/robot/sensors/AprilTagsProcessor.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index 66d8eb03..e5e18d51 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -46,7 +46,7 @@ public class AprilTagsProcessor { Units.inchesToMeters(27.0 / 2.0 - 0.94996), 0, Units.inchesToMeters(8.12331), - new Rotation3d(0, Units.degreesToRadians(-30), 0)); + new Rotation3d(Units.degreesToRadians(90), Units.degreesToRadians(-30), 0)); // TODO Measure these private static final Vector STANDARD_DEVS = VecBuilder.fill(1, 1, Units.degreesToRadians(30)); From 5526b7727a72270b3b0e3024f784ca75b784e16a Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Mon, 18 Mar 2024 14:49:28 -0700 Subject: [PATCH 5/8] Change pitch check to calculate pitch of robot to tag transform This guarantees the ROBOT_TO_CAM transform is correctly accounted for --- .../team2412/robot/sensors/AprilTagsProcessor.java | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index e5e18d51..4a7e0336 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -61,15 +61,12 @@ private static PhotonPipelineResult copy(PhotonPipelineResult result) { private static final double MAX_POSE_AMBIGUITY = 0.1; - // Both of these are in radians - // Negate to convert robot to cam to cam to robot, and negate again because tag is flipped, - // leading to no net effect - private static final double EXPECTED_CAM_TO_TARGET_PITCH = ROBOT_TO_CAM.getRotation().getY(); - private static final double CAM_TO_TARGET_PITCH_TOLERANCE = 0.1; + // Radians + private static final double ROBOT_TO_TARGET_PITCH_TOLERANCE = 0.1; private static boolean hasCorrectPitch(Transform3d camToTarget) { - return Math.abs(camToTarget.getRotation().getY() - EXPECTED_CAM_TO_TARGET_PITCH) - < CAM_TO_TARGET_PITCH_TOLERANCE; + return Math.abs(ROBOT_TO_CAM.getRotation().plus(camToTarget.getRotation()).getY()) + < ROBOT_TO_TARGET_PITCH_TOLERANCE; } private static PhotonTrackedTarget swapBestAndAltTransforms(PhotonTrackedTarget target) { From 5ba5c9ac64541c5d579090810192bfe440758571 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Mon, 18 Mar 2024 19:29:03 -0700 Subject: [PATCH 6/8] Fix rotation calculation (tested) --- .../java/frc/team2412/robot/sensors/AprilTagsProcessor.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index 4a7e0336..d5db2097 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -65,7 +65,8 @@ private static PhotonPipelineResult copy(PhotonPipelineResult result) { private static final double ROBOT_TO_TARGET_PITCH_TOLERANCE = 0.1; private static boolean hasCorrectPitch(Transform3d camToTarget) { - return Math.abs(ROBOT_TO_CAM.getRotation().plus(camToTarget.getRotation()).getY()) + // Intrinsic robot to cam + cam to target = extrinsic cam to target + robot to cam + return Math.abs(camToTarget.getRotation().plus(ROBOT_TO_CAM.getRotation()).getY()) < ROBOT_TO_TARGET_PITCH_TOLERANCE; } From 9c3978a6d30ef3f2d3287ea8e832909a92ba8903 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Mon, 18 Mar 2024 19:46:41 -0700 Subject: [PATCH 7/8] Improve Shuffleboard layout --- .../robot/sensors/AprilTagsProcessor.java | 51 ++++++++++++++----- 1 file changed, 39 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index d5db2097..0b72ebd3 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -111,12 +111,13 @@ private static PhotonPipelineResult filteredPipelineResult(PhotonPipelineResult private final FieldObject2d rawVisionFieldObject; // These are always set with every pipeline result + private double lastRawTimestampSeconds = 0; private PhotonPipelineResult latestResult = null; private PhotonPipelineResult latestFilteredResult = null; private Optional latestPose = Optional.empty(); // These are only set when there's a valid pose - private double lastTimestampSeconds = 0; + private double lastValidTimestampSeconds = 0; private Pose2d lastFieldPose = new Pose2d(-1, -1, new Rotation2d()); private static final AprilTagFieldLayout fieldLayout = @@ -149,44 +150,70 @@ public AprilTagsProcessor(DrivebaseWrapper aprilTagsHelper) { event -> update()); ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("AprilTags"); - shuffleboardTab.addBoolean("Has targets", this::hasTargets).withPosition(0, 0).withSize(1, 1); shuffleboardTab - .addInteger("Num targets", this::getNumTargets) + .addDouble("Last raw timestamp", this::getLastRawTimestampSeconds) + .withPosition(0, 0) + .withSize(1, 1); + shuffleboardTab + .addInteger("Raw num targets", this::getRawNumTargets) .withPosition(0, 1) .withSize(1, 1); shuffleboardTab - .addDouble("Last timestamp", this::getLastTimestampSeconds) + .addInteger("Filtered num targets", this::getFilteredNumTargets) + .withPosition(1, 1) + .withSize(1, 1); + shuffleboardTab + .addBoolean("Has valid targets", this::hasTargets) + .withPosition(1, 2) + .withSize(1, 1); + shuffleboardTab + .addDouble("Last timestamp", this::getLastValidTimestampSeconds) .withPosition(1, 0) .withSize(1, 1); shuffleboardTab .add("3d pose on field", new SendablePose3d(this::getRobotPose)) .withPosition(2, 0) - .withSize(1, 6); + .withSize(2, 2); } public void update() { latestResult = photonCamera.getLatestResult(); latestFilteredResult = filteredPipelineResult(latestResult); + lastRawTimestampSeconds = latestResult.getTimestampSeconds(); latestPose = photonPoseEstimator.update(latestFilteredResult); if (latestPose.isPresent()) { - lastTimestampSeconds = latestPose.get().timestampSeconds; + lastValidTimestampSeconds = latestPose.get().timestampSeconds; lastFieldPose = latestPose.get().estimatedPose.toPose2d(); rawVisionFieldObject.setPose(lastFieldPose); - aprilTagsHelper.addVisionMeasurement(lastFieldPose, lastTimestampSeconds, STANDARD_DEVS); + aprilTagsHelper.addVisionMeasurement(lastFieldPose, lastValidTimestampSeconds, STANDARD_DEVS); var estimatedPose = aprilTagsHelper.getEstimatedPosition(); aprilTagsHelper.getField().setRobotPose(estimatedPose); photonPoseEstimator.setLastPose(estimatedPose); } } - public boolean hasTargets() { - return latestPose.isPresent(); + /** + * Returns the timestamp of the last result we got (regardless of whether it has any valid + * targets) + * + * @return The timestamp of the last result we got in seconds since FPGA startup. + */ + public double getLastRawTimestampSeconds() { + return lastRawTimestampSeconds; } - public int getNumTargets() { + public int getRawNumTargets() { return latestResult == null ? -1 : latestResult.getTargets().size(); } + public int getFilteredNumTargets() { + return latestFilteredResult == null ? -1 : latestFilteredResult.getTargets().size(); + } + + public boolean hasTargets() { + return latestPose.isPresent(); + } + /** * Calculates the robot pose using the best target. Returns null if there is no known robot pose. * @@ -204,7 +231,7 @@ public Pose3d getRobotPose() { * * @return The time we last saw an AprilTag in seconds since FPGA startup. */ - public double getLastTimestampSeconds() { - return lastTimestampSeconds; + public double getLastValidTimestampSeconds() { + return lastValidTimestampSeconds; } } From 31a7fa011651202204ecb8918e66b659c6dc9de5 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Mon, 18 Mar 2024 19:54:34 -0700 Subject: [PATCH 8/8] Check roll as well as pitch --- .../team2412/robot/sensors/AprilTagsProcessor.java | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index 0b72ebd3..dcbf4461 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -62,12 +62,14 @@ private static PhotonPipelineResult copy(PhotonPipelineResult result) { private static final double MAX_POSE_AMBIGUITY = 0.1; // Radians + private static final double ROBOT_TO_TARGET_ROLL_TOLERANCE = 0.1; private static final double ROBOT_TO_TARGET_PITCH_TOLERANCE = 0.1; - private static boolean hasCorrectPitch(Transform3d camToTarget) { + private static boolean hasValidRotation(Transform3d camToTarget) { // Intrinsic robot to cam + cam to target = extrinsic cam to target + robot to cam - return Math.abs(camToTarget.getRotation().plus(ROBOT_TO_CAM.getRotation()).getY()) - < ROBOT_TO_TARGET_PITCH_TOLERANCE; + var robotToTargetRotation = camToTarget.getRotation().plus(ROBOT_TO_CAM.getRotation()); + return (Math.abs(robotToTargetRotation.getX()) < ROBOT_TO_TARGET_ROLL_TOLERANCE) + && (Math.abs(robotToTargetRotation.getY()) < ROBOT_TO_TARGET_PITCH_TOLERANCE); } private static PhotonTrackedTarget swapBestAndAltTransforms(PhotonTrackedTarget target) { @@ -92,8 +94,8 @@ private static PhotonPipelineResult filteredPipelineResult(PhotonPipelineResult copy.targets.remove(i); continue; } - if (!hasCorrectPitch(target.getBestCameraToTarget())) { - if (hasCorrectPitch(target.getAlternateCameraToTarget())) { + if (!hasValidRotation(target.getBestCameraToTarget())) { + if (hasValidRotation(target.getAlternateCameraToTarget())) { target = swapBestAndAltTransforms(target); copy.targets.set(i, target); } else {