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

Run multitag on coprocessor (#816)

Browse Source
This commit is contained in:
Matt
2023-10-17 10:20:00 -04:00
committed by GitHub
parent ededc4f130
commit 47bd077bbb
72 changed files with 1708 additions and 1801 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
View File

@@ -26,6 +26,7 @@ package org.photonvision;
import edu.wpi.first.math.geometry.Pose3d;
import java.util.List;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.targeting.PhotonTrackedTarget;
/** An estimated pose based on pipeline result */
@@ -39,6 +40,9 @@ public class EstimatedRobotPose {
/** A list of the targets used to compute this pose */
public final List<PhotonTrackedTarget> targetsUsed;
/** The strategy actually used to produce this pose */
public final PoseStrategy strategy;
/**
* Constructs an EstimatedRobotPose
*
@@ -46,9 +50,13 @@ public class EstimatedRobotPose {
* @param timestampSeconds timestamp of the estimate
*/
public EstimatedRobotPose(
Pose3d estimatedPose, double timestampSeconds, List<PhotonTrackedTarget> targetsUsed) {
Pose3d estimatedPose,
double timestampSeconds,
List<PhotonTrackedTarget> targetsUsed,
PoseStrategy strategy) {
this.estimatedPose = estimatedPose;
this.timestampSeconds = timestampSeconds;
this.targetsUsed = targetsUsed;
this.strategy = strategy;
}
}

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

@@ -24,7 +24,11 @@
package org.photonvision;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.numbers.*;
@@ -41,6 +45,7 @@ import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.PubSubOption;
import edu.wpi.first.networktables.RawSubscriber;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.networktables.StringSubscriber;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
@@ -72,6 +77,7 @@ public class PhotonCamera implements AutoCloseable {
IntegerSubscriber heartbeatEntry;
private DoubleArraySubscriber cameraIntrinsicsSubscriber;
private DoubleArraySubscriber cameraDistortionSubscriber;
private StringPublisher atflPublisher;
@Override
public void close() {
@@ -95,6 +101,7 @@ public class PhotonCamera implements AutoCloseable {
pipelineIndexRequest.close();
cameraIntrinsicsSubscriber.close();
cameraDistortionSubscriber.close();
atflPublisher.close();
}
private final String path;
@@ -114,6 +121,7 @@ public class PhotonCamera implements AutoCloseable {
Packet packet = new Packet(1);
// Existing is enough to make this multisubscriber do its thing
private final MultiSubscriber m_topicNameSubscriber;
/**
@@ -150,6 +158,10 @@ public class PhotonCamera implements AutoCloseable {
ledModeState = photonvision_root_table.getIntegerTopic("ledModeState").subscribe(-1);
versionEntry = photonvision_root_table.getStringTopic("version").subscribe("");
atflPublisher = photonvision_root_table.getStringTopic("apriltag_field_layout").publish();
// Save the layout locally on Rio; on reboot, should be pushed out to NT clients
atflPublisher.getTopic().setPersistent(true);
m_topicNameSubscriber =
new MultiSubscriber(
instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true));
@@ -319,6 +331,28 @@ public class PhotonCamera implements AutoCloseable {
return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC;
}
/**
* Set the Apriltag Field Layout used by all PhotonVision coprocessors that are (or might later)
* connect to this robot. The topic is marked as persistant, so even if you only call this once
* ever, it will be saved on the RoboRIO and pushed out to all NT clients when code reboots.
* PhotonVision will also store a copy of this layout locally on the coprocessor, but subscribes
* to this topic and the local copy will be updated when this function is called.
*
* @param layout The layout to use for *all* PhotonVision cameras
* @return Success of serializing the JSON. This does *not* mean that all PhotonVision clients
* have updated their internal layouts.
*/
public boolean setApriltagFieldLayout(AprilTagFieldLayout layout) {
try {
var layout_json = new ObjectMapper().writeValueAsString(layout);
atflPublisher.set(layout_json);
return true;
} catch (JsonProcessingException e) {
MathSharedStore.reportError("Error setting ATFL in " + this.getName(), e.getStackTrace());
}
return false;
}
public Optional<Matrix<N3, N3>> getCameraMatrix() {
var cameraMatrix = cameraIntrinsicsSubscriber.get();
if (cameraMatrix != null && cameraMatrix.length == 9) {

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

@@ -68,8 +68,17 @@ public class PhotonPoseEstimator {
/** Return the average of the best target poses using ambiguity as weight. */
AVERAGE_BEST_TARGETS,
/** Use all visible tags to compute a single pose estimate.. */
MULTI_TAG_PNP
/**
* Use all visible tags to compute a single pose estimate on coprocessor. This option needs to
* be enabled on the PhotonVision web UI as well.
*/
MULTI_TAG_PNP_ON_COPROCESSOR,
/**
* Use all visible tags to compute a single pose estimate. This runs on the RoboRIO, and can
* take a lot of time.
*/
MULTI_TAG_PNP_ON_RIO
}
private AprilTagFieldLayout fieldTags;
@@ -173,7 +182,8 @@ public class PhotonPoseEstimator {
*/
public void setMultiTagFallbackStrategy(PoseStrategy strategy) {
checkUpdate(this.multiTagFallbackStrategy, strategy);
if (strategy == PoseStrategy.MULTI_TAG_PNP) {
if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR
|| strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO) {
DriverStation.reportWarning(
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", false);
strategy = PoseStrategy.LOWEST_AMBIGUITY;
@@ -357,8 +367,11 @@ public class PhotonPoseEstimator {
case AVERAGE_BEST_TARGETS:
estimatedPose = averageBestTargetsStrategy(cameraResult);
break;
case MULTI_TAG_PNP:
estimatedPose = multiTagPNPStrategy(cameraResult, cameraMatrix, distCoeffs);
case MULTI_TAG_PNP_ON_RIO:
estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
break;
case MULTI_TAG_PNP_ON_COPROCESSOR:
estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs);
break;
default:
DriverStation.reportError(
@@ -373,7 +386,28 @@ public class PhotonPoseEstimator {
return estimatedPose;
}
private Optional<EstimatedRobotPose> multiTagPNPStrategy(
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N5, N1>> distCoeffsOpt) {
if (result.getMultiTagResult().estimatedPose.isPresent) {
var best_tf = result.getMultiTagResult().estimatedPose.best;
var best =
new Pose3d()
.plus(best_tf) // field-to-camera
.plus(robotToCamera.inverse()); // field-to-robot
return Optional.of(
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
} else {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
}
}
private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N5, N1>> distCoeffsOpt) {
@@ -395,7 +429,11 @@ public class PhotonPoseEstimator {
.plus(robotToCamera.inverse()); // field-to-robot
return Optional.of(
new EstimatedRobotPose(best, result.getTimestampSeconds(), result.getTargets()));
new EstimatedRobotPose(
best,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_RIO));
}
/**
@@ -440,7 +478,8 @@ public class PhotonPoseEstimator {
.transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets()));
result.getTargets(),
PoseStrategy.LOWEST_AMBIGUITY));
}
/**
@@ -494,7 +533,8 @@ public class PhotonPoseEstimator {
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets());
result.getTargets(),
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
}
if (bestTransformDelta < smallestHeightDifference) {
@@ -506,7 +546,8 @@ public class PhotonPoseEstimator {
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets());
result.getTargets(),
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT);
}
}
@@ -568,13 +609,19 @@ public class PhotonPoseEstimator {
smallestPoseDelta = altDifference;
lowestDeltaPose =
new EstimatedRobotPose(
altTransformPosition, result.getTimestampSeconds(), result.getTargets());
altTransformPosition,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
}
if (bestDifference < smallestPoseDelta) {
smallestPoseDelta = bestDifference;
lowestDeltaPose =
new EstimatedRobotPose(
bestTransformPosition, result.getTimestampSeconds(), result.getTargets());
bestTransformPosition,
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.CLOSEST_TO_REFERENCE_POSE);
}
}
return Optional.ofNullable(lowestDeltaPose);
@@ -617,7 +664,8 @@ public class PhotonPoseEstimator {
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds(),
result.getTargets()));
result.getTargets(),
PoseStrategy.AVERAGE_BEST_TARGETS));
}
totalAmbiguity += 1.0 / target.getPoseAmbiguity();
@@ -649,7 +697,10 @@ public class PhotonPoseEstimator {
return Optional.of(
new EstimatedRobotPose(
new Pose3d(transform, rotation), result.getTimestampSeconds(), result.getTargets()));
new Pose3d(transform, rotation),
result.getTimestampSeconds(),
result.getTargets(),
PoseStrategy.AVERAGE_BEST_TARGETS));
}
/**

419
photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java
View File

@@ -1,419 +0,0 @@
/*
* MIT License
*
* Copyright (c) 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 edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose2d;
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;
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 org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
/**
* @deprecated Use {@link PhotonPoseEstimator}
*/
@Deprecated
public class RobotPoseEstimator {
/**
*
*
* <ul>
* <li><strong>LOWEST_AMBIGUITY</strong>: Choose the Pose with the lowest ambiguity
* <li><strong>CLOSEST_TO_CAMERA_HEIGHT</strong>: Choose the Pose which is closest to the camera
* height
* <li><strong>CLOSEST_TO_REFERENCE_POSE</strong>: Choose the Pose which is closest to the pose
* from setReferencePose()
* <li><strong>CLOSEST_TO_LAST_POSE</strong>: Choose the Pose which is closest to the last pose
* calculated
* </ul>
*/
public enum PoseStrategy {
LOWEST_AMBIGUITY,
CLOSEST_TO_CAMERA_HEIGHT,
CLOSEST_TO_REFERENCE_POSE,
CLOSEST_TO_LAST_POSE,
AVERAGE_BEST_TARGETS
}
private AprilTagFieldLayout aprilTags;
private PoseStrategy strategy;
private List<Pair<PhotonCamera, Transform3d>> cameras;
private Pose3d lastPose;
private Pose3d referencePose;
private Set<Integer> reportedErrors;
/**
* Create a new RobotPoseEstimator.
*
* @param aprilTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param cameras An ArrayList of Pairs of PhotonCameras and their respective Transform3ds from
* the center of the robot to the camera mount positions (ie, robot ➔ camera).
*/
public RobotPoseEstimator(
AprilTagFieldLayout aprilTags,
PoseStrategy strategy,
List<Pair<PhotonCamera, Transform3d>> cameras) {
this.aprilTags = aprilTags;
this.strategy = strategy;
this.cameras = cameras;
lastPose = new Pose3d();
reportedErrors = new HashSet<>();
}
/**
* Update the estimated pose using the selected strategy.
*
* @return The updated estimated pose and the latency in milliseconds. Estimated pose may be null
* if no targets were seen
*/
public Optional<Pair<Pose3d, Double>> update() {
if (cameras.isEmpty()) {
DriverStation.reportError("[RobotPoseEstimator] Missing any camera!", false);
return Optional.empty();
}
Pair<Pose3d, Double> pair = getResultFromActiveStrategy();
if (pair != null) {
lastPose = pair.getFirst();
}
return Optional.ofNullable(pair);
}
private Pair<Pose3d, Double> getResultFromActiveStrategy() {
switch (strategy) {
case LOWEST_AMBIGUITY:
return lowestAmbiguityStrategy();
case CLOSEST_TO_CAMERA_HEIGHT:
return closestToCameraHeightStrategy();
case CLOSEST_TO_REFERENCE_POSE:
return closestToReferencePoseStrategy();
case CLOSEST_TO_LAST_POSE:
return closestToLastPoseStrategy();
case AVERAGE_BEST_TARGETS:
return averageBestTargetsStrategy();
default:
DriverStation.reportError("[RobotPoseEstimator] Invalid pose strategy!", false);
return null;
}
}
private Pair<Pose3d, Double> lowestAmbiguityStrategy() {
int lowestAI = -1;
int lowestAJ = -1;
double lowestAmbiguityScore = 10;
ArrayList<PhotonPipelineResult> results = new ArrayList<PhotonPipelineResult>(cameras.size());
// Sample result from each camera
for (int i = 0; i < cameras.size(); i++) {
Pair<PhotonCamera, Transform3d> p = cameras.get(i);
results.add(p.getFirst().getLatestResult());
}
// Loop over each ambiguity of all the cameras
for (int i = 0; i < cameras.size(); i++) {
List<PhotonTrackedTarget> targets = results.get(i).targets;
for (int j = 0; j < targets.size(); j++) {
if (targets.get(j).getPoseAmbiguity() < lowestAmbiguityScore) {
lowestAI = i;
lowestAJ = j;
lowestAmbiguityScore = targets.get(j).getPoseAmbiguity();
}
}
}
// No targets, return null
if (lowestAI == -1 || lowestAJ == -1) {
return null;
}
// Pick the lowest and do the heavy calculations
PhotonTrackedTarget bestTarget = results.get(lowestAI).targets.get(lowestAJ);
Optional<Pose3d> fiducialPose = aprilTags.getTagPose(bestTarget.getFiducialId());
if (fiducialPose.isEmpty()) {
reportFiducialPoseError(bestTarget.getFiducialId());
return null;
}
return Pair.of(
fiducialPose
.get()
.transformBy(bestTarget.getBestCameraToTarget().inverse())
.transformBy(cameras.get(lowestAI).getSecond().inverse()),
results.get(lowestAI).getLatencyMillis());
}
private Pair<Pose3d, Double> closestToCameraHeightStrategy() {
double smallestHeightDifference = Double.MAX_VALUE;
double latency = 0;
Pose3d pose = null;
for (int i = 0; i < cameras.size(); i++) {
Pair<PhotonCamera, Transform3d> p = cameras.get(i);
var result = p.getFirst().getLatestResult();
List<PhotonTrackedTarget> targets = result.targets;
for (int j = 0; j < targets.size(); j++) {
PhotonTrackedTarget target = targets.get(j);
Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
if (fiducialPose.isEmpty()) {
reportFiducialPoseError(target.getFiducialId());
continue;
}
Pose3d targetPose = fiducialPose.get();
double alternativeDifference =
Math.abs(
p.getSecond().getZ()
- targetPose.transformBy(target.getAlternateCameraToTarget().inverse()).getZ());
double bestDifference =
Math.abs(
p.getSecond().getZ()
- targetPose.transformBy(target.getBestCameraToTarget().inverse()).getZ());
if (alternativeDifference < smallestHeightDifference) {
smallestHeightDifference = alternativeDifference;
pose =
targetPose
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(p.getSecond().inverse());
latency = result.getLatencyMillis();
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose =
targetPose
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(p.getSecond().inverse());
latency = result.getLatencyMillis();
}
}
}
return Pair.of(pose, latency);
}
private Pair<Pose3d, Double> closestToReferencePoseStrategy() {
if (referencePose == null) {
DriverStation.reportError(
"[RobotPoseEstimator] Tried to use reference pose strategy without setting the reference!",
false);
return null;
}
double smallestDifference = 10e9;
double latency = 0;
Pose3d pose = null;
for (int i = 0; i < cameras.size(); i++) {
Pair<PhotonCamera, Transform3d> p = cameras.get(i);
var result = p.getFirst().getLatestResult();
List<PhotonTrackedTarget> targets = result.targets;
for (int j = 0; j < targets.size(); j++) {
PhotonTrackedTarget target = targets.get(j);
Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
if (fiducialPose.isEmpty()) {
reportFiducialPoseError(target.getFiducialId());
continue;
}
Pose3d targetPose = fiducialPose.get();
Pose3d botBestPose =
targetPose
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(p.getSecond().inverse());
Pose3d botAltPose =
targetPose
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(p.getSecond().inverse());
double alternativeDifference = Math.abs(calculateDifference(referencePose, botAltPose));
double bestDifference = Math.abs(calculateDifference(referencePose, botBestPose));
if (alternativeDifference < smallestDifference) {
smallestDifference = alternativeDifference;
pose = botAltPose;
latency = result.getLatencyMillis();
}
if (bestDifference < smallestDifference) {
smallestDifference = bestDifference;
pose = botBestPose;
latency = result.getLatencyMillis();
}
}
}
return Pair.of(pose, latency);
}
private Pair<Pose3d, Double> closestToLastPoseStrategy() {
setReferencePose(lastPose);
return closestToReferencePoseStrategy();
}
/** Return the average of the best target poses using ambiguity as weight */
private Pair<Pose3d, Double> averageBestTargetsStrategy() {
// Pair of Double, Double = Ambiguity, Mili
List<Pair<Pose3d, Pair<Double, Double>>> tempPoses = new ArrayList<>();
double totalAmbiguity = 0;
for (int i = 0; i < cameras.size(); i++) {
Pair<PhotonCamera, Transform3d> p = cameras.get(i);
var result = p.getFirst().getLatestResult();
List<PhotonTrackedTarget> targets = result.targets;
for (int j = 0; j < targets.size(); j++) {
PhotonTrackedTarget target = targets.get(j);
Optional<Pose3d> fiducialPose = aprilTags.getTagPose(target.getFiducialId());
if (fiducialPose.isEmpty()) {
reportFiducialPoseError(target.getFiducialId());
continue;
}
Pose3d targetPose = fiducialPose.get();
try {
totalAmbiguity += 1. / target.getPoseAmbiguity();
} catch (ArithmeticException e) {
// A total ambiguity of zero exists, using that pose instead!",
return Pair.of(
targetPose
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(p.getSecond().inverse()),
result.getLatencyMillis());
}
tempPoses.add(
Pair.of(
targetPose
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(p.getSecond().inverse()),
Pair.of(target.getPoseAmbiguity(), result.getLatencyMillis())));
}
}
Translation3d transform = new Translation3d();
Rotation3d rotation = new Rotation3d();
double latency = 0;
if (tempPoses.isEmpty()) {
return null;
}
if (totalAmbiguity == 0) {
Pose3d p = tempPoses.get(0).getFirst();
double l = tempPoses.get(0).getSecond().getSecond();
return Pair.of(p, l);
}
for (Pair<Pose3d, Pair<Double, Double>> pair : tempPoses) {
double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity;
transform = transform.plus(pair.getFirst().getTranslation().times(weight));
rotation = rotation.plus(pair.getFirst().getRotation().times(weight));
latency += pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well
}
return Pair.of(new Pose3d(transform, rotation), latency);
}
/**
* Difference is defined as the vector magnitude between the two poses
*
* @return The absolute "difference" (>=0) between two Pose3ds.
*/
private double calculateDifference(Pose3d x, Pose3d y) {
return x.getTranslation().getDistance(y.getTranslation());
}
/**
* @param aprilTags the aprilTags to set
*/
public void setAprilTags(AprilTagFieldLayout aprilTags) {
this.aprilTags = aprilTags;
}
/**
* @return the aprilTags
*/
public AprilTagFieldLayout getAprilTags() {
return aprilTags;
}
/**
* @return the strategy
*/
public PoseStrategy getStrategy() {
return strategy;
}
/**
* @param strategy the strategy to set
*/
public void setStrategy(PoseStrategy strategy) {
this.strategy = strategy;
}
/**
* @return the referencePose
*/
public Pose3d getReferencePose() {
return referencePose;
}
/**
* Update the stored reference pose for use with CLOSEST_TO_REFERENCE_POSE
*
* @param referencePose the referencePose to set
*/
public void setReferencePose(Pose3d referencePose) {
this.referencePose = referencePose;
}
/**
* Update the stored reference pose for use with CLOSEST_TO_REFERENCE_POSE
*
* @param referencePose the referencePose to set
*/
public void setReferencePose(Pose2d referencePose) {
setReferencePose(new Pose3d(referencePose));
}
/**
* Update the stored last pose. Useful for setting the initial estimate with CLOSEST_TO_LAST_POSE
*
* @param lastPose the lastPose to set
*/
public void setLastPose(Pose3d lastPose) {
this.lastPose = lastPose;
}
private void reportFiducialPoseError(int fiducialId) {
if (!reportedErrors.contains(fiducialId)) {
DriverStation.reportError(
"[RobotPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false);
reportedErrors.add(fiducialId);
}
}
}

66
photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java
View File

@@ -1,66 +0,0 @@
/*
* MIT License
*
* Copyright (c) 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.estimation;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform3d;
/** Holds various helper geometries describing the relation between camera and target. */
public class CameraTargetRelation {
public final Pose3d camPose;
public final Transform3d camToTarg;
public final double camToTargDist;
public final double camToTargDistXY;
public final Rotation2d camToTargYaw;
public final Rotation2d camToTargPitch;
/** Angle from the camera's relative x-axis */
public final Rotation2d camToTargAngle;
public final Transform3d targToCam;
public final Rotation2d targToCamYaw;
public final Rotation2d targToCamPitch;
/** Angle from the target's relative x-axis */
public final Rotation2d targToCamAngle;
public CameraTargetRelation(Pose3d cameraPose, Pose3d targetPose) {
this.camPose = cameraPose;
camToTarg = new Transform3d(cameraPose, targetPose);
camToTargDist = camToTarg.getTranslation().getNorm();
camToTargDistXY =
Math.hypot(camToTarg.getTranslation().getX(), camToTarg.getTranslation().getY());
camToTargYaw = new Rotation2d(camToTarg.getX(), camToTarg.getY());
camToTargPitch = new Rotation2d(camToTargDistXY, -camToTarg.getZ());
camToTargAngle =
new Rotation2d(Math.hypot(camToTargYaw.getRadians(), camToTargPitch.getRadians()));
targToCam = new Transform3d(targetPose, cameraPose);
targToCamYaw = new Rotation2d(targToCam.getX(), targToCam.getY());
targToCamPitch = new Rotation2d(camToTargDistXY, -targToCam.getZ());
targToCamAngle =
new Rotation2d(Math.hypot(targToCamYaw.getRadians(), targToCamPitch.getRadians()));
}
}

582
photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View File

@@ -1,582 +0,0 @@
/*
* MIT License
*
* Copyright (c) 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.estimation;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.VecBuilder;
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;
import edu.wpi.first.math.numbers.*;
import edu.wpi.first.util.RuntimeLoader;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.ejml.simple.SimpleMatrix;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.MatOfInt;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.MatOfPoint3f;
import org.opencv.core.Point;
import org.opencv.core.Point3;
import org.opencv.core.Rect;
import org.opencv.core.RotatedRect;
import org.opencv.imgproc.Imgproc;
import org.photonvision.targeting.TargetCorner;
public final class OpenCVHelp {
private static RotTrlTransform3d NWU_TO_EDN;
private static RotTrlTransform3d EDN_TO_NWU;
static {
try {
var loader =
new RuntimeLoader<>(
Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
loader.loadLibrary();
} catch (Exception e) {
throw new RuntimeException("Failed to load native libraries!", e);
}
NWU_TO_EDN =
new RotTrlTransform3d(
new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, -1, 0, 0, 0, -1, 1, 0, 0)),
new Translation3d());
EDN_TO_NWU =
new RotTrlTransform3d(
new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0)),
new Translation3d());
}
public static Mat matrixToMat(SimpleMatrix matrix) {
var mat = new Mat(matrix.numRows(), matrix.numCols(), CvType.CV_64F);
mat.put(0, 0, matrix.getDDRM().getData());
return mat;
}
public static Matrix<Num, Num> matToMatrix(Mat mat) {
double[] data = new double[(int) mat.total() * mat.channels()];
var doubleMat = new Mat(mat.rows(), mat.cols(), CvType.CV_64F);
mat.convertTo(doubleMat, CvType.CV_64F);
doubleMat.get(0, 0, data);
return new Matrix<>(new SimpleMatrix(mat.rows(), mat.cols(), true, data));
}
/**
* Creates a new {@link MatOfPoint3f} with these 3d translations. The opencv tvec is a vector with
* three elements representing {x, y, z} in the EDN coordinate system.
*
* @param translations The translations to convert into a MatOfPoint3f
*/
public static MatOfPoint3f translationToTvec(Translation3d... translations) {
Point3[] points = new Point3[translations.length];
for (int i = 0; i < translations.length; i++) {
var trl = translationNWUtoEDN(translations[i]);
points[i] = new Point3(trl.getX(), trl.getY(), trl.getZ());
}
return new MatOfPoint3f(points);
}
/**
* Returns a new 3d translation from this {@link Mat}. The opencv tvec is a vector with three
* elements representing {x, y, z} in the EDN coordinate system.
*
* @param tvecInput The tvec to create a Translation3d from
*/
public static Translation3d tvecToTranslation(Mat tvecInput) {
float[] data = new float[3];
var wrapped = new Mat(tvecInput.rows(), tvecInput.cols(), CvType.CV_32F);
tvecInput.convertTo(wrapped, CvType.CV_32F);
wrapped.get(0, 0, data);
wrapped.release();
return translationEDNtoNWU(new Translation3d(data[0], data[1], data[2]));
}
/**
* Creates a new {@link MatOfPoint3f} with this 3d rotation. The opencv rvec Mat is a vector with
* three elements representing the axis scaled by the angle in the EDN coordinate system. (angle =
* norm, and axis = rvec / norm)
*
* @param rotation The rotation to convert into a MatOfPoint3f
*/
public static MatOfPoint3f rotationToRvec(Rotation3d rotation) {
rotation = rotationNWUtoEDN(rotation);
return new MatOfPoint3f(new Point3(rotation.getQuaternion().toRotationVector().getData()));
}
/**
* Returns a 3d rotation from this {@link Mat}. The opencv rvec Mat is a vector with three
* elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm,
* and axis = rvec / norm)
*
* @param rvecInput The rvec to create a Rotation3d from
*/
public static Rotation3d rvecToRotation(Mat rvecInput) {
// Get the 'rodriguez' (axis-angle, where the norm is the angle about the normalized direction
// of the vector)
float[] data = new float[3];
var wrapped = new Mat(rvecInput.rows(), rvecInput.cols(), CvType.CV_32F);
rvecInput.convertTo(wrapped, CvType.CV_32F);
wrapped.get(0, 0, data);
wrapped.release();
return rotationEDNtoNWU(new Rotation3d(VecBuilder.fill(data[0], data[1], data[2])));
}
public static Point avgPoint(Point[] points) {
if (points == null || points.length == 0) return null;
var pointMat = new MatOfPoint2f(points);
Core.reduce(pointMat, pointMat, 0, Core.REDUCE_AVG);
var avgPt = pointMat.toArray()[0];
pointMat.release();
return avgPt;
}
public static Point[] cornersToPoints(List<TargetCorner> corners) {
var points = new Point[corners.size()];
for (int i = 0; i < corners.size(); i++) {
var corn = corners.get(i);
points[i] = new Point(corn.x, corn.y);
}
return points;
}
public static Point[] cornersToPoints(TargetCorner... corners) {
var points = new Point[corners.length];
for (int i = 0; i < corners.length; i++) {
points[i] = new Point(corners[i].x, corners[i].y);
}
return points;
}
public static List<TargetCorner> pointsToCorners(Point... points) {
var corners = new ArrayList<TargetCorner>(points.length);
for (int i = 0; i < points.length; i++) {
corners.add(new TargetCorner(points[i].x, points[i].y));
}
return corners;
}
public static List<TargetCorner> pointsToCorners(MatOfPoint2f matInput) {
var corners = new ArrayList<TargetCorner>();
float[] data = new float[(int) matInput.total() * matInput.channels()];
matInput.get(0, 0, data);
for (int i = 0; i < (int) matInput.total(); i++) {
corners.add(new TargetCorner(data[0 + 2 * i], data[1 + 2 * i]));
}
return corners;
}
/**
* Reorders the list, optionally indexing backwards and wrapping around to the last element after
* the first, and shifting all indices in the direction of indexing.
*
* <p>e.g.
*
* <p>({1,2,3}, false, 1) == {2,3,1}
*
* <p>({1,2,3}, true, 0) == {1,3,2}
*
* <p>({1,2,3}, true, 1) == {3,2,1}
*
* @param <T> Element type
* @param elements
* @param backwards If indexing should happen in reverse (0, size-1, size-2, ...)
* @param shiftStart How much the inital index should be shifted (instead of starting at index 0,
* start at shiftStart, negated if backwards)
* @return Reordered list
*/
public static <T> List<T> reorderCircular(List<T> elements, boolean backwards, int shiftStart) {
int size = elements.size();
int dir = backwards ? -1 : 1;
var reordered = new ArrayList<>(elements);
for (int i = 0; i < size; i++) {
int index = (i * dir + shiftStart * dir) % size;
if (index < 0) index = size + index;
reordered.set(i, elements.get(index));
}
return reordered;
}
// TODO: RotTrlTransform3d removal awaiting Rotation3d performance improvements
/**
* Convert a rotation delta from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0}
* in EDN, this would be {0, -1, 0} in NWU.
*/
private static Rotation3d rotationEDNtoNWU(Rotation3d rot) {
return new RotTrlTransform3d(EDN_TO_NWU.apply(rot), new Translation3d())
.apply(EDN_TO_NWU.inverse().getRotation());
}
/**
* Convert a rotation delta from NWU to EDN. For example, if you have a rotation X,Y,Z {1, 0, 0}
* in NWU, this would be {0, 0, 1} in EDN.
*/
private static Rotation3d rotationNWUtoEDN(Rotation3d rot) {
return new RotTrlTransform3d(NWU_TO_EDN.apply(rot), new Translation3d())
.apply(NWU_TO_EDN.inverse().getRotation());
}
/**
* Convert a translation from EDN to NWU. For example, if you have a translation X,Y,Z {1, 0, 0}
* in EDN, this would be {0, -1, 0} in NWU.
*/
private static Translation3d translationEDNtoNWU(Translation3d trl) {
return EDN_TO_NWU.apply(trl);
}
/**
* Convert a translation from NWU to EDN. For example, if you have a translation X,Y,Z {1, 0, 0}
* in NWU, this would be {0, 0, 1} in EDN.
*/
private static Translation3d translationNWUtoEDN(Translation3d trl) {
return NWU_TO_EDN.apply(trl);
}
/**
* Project object points from the 3d world into the 2d camera image. The camera
* properties(intrinsics, distortion) determine the results of this projection.
*
* @param cameraMatrix the camera intrinsics matrix in standard opencv form
* @param distCoeffs the camera distortion matrix in standard opencv form
* @param camRt The change in basis from world coordinates to camera coordinates. See {@link
* RotTrlTransform3d#makeRelativeTo(Pose3d)}.
* @param objectTranslations The 3d points to be projected
* @return The 2d points in pixels which correspond to the camera's image of the 3d points
*/
public static Point[] projectPoints(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
RotTrlTransform3d camRt,
List<Translation3d> objectTranslations) {
// translate to opencv classes
var objectPoints = translationToTvec(objectTranslations.toArray(new Translation3d[0]));
// opencv rvec/tvec describe a change in basis from world to camera
var rvec = rotationToRvec(camRt.getRotation());
var tvec = translationToTvec(camRt.getTranslation());
var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
var distCoeffsMat = new MatOfDouble(matrixToMat(distCoeffs.getStorage()));
var imagePoints = new MatOfPoint2f();
// project to 2d
Calib3d.projectPoints(objectPoints, rvec, tvec, cameraMatrixMat, distCoeffsMat, imagePoints);
var points = imagePoints.toArray();
// release our Mats from native memory
objectPoints.release();
rvec.release();
tvec.release();
cameraMatrixMat.release();
distCoeffsMat.release();
imagePoints.release();
return points;
}
/**
* Undistort 2d image points using a given camera's intrinsics and distortion.
*
* <p>2d image points from {@link #projectPoints(Matrix, Matrix, RotTrlTransform3d, List)
* projectPoints()} will naturally be distorted, so this operation is important if the image
* points need to be directly used (e.g. 2d yaw/pitch).
*
* @param cameraMatrix The camera intrinsics matrix in standard opencv form
* @param distCoeffs The camera distortion matrix in standard opencv form
* @param points The distorted image points
* @return The undistorted image points
*/
public static Point[] undistortPoints(
Matrix<N3, N3> cameraMatrix, Matrix<N5, N1> distCoeffs, Point[] points) {
var distMat = new MatOfPoint2f(points);
var undistMat = new MatOfPoint2f();
var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
Calib3d.undistortImagePoints(distMat, undistMat, cameraMatrixMat, distCoeffsMat);
var undistPoints = undistMat.toArray();
distMat.release();
undistMat.release();
cameraMatrixMat.release();
distCoeffsMat.release();
return undistPoints;
}
/**
* Gets the (upright) rectangle which bounds this contour.
*
* <p>Note that rectangle size and position are stored with ints and do not have sub-pixel
* accuracy.
*
* @param points The points to be bounded
* @return Rectangle bounding the given points
*/
public static Rect getBoundingRect(Point[] points) {
var pointMat = new MatOfPoint2f(points);
var rect = Imgproc.boundingRect(pointMat);
pointMat.release();
return rect;
}
/**
* Gets the rotated rectangle with minimum area which bounds this contour.
*
* <p>Note that rectangle size and position are stored with floats and have sub-pixel accuracy.
*
* @param points The points to be bounded
* @return Rotated rectangle bounding the given points
*/
public static RotatedRect getMinAreaRect(Point[] points) {
var pointMat = new MatOfPoint2f(points);
var rect = Imgproc.minAreaRect(pointMat);
pointMat.release();
return rect;
}
/**
* Gets the convex hull contour (the outline) of a list of points.
*
* @param points The input contour
* @return The subset of points defining the convex hull. Note that these use ints and not floats.
*/
public static Point[] getConvexHull(Point[] points) {
var pointMat = new MatOfPoint(points);
// outputHull gives us indices (of corn) that make a convex hull contour
var outputHull = new MatOfInt();
Imgproc.convexHull(pointMat, outputHull);
int[] indices = outputHull.toArray();
outputHull.release();
pointMat.release();
var convexPoints = new Point[indices.length];
for (int i = 0; i < indices.length; i++) {
convexPoints[i] = points[indices[i]];
}
return convexPoints;
}
/**
* Finds the transformation(s) that map the camera's pose to the target's pose. The camera's pose
* relative to the target is determined by the supplied 3d points of the target's model and their
* associated 2d points imaged by the camera. The supplied model translations must be relative to
* the target's pose.
*
* <p>For planar targets, there may be an alternate solution which is plausible given the 2d image
* points. This has an associated "ambiguity" which describes the ratio of reprojection error
* between the "best" and "alternate" solution.
*
* <p>This method is intended for use with individual AprilTags, and will not work unless 4 points
* are provided.
*
* @param cameraMatrix The camera intrinsics matrix in standard opencv form
* @param distCoeffs The camera distortion matrix in standard opencv form
* @param modelTrls The translations of the object corners. These should have the object pose as
* their origin. These must come in a specific, pose-relative order (in NWU):
* <ul>
* <li>Point 0: [0, -squareLength / 2, squareLength / 2]
* <li>Point 1: [0, squareLength / 2, squareLength / 2]
* <li>Point 2: [0, squareLength / 2, -squareLength / 2]
* <li>Point 3: [0, -squareLength / 2, -squareLength / 2]
* </ul>
*
* @param imagePoints The projection of these 3d object points into the 2d camera image. The order
* should match the given object point translations.
* @return The resulting transformation that maps the camera pose to the target pose and the
* ambiguity if an alternate solution is available.
*/
public static PNPResults solvePNP_SQUARE(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<Translation3d> modelTrls,
Point[] imagePoints) {
// solvepnp inputs
MatOfPoint3f objectMat = new MatOfPoint3f();
MatOfPoint2f imageMat = new MatOfPoint2f();
MatOfDouble cameraMatrixMat = new MatOfDouble();
MatOfDouble distCoeffsMat = new MatOfDouble();
var rvecs = new ArrayList<Mat>();
var tvecs = new ArrayList<Mat>();
Mat rvec = Mat.zeros(3, 1, CvType.CV_32F);
Mat tvec = Mat.zeros(3, 1, CvType.CV_32F);
Mat reprojectionError = Mat.zeros(2, 1, CvType.CV_32F);
try {
// IPPE_SQUARE expects our corners in a specific order
modelTrls = reorderCircular(modelTrls, true, -1);
imagePoints = reorderCircular(Arrays.asList(imagePoints), true, -1).toArray(Point[]::new);
// translate to opencv classes
translationToTvec(modelTrls.toArray(new Translation3d[0])).assignTo(objectMat);
imageMat.fromArray(imagePoints);
matrixToMat(cameraMatrix.getStorage()).assignTo(cameraMatrixMat);
matrixToMat(distCoeffs.getStorage()).assignTo(distCoeffsMat);
float[] errors = new float[2];
Transform3d best = null;
Transform3d alt = null;
for (int tries = 0; tries < 2; tries++) {
// calc rvecs/tvecs and associated reprojection error from image points
Calib3d.solvePnPGeneric(
objectMat,
imageMat,
cameraMatrixMat,
distCoeffsMat,
rvecs,
tvecs,
false,
Calib3d.SOLVEPNP_IPPE_SQUARE,
rvec,
tvec,
reprojectionError);
reprojectionError.get(0, 0, errors);
// convert to wpilib coordinates
best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));
if (tvecs.size() > 1) {
alt = new Transform3d(tvecToTranslation(tvecs.get(1)), rvecToRotation(rvecs.get(1)));
}
// check if we got a NaN result
if (!Double.isNaN(errors[0])) break;
else { // add noise and retry
double[] br = imageMat.get(0, 0);
br[0] -= 0.001;
br[1] -= 0.001;
imageMat.put(0, 0, br);
}
}
// check if solvePnP failed with NaN results and retrying failed
if (Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result");
if (alt != null)
return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]);
else return new PNPResults(best, errors[0]);
}
// solvePnP failed
catch (Exception e) {
System.err.println("SolvePNP_SQUARE failed!");
e.printStackTrace();
return new PNPResults();
} finally {
// release our Mats from native memory
objectMat.release();
imageMat.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();
for (var v : tvecs) v.release();
rvec.release();
tvec.release();
reprojectionError.release();
}
}
/**
* Finds the transformation that maps the camera's pose to the origin of the supplied object. An
* "object" is simply a set of known 3d translations that correspond to the given 2d points. If,
* for example, the object translations are given relative to close-right corner of the blue
* alliance(the default origin), a camera-to-origin transformation is returned. If the
* translations are relative to a target's pose, a camera-to-target transformation is returned.
*
* <p>There must be at least 3 points to use this method. This does not return an alternate
* solution-- if you are intending to use solvePNP on a single AprilTag, see {@link
* #solvePNP_SQUARE} instead.
*
* @param cameraMatrix The camera intrinsics matrix in standard opencv form
* @param distCoeffs The camera distortion matrix in standard opencv form
* @param objectTrls The translations of the object corners, relative to the field.
* @param imagePoints The projection of these 3d object points into the 2d camera image. The order
* should match the given object point translations.
* @return The resulting transformation that maps the camera pose to the target pose. If the 3d
* model points are supplied relative to the origin, this transformation brings the camera to
* the origin.
*/
public static PNPResults solvePNP_SQPNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<Translation3d> objectTrls,
Point[] imagePoints) {
try {
// translate to opencv classes
MatOfPoint3f objectMat = translationToTvec(objectTrls.toArray(new Translation3d[0]));
MatOfPoint2f imageMat = new MatOfPoint2f(imagePoints);
Mat cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
Mat distCoeffsMat = matrixToMat(distCoeffs.getStorage());
var rvecs = new ArrayList<Mat>();
var tvecs = new ArrayList<Mat>();
Mat rvec = Mat.zeros(3, 1, CvType.CV_32F);
Mat tvec = Mat.zeros(3, 1, CvType.CV_32F);
Mat reprojectionError = new Mat();
// calc rvec/tvec from image points
Calib3d.solvePnPGeneric(
objectMat,
imageMat,
cameraMatrixMat,
distCoeffsMat,
rvecs,
tvecs,
false,
Calib3d.SOLVEPNP_SQPNP,
rvec,
tvec,
reprojectionError);
float[] error = new float[1];
reprojectionError.get(0, 0, error);
// convert to wpilib coordinates
var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));
// release our Mats from native memory
objectMat.release();
imageMat.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();
for (var v : tvecs) v.release();
rvec.release();
tvec.release();
reprojectionError.release();
// check if solvePnP failed with NaN results
if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result");
return new PNPResults(best, error[0]);
} catch (Exception e) {
System.err.println("SolvePNP_SQPNP failed!");
e.printStackTrace();
return new PNPResults();
}
}
}

93
photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java
View File

@@ -1,93 +0,0 @@
/*
* MIT License
*
* Copyright (c) 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.estimation;
import edu.wpi.first.math.geometry.Transform3d;
/**
* The best estimated transformation from solvePnP, and possibly an alternate transformation
* depending on the solvePNP method. If an alternate solution is present, the ambiguity value
* represents the ratio of reprojection error in the best solution to the alternate (best /
* alternate).
*
* <p>Note that the coordinate frame of these transforms depends on the implementing solvePnP
* method.
*/
public class PNPResults {
/**
* If this result is valid. A false value indicates there was an error in estimation, and this
* result should not be used.
*/
public final boolean isPresent;
/**
* The best-fit transform. The coordinate frame of this transform depends on the method which gave
* this result.
*/
public final Transform3d best;
/** Reprojection error of the best solution, in pixels */
public final double bestReprojErr;
/**
* Alternate, ambiguous solution from solvepnp. If no alternate solution is found, this is equal
* to the best solution.
*/
public final Transform3d alt;
/** If no alternate solution is found, this is bestReprojErr */
public final double altReprojErr;
/** If no alternate solution is found, this is 0 */
public final double ambiguity;
/** An empty (invalid) result. */
public PNPResults() {
this.isPresent = false;
this.best = new Transform3d();
this.alt = new Transform3d();
this.ambiguity = 0;
this.bestReprojErr = 0;
this.altReprojErr = 0;
}
public PNPResults(Transform3d best, double bestReprojErr) {
this(best, best, 0, bestReprojErr, bestReprojErr);
}
public PNPResults(
Transform3d best,
Transform3d alt,
double ambiguity,
double bestReprojErr,
double altReprojErr) {
this.isPresent = true;
this.best = best;
this.alt = alt;
this.ambiguity = ambiguity;
this.bestReprojErr = bestReprojErr;
this.altReprojErr = altReprojErr;
}
}

233
photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java
View File

@@ -1,233 +0,0 @@
/*
* MIT License
*
* Copyright (c) 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.estimation;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.util.List;
import java.util.stream.Collectors;
/**
* Represents a transformation that first rotates a pose around the origin, and then translates it.
*/
public class RotTrlTransform3d {
private final Translation3d trl;
private final Rotation3d rot;
// TODO: removal awaiting wpilib Rotation3d performance improvements
private double m_w;
private double m_x;
private double m_y;
private double m_z;
/**
* A rotation-translation transformation.
*
* <p>Applying this RotTrlTransform3d to poses will preserve their current origin-to-pose
* transform as if the origin was transformed by these components instead.
*
* @param rot The rotation component
* @param trl The translation component
*/
public RotTrlTransform3d(Rotation3d rot, Translation3d trl) {
this.rot = rot;
var quat = rot.getQuaternion();
m_w = quat.getW();
m_x = quat.getX();
m_y = quat.getY();
m_z = quat.getZ();
this.trl = trl;
}
public RotTrlTransform3d(Pose3d initial, Pose3d last) {
// this.rot = last.getRotation().minus(initial.getRotation());
// this.trl = last.getTranslation().minus(initial.getTranslation().rotateBy(rot));
var quat = initial.getRotation().getQuaternion();
m_w = quat.getW();
m_x = quat.getX();
m_y = quat.getY();
m_z = quat.getZ();
this.rot = invrotate(last.getRotation());
this.trl = last.getTranslation().minus(rotate(initial.getTranslation()));
}
/**
* Creates a rotation-translation transformation from a Transform3d.
*
* <p>Applying this RotTrlTransform3d to poses will preserve their current origin-to-pose
* transform as if the origin was transformed by trf instead.
*
* @param trf The origin transformation
*/
public RotTrlTransform3d(Transform3d trf) {
this(trf.getRotation(), trf.getTranslation());
}
public RotTrlTransform3d() {
this(new Rotation3d(), new Translation3d());
}
private Translation3d rotate(Translation3d otrl) {
final var p = new Quaternion(0.0, otrl.getX(), otrl.getY(), otrl.getZ());
final var qprime = times(times(p), new Quaternion(m_w, -m_x, -m_y, -m_z));
return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
}
private Translation3d invrotate(Translation3d otrl) {
m_x = -m_x;
m_y = -m_y;
m_z = -m_z;
var result = rotate(otrl);
m_x = -m_x;
m_y = -m_y;
m_z = -m_z;
return result;
}
private Rotation3d rotate(Rotation3d orot) {
return new Rotation3d(times(orot.getQuaternion()));
}
private Rotation3d invrotate(Rotation3d orot) {
m_x = -m_x;
m_y = -m_y;
m_z = -m_z;
var result = rotate(orot);
m_x = -m_x;
m_y = -m_y;
m_z = -m_z;
return result;
}
/**
* The rotation-translation transformation that makes poses in the world consider this pose as the
* new origin, or change the basis to this pose.
*
* @param pose The new origin
*/
public static RotTrlTransform3d makeRelativeTo(Pose3d pose) {
return new RotTrlTransform3d(pose.getRotation(), pose.getTranslation()).inverse();
}
/** The inverse of this transformation. Applying the inverse will "undo" this transformation. */
public RotTrlTransform3d inverse() {
// var inverseRot = rot.unaryMinus();
// var inverseTrl = trl.rotateBy(inverseRot).unaryMinus();
// return new RotTrlTransform3d(inverseRot, inverseTrl);
var inverseTrl = invrotate(trl).unaryMinus();
return new RotTrlTransform3d(new Rotation3d(new Quaternion(m_w, -m_x, -m_y, -m_z)), inverseTrl);
}
/** This transformation as a Transform3d (as if of the origin) */
public Transform3d getTransform() {
return new Transform3d(trl, rot);
}
/** The translation component of this transformation */
public Translation3d getTranslation() {
return trl;
}
/** The rotation component of this transformation */
public Rotation3d getRotation() {
return rot;
}
public Translation3d apply(Translation3d trl) {
// return trl.rotateBy(rot).plus(this.trl);
return rotate(trl).plus(this.trl);
}
public List<Translation3d> applyTrls(List<Translation3d> trls) {
return trls.stream().map(this::apply).collect(Collectors.toList());
}
public Rotation3d apply(Rotation3d rot) {
return rotate(rot);
}
public List<Rotation3d> applyRots(List<Rotation3d> rots) {
return rots.stream().map(this::apply).collect(Collectors.toList());
}
public Pose3d apply(Pose3d pose) {
// return new Pose3d(pose.getTranslation().rotateBy(rot).plus(trl),
// pose.getRotation().plus(rot));
return new Pose3d(apply(pose.getTranslation()), apply(pose.getRotation()));
}
public List<Pose3d> applyPoses(List<Pose3d> poses) {
return poses.stream().map(this::apply).collect(Collectors.toList());
}
// TODO: removal awaiting wpilib Rotation3d performance improvements
private Quaternion times(Quaternion other) {
final double o_w = other.getW();
final double o_x = other.getX();
final double o_y = other.getY();
final double o_z = other.getZ();
return times(m_w, m_x, m_y, m_z, o_w, o_x, o_y, o_z);
}
private static Quaternion times(Quaternion a, Quaternion b) {
final double m_w = a.getW();
final double m_x = a.getX();
final double m_y = a.getY();
final double m_z = a.getZ();
final double o_w = b.getW();
final double o_x = b.getX();
final double o_y = b.getY();
final double o_z = b.getZ();
return times(m_w, m_x, m_y, m_z, o_w, o_x, o_y, o_z);
}
private static Quaternion times(
double m_w,
double m_x,
double m_y,
double m_z,
double o_w,
double o_x,
double o_y,
double o_z) {
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
// v₁ x v₂
final double cross_x = m_y * o_z - o_y * m_z;
final double cross_y = o_x * m_z - m_x * o_z;
final double cross_z = m_x * o_y - o_x * m_y;
// v = w₁v₂ + w₂v₁ + v₁ x v₂
final double new_x = o_x * m_w + (m_x * o_w) + cross_x;
final double new_y = o_y * m_w + (m_y * o_w) + cross_y;
final double new_z = o_z * m_w + (m_z * o_w) + cross_z;
return new Quaternion(m_w * o_w - (m_x * o_x + m_y * o_y + m_z * o_z), new_x, new_y, new_z);
}
}

190
photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java
View File

@@ -1,190 +0,0 @@
/*
* MIT License
*
* Copyright (c) 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.estimation;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import java.util.ArrayList;
import java.util.List;
import java.util.stream.Collectors;
/** Describes the 3d model of a target. */
public class TargetModel {
/**
* Translations of this target's vertices relative to its pose. Rectangular and spherical targets
* will have four vertices. See their respective constructors for more info.
*/
public final List<Translation3d> vertices;
public final boolean isPlanar;
public final boolean isSpherical;
public static final TargetModel kTag16h5 =
new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6));
/**
* Creates a rectangular, planar target model given the width and height. The model has four
* vertices:
*
* <ul>
* <li>Point 0: [0, -width/2, -height/2]
* <li>Point 1: [0, width/2, -height/2]
* <li>Point 2: [0, width/2, height/2]
* <li>Point 3: [0, -width/2, height/2]
* </ul>
*/
public TargetModel(double widthMeters, double heightMeters) {
this.vertices =
List.of(
// this order is relevant for AprilTag compatibility
new Translation3d(0, -widthMeters / 2.0, -heightMeters / 2.0),
new Translation3d(0, widthMeters / 2.0, -heightMeters / 2.0),
new Translation3d(0, widthMeters / 2.0, heightMeters / 2.0),
new Translation3d(0, -widthMeters / 2.0, heightMeters / 2.0));
this.isPlanar = true;
this.isSpherical = false;
}
/**
* Creates a cuboid target model given the length, width, height. The model has eight vertices:
*
* <ul>
* <li>Point 0: [length/2, -width/2, -height/2]
* <li>Point 1: [length/2, width/2, -height/2]
* <li>Point 2: [length/2, width/2, height/2]
* <li>Point 3: [length/2, -width/2, height/2]
* <li>Point 4: [-length/2, -width/2, height/2]
* <li>Point 5: [-length/2, width/2, height/2]
* <li>Point 6: [-length/2, width/2, -height/2]
* <li>Point 7: [-length/2, -width/2, -height/2]
* </ul>
*/
public TargetModel(double lengthMeters, double widthMeters, double heightMeters) {
this(
List.of(
new Translation3d(lengthMeters / 2.0, -widthMeters / 2.0, -heightMeters / 2.0),
new Translation3d(lengthMeters / 2.0, widthMeters / 2.0, -heightMeters / 2.0),
new Translation3d(lengthMeters / 2.0, widthMeters / 2.0, heightMeters / 2.0),
new Translation3d(lengthMeters / 2.0, -widthMeters / 2.0, heightMeters / 2.0),
new Translation3d(-lengthMeters / 2.0, -widthMeters / 2.0, heightMeters / 2.0),
new Translation3d(-lengthMeters / 2.0, widthMeters / 2.0, heightMeters / 2.0),
new Translation3d(-lengthMeters / 2.0, widthMeters / 2.0, -heightMeters / 2.0),
new Translation3d(-lengthMeters / 2.0, -widthMeters / 2.0, -heightMeters / 2.0)));
}
/**
* Creates a spherical target model which has similar dimensions regardless of its rotation. This
* model has four vertices:
*
* <ul>
* <li>Point 0: [0, -radius, 0]
* <li>Point 1: [0, 0, -radius]
* <li>Point 2: [0, radius, 0]
* <li>Point 3: [0, 0, radius]
* </ul>
*
* <i>Q: Why these vertices?</i> A: This target should be oriented to the camera every frame, much
* like a sprite/decal, and these vertices represent the ellipse vertices (maxima). These vertices
* are used for drawing the image of this sphere, but do not match the corners that will be
* published by photonvision.
*/
public TargetModel(double diameterMeters) {
double radius = diameterMeters / 2.0;
this.vertices =
List.of(
new Translation3d(0, -radius, 0),
new Translation3d(0, 0, -radius),
new Translation3d(0, radius, 0),
new Translation3d(0, 0, radius));
this.isPlanar = false;
this.isSpherical = true;
}
/**
* Creates a target model from arbitrary 3d vertices. Automatically determines if the given
* vertices are planar(x == 0). More than 2 vertices must be given. If this is a planar model, the
* vertices should define a non-intersecting contour.
*
* @param vertices Translations representing the vertices of this target model relative to its
* pose.
*/
public TargetModel(List<Translation3d> vertices) {
this.isSpherical = false;
if (vertices == null || vertices.size() <= 2) {
vertices = new ArrayList<>();
this.isPlanar = false;
} else {
boolean cornersPlanar = true;
for (Translation3d corner : vertices) {
if (corner.getX() != 0) cornersPlanar = false;
}
this.isPlanar = cornersPlanar;
}
this.vertices = vertices;
}
/**
* This target's vertices offset from its field pose.
*
* <p>Note: If this target is spherical, use {@link #getOrientedPose(Translation3d,
* Translation3d)} with this method.
*/
public List<Translation3d> getFieldVertices(Pose3d targetPose) {
var basisChange = new RotTrlTransform3d(targetPose.getRotation(), targetPose.getTranslation());
return vertices.stream().map(t -> basisChange.apply(t)).collect(Collectors.toList());
}
/**
* Returns a Pose3d with the given target translation oriented (with its relative x-axis aligned)
* to the camera translation. This is used for spherical targets which should not have their
* projection change regardless of their own rotation.
*
* @param tgtTrl This target's translation
* @param cameraTrl Camera's translation
* @return This target's pose oriented to the camera
*/
public static Pose3d getOrientedPose(Translation3d tgtTrl, Translation3d cameraTrl) {
var relCam = cameraTrl.minus(tgtTrl);
var orientToCam =
new Rotation3d(
0,
new Rotation2d(Math.hypot(relCam.getX(), relCam.getY()), -relCam.getZ()).getRadians(),
new Rotation2d(relCam.getX(), relCam.getY()).getRadians());
return new Pose3d(tgtTrl, orientToCam);
}
@Override
public boolean equals(Object obj) {
if (this == obj) return true;
if (obj instanceof TargetModel) {
var o = (TargetModel) obj;
return vertices.equals(o.vertices) && isPlanar == o.isPlanar && isSpherical == o.isSpherical;
}
return false;
}
}

138
photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java
View File

@@ -1,138 +0,0 @@
/*
* MIT License
*
* Copyright (c) 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.estimation;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.*;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import java.util.stream.Collectors;
import org.opencv.core.Point;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
public class VisionEstimation {
/** Get the visible {@link AprilTag}s which are in the tag layout using the visible tag IDs. */
public static List<AprilTag> getVisibleLayoutTags(
List<PhotonTrackedTarget> visTags, AprilTagFieldLayout tagLayout) {
return visTags.stream()
.map(
t -> {
int id = t.getFiducialId();
var maybePose = tagLayout.getTagPose(id);
return maybePose.map(pose3d -> new AprilTag(id, pose3d)).orElse(null);
})
.filter(Objects::nonNull)
.collect(Collectors.toList());
}
/**
* Performs solvePNP using 3d-2d point correspondences of visible AprilTags to estimate the
* field-to-camera transformation. If only one tag is visible, the result may have an alternate
* solution.
*
* <p><b>Note:</b> The returned transformation is from the field origin to the camera pose!
*
* <p>With only one tag: {@link OpenCVHelp#solvePNP_SQUARE}
*
* <p>With multiple tags: {@link OpenCVHelp#solvePNP_SQPNP}
*
* @param cameraMatrix The camera intrinsics matrix in standard opencv form
* @param distCoeffs The camera distortion matrix in standard opencv form
* @param visTags The visible tags reported by PV. Non-tag targets are automatically excluded.
* @param tagLayout The known tag layout on the field
* @return The transformation that maps the field origin to the camera pose. Ensure the {@link
* PNPResults} are present before utilizing them.
*/
public static PNPResults estimateCamPosePNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<PhotonTrackedTarget> visTags,
AprilTagFieldLayout tagLayout) {
if (tagLayout == null
|| visTags == null
|| tagLayout.getTags().size() == 0
|| visTags.size() == 0) {
return new PNPResults();
}
var corners = new ArrayList<TargetCorner>();
var knownTags = new ArrayList<AprilTag>();
// ensure these are AprilTags in our layout
for (var tgt : visTags) {
int id = tgt.getFiducialId();
tagLayout
.getTagPose(id)
.ifPresent(
pose -> {
knownTags.add(new AprilTag(id, pose));
corners.addAll(tgt.getDetectedCorners());
});
}
if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
return new PNPResults();
}
Point[] points = OpenCVHelp.cornersToPoints(corners);
// single-tag pnp
if (knownTags.size() == 1) {
var camToTag =
OpenCVHelp.solvePNP_SQUARE(
cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, points);
if (!camToTag.isPresent) return new PNPResults();
var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse());
var altPose = new Pose3d();
if (camToTag.ambiguity != 0)
altPose = knownTags.get(0).pose.transformBy(camToTag.alt.inverse());
var o = new Pose3d();
return new PNPResults(
new Transform3d(o, bestPose),
new Transform3d(o, altPose),
camToTag.ambiguity,
camToTag.bestReprojErr,
camToTag.altReprojErr);
}
// multi-tag pnp
else {
var objectTrls = new ArrayList<Translation3d>();
for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.getFieldVertices(tag.pose));
var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points);
if (!camToOrigin.isPresent) return new PNPResults();
return new PNPResults(
camToOrigin.best.inverse(),
camToOrigin.alt.inverse(),
camToOrigin.ambiguity,
camToOrigin.bestReprojErr,
camToOrigin.altReprojErr);
}
}
}

2
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View File

@@ -50,9 +50,9 @@ import org.photonvision.PhotonTargetSortMode;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.estimation.PNPResults;
import org.photonvision.estimation.RotTrlTransform3d;
import org.photonvision.estimation.TargetModel;
import org.photonvision.targeting.PNPResults;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;

4
photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java
View File

@@ -35,6 +35,7 @@ import org.photonvision.PhotonCamera;
import org.photonvision.PhotonTargetSortMode;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
@@ -140,7 +141,8 @@ public class SimPhotonCamera {
targetList.sort(sortMode.getComparator());
}
PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, targetList);
PhotonPipelineResult newResult =
new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResults());
var newPacket = new Packet(newResult.getPacketSize());
newResult.populatePacket(newPacket);
ts.rawBytesEntry.set(newPacket.getData());

113
photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp Normal file
View File

@@ -0,0 +1,113 @@
/*
* MIT License
*
* Copyright (c) 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.
*/
#include "photonlib/MultiTargetPNPResult.h"
namespace photonlib {
Packet& operator<<(Packet& packet, const MultiTargetPnpResult& target) {
packet << target.result;
size_t i;
for (i = 0; i < target.fiducialIdsUsed.capacity(); i++) {
if (i < target.fiducialIdsUsed.size()) {
packet << static_cast<int16_t>(target.fiducialIdsUsed[i]);
} else {
packet << static_cast<int16_t>(-1);
}
}
return packet;
}
Packet& operator>>(Packet& packet, MultiTargetPnpResult& target) {
packet >> target.result;
target.fiducialIdsUsed.clear();
for (size_t i = 0; i < target.fiducialIdsUsed.capacity(); i++) {
int16_t id = 0;
packet >> id;
if (id > -1) {
target.fiducialIdsUsed.push_back(id);
}
}
return packet;
}
// Encode a transform3d
Packet& operator<<(Packet& packet, const frc::Transform3d& transform) {
packet << transform.Translation().X().value()
<< transform.Translation().Y().value()
<< transform.Translation().Z().value()
<< transform.Rotation().GetQuaternion().W()
<< transform.Rotation().GetQuaternion().X()
<< transform.Rotation().GetQuaternion().Y()
<< transform.Rotation().GetQuaternion().Z();
return packet;
}
// Decode a transform3d
Packet& operator>>(Packet& packet, frc::Transform3d& transform) {
frc::Transform3d ret;
// We use these for best and alt transforms below
double x = 0;
double y = 0;
double z = 0;
double w = 0;
// decode and unitify translation
packet >> x >> y >> z;
const auto translation = frc::Translation3d(
units::meter_t(x), units::meter_t(y), units::meter_t(z));
// decode and add units to rotation
packet >> w >> x >> y >> z;
const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
transform = frc::Transform3d(translation, rotation);
return packet;
}
Packet& operator<<(Packet& packet, PNPResults const& result) {
packet << result.isValid << result.best << result.alt
<< result.bestReprojectionErr << result.altReprojectionErr
<< result.ambiguity;
return packet;
}
Packet& operator>>(Packet& packet, PNPResults& result) {
packet >> result.isValid >> result.best >> result.alt >>
result.bestReprojectionErr >> result.altReprojectionErr >>
result.ambiguity;
return packet;
}
} // namespace photonlib

6
photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp
View File

@@ -40,7 +40,7 @@ bool PhotonPipelineResult::operator!=(const PhotonPipelineResult& other) const {
Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
// Encode latency and number of targets.
packet << result.latency.value() * 1000
packet << result.latency.value() * 1000 << result.m_pnpResults
<< static_cast<int8_t>(result.targets.size());
// Encode the information of each target.
@@ -52,9 +52,9 @@ Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
Packet& operator>>(Packet& packet, PhotonPipelineResult& result) {
// Decode latency, existence of targets, and number of targets.
int8_t targetCount = 0;
double latencyMillis = 0;
packet >> latencyMillis >> targetCount;
int8_t targetCount = 0;
packet >> latencyMillis >> result.m_pnpResults >> targetCount;
result.latency = units::second_t(latencyMillis / 1000.0);
result.targets.clear();

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

@@ -82,7 +82,8 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
poseCacheTimestamp(-1_s) {}
void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
if (strategy == MULTI_TAG_PNP) {
if (strategy == MULTI_TAG_PNP_ON_COPROCESSOR ||
strategy == MULTI_TAG_PNP_ON_RIO) {
FRC_ReportError(
frc::warn::Warning,
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity",
@@ -162,8 +163,12 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
case AVERAGE_BEST_TARGETS:
ret = AverageBestTargetsStrategy(result);
break;
case ::photonlib::MULTI_TAG_PNP:
ret = MultiTagPnpStrategy(result, cameraMatrixData, cameraDistCoeffs);
case MULTI_TAG_PNP_ON_COPROCESSOR:
ret =
MultiTagOnCoprocStrategy(result, cameraMatrixData, cameraDistCoeffs);
break;
case MULTI_TAG_PNP_ON_RIO:
ret = MultiTagOnRioStrategy(result, cameraMatrixData, cameraDistCoeffs);
break;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
@@ -205,7 +210,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
fiducialPose.value()
.TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets()};
result.GetTimestamp(), result.GetTargets(), LOWEST_AMBIGUITY};
}
std::optional<EstimatedRobotPose>
@@ -241,14 +246,14 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets()};
result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets()};
result.GetTimestamp(), result.GetTargets(), CLOSEST_TO_CAMERA_HEIGHT};
}
}
@@ -299,7 +304,8 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
}
}
return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets()};
return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets(),
CLOSEST_TO_REFERENCE_POSE};
}
std::optional<std::array<cv::Point3d, 4>> detail::CalcTagCorners(
@@ -351,7 +357,24 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
Rotation3d(rv));
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs) {
if (result.MultiTagResult().result.isValid) {
const auto field2camera = result.MultiTagResult().result.best;
const auto fieldToRobot =
frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
return photonlib::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
result.GetTargets(),
MULTI_TAG_PNP_ON_COPROCESSOR);
}
return Update(result, std::nullopt, std::nullopt,
this->multiTagFallbackStrategy);
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs) {
using namespace frc;
@@ -404,7 +427,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
return photonlib::EstimatedRobotPose(
pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(),
result.GetTargets());
result.GetTargets(), MULTI_TAG_PNP_ON_RIO);
}
std::optional<EstimatedRobotPose>
@@ -430,7 +453,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
return EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp(), result.GetTargets()};
result.GetTimestamp(), result.GetTargets(), AVERAGE_BEST_TARGETS};
}
totalAmbiguity += 1. / target.GetPoseAmbiguity();
@@ -450,6 +473,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
}
return EstimatedRobotPose{frc::Pose3d(transform, rotation),
result.GetTimestamp(), result.GetTargets()};
result.GetTimestamp(), result.GetTargets(),
AVERAGE_BEST_TARGETS};
}
} // namespace photonlib

282
photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp
View File

@@ -1,282 +0,0 @@
/*
* MIT License
*
* Copyright (c) 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.
*/
#include "photonlib/RobotPoseEstimator.h"
#include <iostream>
#include <limits>
#include <map>
#include <span>
#include <string>
#include <utility>
#include <vector>
#include <frc/Errors.h>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <units/time.h>
#include "photonlib/PhotonCamera.h"
#include "photonlib/PhotonPipelineResult.h"
#include "photonlib/PhotonTrackedTarget.h"
namespace photonlib {
RobotPoseEstimator::RobotPoseEstimator(
std::shared_ptr<frc::AprilTagFieldLayout> tags, PoseStrategy strat,
std::vector<std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>>
cams)
: aprilTags(tags),
strategy(strat),
cameras(std::move(cams)),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()) {}
std::pair<frc::Pose3d, units::second_t> RobotPoseEstimator::Update() {
if (cameras.empty()) {
return std::make_pair(lastPose, units::second_t(0));
}
std::pair<frc::Pose3d, units::second_t> pair;
switch (strategy) {
case LOWEST_AMBIGUITY:
pair = LowestAmbiguityStrategy();
lastPose = pair.first;
return pair;
case CLOSEST_TO_CAMERA_HEIGHT:
pair = ClosestToCameraHeightStrategy();
lastPose = pair.first;
return pair;
case CLOSEST_TO_REFERENCE_POSE:
pair = ClosestToReferencePoseStrategy();
lastPose = pair.first;
return pair;
case CLOSEST_TO_LAST_POSE:
SetReferencePose(lastPose);
pair = ClosestToReferencePoseStrategy();
lastPose = pair.first;
return pair;
case AVERAGE_BEST_TARGETS:
pair = AverageBestTargetsStrategy();
lastPose = pair.first;
return pair;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
"");
}
return std::make_pair(lastPose, units::second_t(0));
}
std::pair<frc::Pose3d, units::second_t>
RobotPoseEstimator::LowestAmbiguityStrategy() {
int lowestAI = -1;
int lowestAJ = -1;
double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
std::span<const PhotonTrackedTarget> targets =
p.first->GetLatestResult().GetTargets();
for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
if (targets[j].GetPoseAmbiguity() < lowestAmbiguityScore) {
lowestAI = i;
lowestAJ = j;
lowestAmbiguityScore = targets[j].GetPoseAmbiguity();
}
}
}
if (lowestAI == -1 || lowestAJ == -1) {
return std::make_pair(lastPose, units::second_t(0));
}
PhotonTrackedTarget bestTarget =
cameras[lowestAI].first->GetLatestResult().GetTargets()[lowestAJ];
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::make_pair(lastPose, units::second_t(0));
}
return std::make_pair(
fiducialPose.value()
.TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(cameras[lowestAI].second.Inverse()),
cameras[lowestAI].first->GetLatestResult().GetTimestamp());
}
std::pair<frc::Pose3d, units::second_t>
RobotPoseEstimator::ClosestToCameraHeightStrategy() {
units::meter_t smallestHeightDifference =
units::meter_t(std::numeric_limits<double>::infinity());
units::second_t stateTimestamp = units::second_t(0);
frc::Pose3d pose = lastPose;
for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
std::span<const PhotonTrackedTarget> targets =
p.first->GetLatestResult().GetTargets();
for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
PhotonTrackedTarget target = targets[j];
std::optional<frc::Pose3d> fiducialPose =
aprilTags->GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
units::meter_t alternativeDifference = units::math::abs(
p.second.Z() -
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.Z());
units::meter_t bestDifference = units::math::abs(
p.second.Z() -
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z());
if (alternativeDifference < smallestHeightDifference) {
smallestHeightDifference = alternativeDifference;
pose = targetPose.TransformBy(
target.GetAlternateCameraToTarget().Inverse());
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
}
}
return std::make_pair(pose, stateTimestamp);
}
std::pair<frc::Pose3d, units::second_t>
RobotPoseEstimator::ClosestToReferencePoseStrategy() {
units::meter_t smallestDifference =
units::meter_t(std::numeric_limits<double>::infinity());
units::second_t stateTimestamp = units::second_t(0);
frc::Pose3d pose = lastPose;
for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
std::span<const PhotonTrackedTarget> targets =
p.first->GetLatestResult().GetTargets();
for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
PhotonTrackedTarget target = targets[j];
std::optional<frc::Pose3d> fiducialPose =
aprilTags->GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
units::meter_t alternativeDifference =
units::math::abs(referencePose.Translation().Distance(
targetPose
.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.Translation()));
units::meter_t bestDifference =
units::math::abs(referencePose.Translation().Distance(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.Translation()));
if (alternativeDifference < smallestDifference) {
smallestDifference = alternativeDifference;
pose = targetPose.TransformBy(
target.GetAlternateCameraToTarget().Inverse());
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
if (bestDifference < smallestDifference) {
smallestDifference = bestDifference;
pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
}
}
return std::make_pair(pose, stateTimestamp);
}
std::pair<frc::Pose3d, units::second_t>
RobotPoseEstimator::AverageBestTargetsStrategy() {
std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
tempPoses;
double totalAmbiguity = 0;
units::second_t timstampSum = units::second_t(0);
for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
std::span<const PhotonTrackedTarget> targets =
p.first->GetLatestResult().GetTargets();
timstampSum += p.first->GetLatestResult().GetTimestamp();
for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
PhotonTrackedTarget target = targets[j];
std::optional<frc::Pose3d> fiducialPose =
aprilTags->GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
if (target.GetPoseAmbiguity() == 0) {
FRC_ReportError(frc::warn::Warning,
"Pose ambiguity of zero exists, using that instead!",
"");
return std::make_pair(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
p.first->GetLatestResult().GetLatency() / 1000.);
}
totalAmbiguity += 1. / target.GetPoseAmbiguity();
tempPoses.push_back(std::make_pair(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
std::make_pair(target.GetPoseAmbiguity(),
p.first->GetLatestResult().GetTimestamp())));
}
}
frc::Translation3d transform = frc::Translation3d();
frc::Rotation3d rotation = frc::Rotation3d();
for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
tempPoses) {
double weight = (1. / pair.second.first) / totalAmbiguity;
transform = transform + pair.first.Translation() * weight;
rotation = rotation + pair.first.Rotation() * weight;
}
return std::make_pair(frc::Pose3d(transform, rotation),
timstampSum / cameras.size());
}
} // namespace photonlib

61
photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h Normal file
View File

@@ -0,0 +1,61 @@
/*
* MIT License
*
* Copyright (c) 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.
*/
#pragma once
#include <frc/geometry/Transform3d.h>
#include <wpi/SmallVector.h>
#include "photonlib/Packet.h"
namespace photonlib {
class PNPResults {
public:
// This could be wrapped in an std::optional, but chose to do it this way to
// mirror Java
bool isValid;
frc::Transform3d best;
double bestReprojectionErr;
frc::Transform3d alt;
double altReprojectionErr;
double ambiguity;
friend Packet& operator<<(Packet& packet, const PNPResults& result);
friend Packet& operator>>(Packet& packet, PNPResults& result);
};
class MultiTargetPnpResult {
public:
PNPResults result;
wpi::SmallVector<int16_t, 32> fiducialIdsUsed;
friend Packet& operator<<(Packet& packet, const MultiTargetPnpResult& result);
friend Packet& operator>>(Packet& packet, MultiTargetPnpResult& result);
};
} // namespace photonlib

9
photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h
View File

@@ -31,6 +31,7 @@
#include <units/time.h>
#include <wpi/SmallVector.h>
#include "photonlib/MultiTargetPNPResult.h"
#include "photonlib/Packet.h"
#include "photonlib/PhotonTrackedTarget.h"
@@ -87,6 +88,13 @@ class PhotonPipelineResult {
*/
units::second_t GetTimestamp() const { return timestamp; }
/**
* Return the latest mulit-target result, as calculated on your coprocessor.
* Be sure to check getMultiTagResult().estimatedPose.isValid before using the
* pose estimate!
*/
const MultiTargetPnpResult& MultiTagResult() const { return m_pnpResults; }
/**
* Sets the timestamp in seconds
* @param timestamp The timestamp in seconds
@@ -119,6 +127,7 @@ class PhotonPipelineResult {
units::second_t latency = 0_s;
units::second_t timestamp = -1_s;
wpi::SmallVector<PhotonTrackedTarget, 10> targets;
MultiTargetPnpResult m_pnpResults;
inline static bool HAS_WARNED = false;
};
} // namespace photonlib

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

@@ -44,7 +44,8 @@ enum PoseStrategy {
CLOSEST_TO_REFERENCE_POSE,
CLOSEST_TO_LAST_POSE,
AVERAGE_BEST_TARGETS,
MULTI_TAG_PNP
MULTI_TAG_PNP_ON_COPROCESSOR,
MULTI_TAG_PNP_ON_RIO,
};
struct EstimatedRobotPose {
@@ -57,11 +58,16 @@ struct EstimatedRobotPose {
/** A list of the targets used to compute this pose */
wpi::SmallVector<PhotonTrackedTarget, 10> targetsUsed;
/** The strategy actually used to produce this pose */
PoseStrategy strategy;
EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_,
std::span<const PhotonTrackedTarget> targets)
std::span<const PhotonTrackedTarget> targets,
PoseStrategy strategy_)
: estimatedPose(pose_),
timestamp(time_),
targetsUsed(targets.data(), targets.data() + targets.size()) {}
targetsUsed(targets.data(), targets.data() + targets.size()),
strategy(strategy_) {}
};
/**
@@ -260,14 +266,23 @@ class PhotonPoseEstimator {
std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
PhotonPipelineResult result);
/**
* Return the pose calculated by combining all tags into one on coprocessor
*
* @return the estimated position of the robot in the FCS
*/
std::optional<EstimatedRobotPose> MultiTagOnCoprocStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs);
/**
* Return the pose calculation using all targets in view in the same PNP()
calculation
*
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> MultiTagPnpStrategy(
std::optional<EstimatedRobotPose> MultiTagOnRioStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs);

187
photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h
View File

@@ -1,187 +0,0 @@
/*
* MIT License
*
* Copyright (c) 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.
*/
#pragma once
#include <map>
#include <memory>
#include <utility>
#include <vector>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Transform3d.h>
#include "photonlib/PhotonCamera.h"
namespace frc {
class AprilTagFieldLayout;
} // namespace frc
namespace photonlib {
enum PoseStrategy : int {
LOWEST_AMBIGUITY,
CLOSEST_TO_CAMERA_HEIGHT,
CLOSEST_TO_REFERENCE_POSE,
CLOSEST_TO_LAST_POSE,
AVERAGE_BEST_TARGETS
};
/**
* The RobotPoseEstimator class filters or combines readings from all the
* fiducials visible at a given timestamp on the field to produce a single robot
* in field pose, using the strategy set below. Example usage can be found in
* our apriltagExample example project.
*/
class RobotPoseEstimator {
public:
using map_value_type =
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
using size_type = std::vector<map_value_type>::size_type;
/**
* Create a new RobotPoseEstimator.
*
* <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
* <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
* at (1.0,2.0,3.0) </code> }
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param cameras An ArrayList of Pairs of PhotonCameras and their respective
* Transform3ds from the center of the robot to the cameras.
*/
explicit RobotPoseEstimator(
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
PoseStrategy strategy, std::vector<map_value_type> cameras);
/**
* Get the AprilTagFieldLayout being used by the PositionEstimator.
*
* @return the AprilTagFieldLayout
*/
std::shared_ptr<frc::AprilTagFieldLayout> getFieldLayout() const {
return aprilTags;
}
/**
* Set the cameras to be used by the PoseEstimator.
*
* @param cameras cameras to set.
*/
inline void SetCameras(
const std::vector<std::pair<std::shared_ptr<PhotonCamera>,
frc::Transform3d>>& cameras) {
this->cameras = cameras;
}
/**
* Get the Position Estimation Strategy being used by the Position Estimator.
*
* @return the strategy
*/
PoseStrategy GetPoseStrategy() const { return strategy; }
/**
* Set the Position Estimation Strategy used by the Position Estimator.
*
* @param strategy the strategy to set
*/
inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
/**
* Return the reference position that is being used by the estimator.
*
* @return the referencePose
*/
frc::Pose3d GetReferencePose() const { return referencePose; }
/**
* Update the stored reference pose for use when using the
* CLOSEST_TO_REFERENCE_POSE strategy.
*
* @param referencePose the referencePose to set
*/
inline void SetReferencePose(frc::Pose3d referencePose) {
this->referencePose = referencePose;
}
/**
* Update the stored last pose. Useful for setting the initial estimate when
* using the CLOSEST_TO_LAST_POSE strategy.
*
* @param lastPose the lastPose to set
*/
inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
std::pair<frc::Pose3d, units::second_t> Update();
private:
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags;
PoseStrategy strategy;
std::vector<map_value_type> cameras;
frc::Pose3d lastPose;
frc::Pose3d referencePose;
/**
* Return the estimated position of the robot with the lowest position
* ambiguity from a List of pipeline results.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::pair<frc::Pose3d, units::second_t> LowestAmbiguityStrategy();
/**
* Return the estimated position of the robot using the target with the lowest
* delta height difference between the estimated and actual height of the
* camera.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::pair<frc::Pose3d, units::second_t> ClosestToCameraHeightStrategy();
/**
* Return the estimated position of the robot using the target with the lowest
* delta in the vector magnitude between it and the reference pose.
*
* @param referencePose reference pose to check vector magnitude difference
* against.
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::pair<frc::Pose3d, units::second_t> ClosestToReferencePoseStrategy();
/**
* Return the average of the best target poses using ambiguity as weight.
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this
* estimation.
*/
std::pair<frc::Pose3d, units::second_t> AverageBestTargetsStrategy();
};
} // namespace photonlib

60
photon-lib/src/test/java/org/photonvision/PacketTest.java
View File

@@ -30,6 +30,8 @@ import java.util.List;
import org.junit.jupiter.api.Assertions;
import org.junit.jupiter.api.Test;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.PNPResults;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
@@ -127,4 +129,62 @@ class PacketTest {
Assertions.assertEquals(result2, b2);
}
@Test
public void testMultiTargetSerde() {
var result =
new PhotonPipelineResult(
2,
List.of(
new PhotonTrackedTarget(
3.0,
-4.0,
9.0,
4.0,
2,
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.25,
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))),
new PhotonTrackedTarget(
3.0,
-4.0,
9.1,
6.7,
3,
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
0.25,
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)))),
new MultiTargetPNPResults(
new PNPResults(
new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1),
List.of(1, 2, 3)));
Packet packet = new Packet(result.getPacketSize());
result.populatePacket(packet);
var result_deserialized = new PhotonPipelineResult();
result_deserialized.createFromPacket(packet);
Assertions.assertEquals(result, result_deserialized);
}
}

5
photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java
View File

@@ -84,7 +84,10 @@ public class ApriltagWorkbenchTest {
var pe =
new PhotonPoseEstimator(
tagLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, cam, robotToCamera);
tagLayout,
PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
cam,
robotToCamera);
var field = new Field2d();
SmartDashboard.putData(field);

442
photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp
View File

@@ -1,442 +0,0 @@
/*
* MIT License
*
* Copyright (c) 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.
*/
#include <map>
#include <utility>
#include <vector>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <units/angle.h>
#include <units/length.h>
#include <wpi/SmallVector.h>
#include "gtest/gtest.h"
#include "photonlib/PhotonCamera.h"
#include "photonlib/PhotonPipelineResult.h"
#include "photonlib/PhotonTrackedTarget.h"
#include "photonlib/RobotPoseEstimator.h"
static wpi::SmallVector<std::pair<double, double>, 4> corners{
std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
static std::vector<std::pair<double, double>> detectedCorners{
std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())}};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
cameras;
std::shared_ptr<photonlib::PhotonCamera> cameraOne =
std::make_shared<photonlib::PhotonCamera>("test");
std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
std::make_shared<photonlib::PhotonCamera>("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
cameraOne->testResult.SetTimestamp(units::second_t(11));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
cameraTwo->testResult.SetTimestamp(units::second_t(units::second_t(16)));
cameras.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
cameras.push_back(std::make_pair(
cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
photonlib::RobotPoseEstimator estimator(aprilTags,
photonlib::LOWEST_AMBIGUITY, cameras);
std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
estimator.Update();
frc::Pose3d pose = estimatedPose.first;
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
}
TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())},
};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
cameras;
std::shared_ptr<photonlib::PhotonCamera> cameraOne =
std::make_shared<photonlib::PhotonCamera>("test");
std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
std::make_shared<photonlib::PhotonCamera>("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
cameraOne->testResult.SetTimestamp(units::second_t(4));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
cameraOne->testResult.SetTimestamp(units::second_t(12));
cameras.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 4_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
cameras.push_back(std::make_pair(
cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
photonlib::RobotPoseEstimator estimator(
aprilTags, photonlib::CLOSEST_TO_CAMERA_HEIGHT, cameras);
std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
estimator.Update();
frc::Pose3d pose = estimatedPose.first;
EXPECT_NEAR(12, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
EXPECT_NEAR(4, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(4, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(4, units::unit_cast<double>(pose.Z()), .01);
}
TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())},
};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
cameras;
std::shared_ptr<photonlib::PhotonCamera> cameraOne =
std::make_shared<photonlib::PhotonCamera>("test");
std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
std::make_shared<photonlib::PhotonCamera>("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
cameraOne->testResult.SetTimestamp(units::second_t(4));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
cameraTwo->testResult.SetTimestamp(units::second_t(17));
cameras.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
cameras.push_back(std::make_pair(
cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
photonlib::RobotPoseEstimator estimator(
aprilTags, photonlib::CLOSEST_TO_REFERENCE_POSE, cameras);
estimator.SetReferencePose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
estimator.Update();
frc::Pose3d pose = estimatedPose.first;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(.9, units::unit_cast<double>(pose.Z()), .01);
}
TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())}};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
cameras;
std::shared_ptr<photonlib::PhotonCamera> cameraOne =
std::make_shared<photonlib::PhotonCamera>("test");
std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
std::make_shared<photonlib::PhotonCamera>("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
cameras.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
cameras.push_back(std::make_pair(
cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
photonlib::RobotPoseEstimator estimator(
aprilTags, photonlib::CLOSEST_TO_LAST_POSE, cameras);
estimator.SetLastPose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
estimator.Update();
frc::Pose3d pose = estimatedPose.first;
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targetsThree{
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 0,
frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne->testResult = {2_s, targetsThree};
cameraOne->testResult.SetTimestamp(units::second_t(7));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsFour{
photonlib::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraTwo->testResult = {4_s, targetsFour};
cameraTwo->testResult.SetTimestamp(units::second_t(13));
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
camerasUpdated;
camerasUpdated.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
camerasUpdated.push_back(std::make_pair(
cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
estimator.SetCameras(camerasUpdated);
estimatedPose = estimator.Update();
pose = estimatedPose.first;
EXPECT_NEAR(7.0, units::unit_cast<double>(estimatedPose.second) / 1000.0,
.01);
EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
}
TEST(RobotPoseEstimatorTest, AverageBestPoses) {
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())}};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
cameras;
std::shared_ptr<photonlib::PhotonCamera> cameraOne =
std::make_shared<photonlib::PhotonCamera>("test");
std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
std::make_shared<photonlib::PhotonCamera>("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
cameraOne->testResult.SetTimestamp(units::second_t(10));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
cameraTwo->testResult.SetTimestamp(units::second_t(20));
cameras.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
cameras.push_back(std::make_pair(
cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad))));
photonlib::RobotPoseEstimator estimator(
aprilTags, photonlib::AVERAGE_BEST_TARGETS, cameras);
std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
estimator.Update();
frc::Pose3d pose = estimatedPose.first;
EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.second) / 1000.0,
.01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
}