Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-21 01:01: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());