Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-05 03:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Multi-tag pnp in robot code (#787)

Browse Source 
---------

Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com>
Co-authored-by: Joseph Farkas <16584585+MrRedness@users.noreply.github.com>
This commit is contained in:
Matt
2023-02-13 17:57:01 -05:00
committed by GitHub
parent a2dfe48679
commit 5b86360b9b
33 changed files with 1785 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@@ -154,4 +154,4 @@ photon-server/src/main/resources/nativelibraries/apriltag/*
photonlib-java-examples/*/vendordeps/*
photonlib-cpp-examples/*/vendordeps/*
networktables.json
*/networktables.json

9
photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View File

@@ -180,6 +180,15 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
ts.bestTargetPosY.set(0);
}
var fsp = result.inputAndOutputFrame.frameStaticProperties;
if (fsp.cameraCalibration != null) {
ts.cameraIntrinsicsPublisher.accept(fsp.cameraCalibration.getIntrinsicsArr());
ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getExtrinsicsArr());
} else {
ts.cameraIntrinsicsPublisher.accept(new double[] {});
ts.cameraDistortionPublisher.accept(new double[] {});
}
ts.heartbeatPublisher.set(heartbeatCounter++);
// TODO...nt4... is this needed?

18
photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java
View File

@@ -44,6 +44,10 @@ public class CameraCalibrationCoefficients implements Releasable {
@JsonProperty("standardDeviation")
public final double standardDeviation;
@JsonIgnore private final double[] intrinsicsArr = new double[9];
@JsonIgnore private final double[] extrinsicsArr = new double[5];
@JsonCreator
public CameraCalibrationCoefficients(
@JsonProperty("resolution") Size resolution,
@@ -56,6 +60,10 @@ public class CameraCalibrationCoefficients implements Releasable {
this.distCoeffs = distCoeffs;
this.perViewErrors = perViewErrors;
this.standardDeviation = standardDeviation;
// do this once so gets are quick
getCameraIntrinsicsMat().get(0, 0, intrinsicsArr);
getDistCoeffsMat().get(0, 0, extrinsicsArr);
}
@JsonIgnore
@@ -68,6 +76,16 @@ public class CameraCalibrationCoefficients implements Releasable {
return distCoeffs.getAsMatOfDouble();
}
@JsonIgnore
public double[] getIntrinsicsArr() {
return intrinsicsArr;
}
@JsonIgnore
public double[] getExtrinsicsArr() {
return extrinsicsArr;
}
@JsonIgnore
public double[] getPerViewErrors() {
return perViewErrors;

6
photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java
View File

@@ -23,6 +23,7 @@ import java.util.Arrays;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.vision.opencv.Releasable;
public class JsonMat implements Releasable {
@@ -106,4 +107,9 @@ public class JsonMat implements Releasable {
public void release() {
getAsMat().release();
}
public Packet populatePacket(Packet packet) {
packet.encode(this.data);
return packet;
}
}

2
photon-core/src/main/java/org/photonvision/vision/frame/FrameStaticProperties.java
View File

@@ -61,10 +61,12 @@ public class FrameStaticProperties {
imageArea = this.imageWidth * this.imageHeight;
// Todo -- if we have calibration, use it's center point?
centerX = ((double) this.imageWidth / 2) - 0.5;
centerY = ((double) this.imageHeight / 2) - 0.5;
centerPoint = new Point(centerX, centerY);
// TODO if we have calibration use it here instead
// pinhole model calculations
DoubleCouple horizVertViews =
calculateHorizontalVerticalFoV(this.fov, this.imageWidth, this.imageHeight);

16
photon-lib/build.gradle
View File

@@ -22,6 +22,14 @@ targetCompatibility = JavaVersion.VERSION_11
wpilibTools.deps.wpilibVersion = wpilibVersion
println("Buidling for wpilib ${wpilibTools.deps.wpilibVersion}")
// From wpilib shared/config.gradle:
// NativeUtils adds the following OpenCV warning suppression for Linux, but not
// for macOS
// https://github.com/opencv/opencv/issues/20269
nativeUtils.platformConfigs.osxuniversal.cppCompiler.args.add("-Wno-deprecated-anon-enum-enum-conversion")
nativeUtils.platformConfigs.linuxathena.cppCompiler.args.add("-Wno-deprecated-anon-enum-enum-conversion")
// nativeUtils.platformConfigs.linuxx64.cppCompiler.args.add("-Wno-deprecated-anon-enum-enum-conversion")
// Apply Java configuration
dependencies {
implementation project(":photon-targeting")
@@ -34,6 +42,11 @@ dependencies {
implementation wpilibTools.deps.wpilibJava("wpilibj")
implementation wpilibTools.deps.wpilibJava("apriltag")
testImplementation wpilibTools.deps.wpilibJava("cscore")
implementation "edu.wpi.first.thirdparty.frc2023.opencv:opencv-java:$opencvVersion"
implementation "edu.wpi.first.thirdparty.frc2023.opencv:opencv-jni:$opencvVersion:$jniPlatform"
implementation "org.ejml:ejml-simple:0.41"
// Junit
testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2")
testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2")
@@ -66,6 +79,7 @@ model {
}
nativeUtils.useRequiredLibrary(it, "wpilib_shared")
nativeUtils.useRequiredLibrary(it, "apriltag_shared")
nativeUtils.useRequiredLibrary(it, "opencv_shared")
}
}
testSuites {
@@ -82,6 +96,7 @@ model {
nativeUtils.useRequiredLibrary(it, "wpilib_executable_shared")
nativeUtils.useRequiredLibrary(it, "apriltag_shared")
nativeUtils.useRequiredLibrary(it, "googletest_static")
nativeUtils.useRequiredLibrary(it, "opencv_shared")
}
}
}
@@ -147,3 +162,4 @@ testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet")
testNativeConfig.dependencies.add wpilibTools.deps.wpilib("hal")
testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpiutil")
testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpimath")
testNativeConfig.dependencies.add wpilibTools.deps.cscore()

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

@@ -25,6 +25,8 @@
package org.photonvision;
import edu.wpi.first.math.geometry.Pose3d;
import java.util.List;
import org.photonvision.targeting.PhotonTrackedTarget;
/** An estimated pose based on pipeline result */
public class EstimatedRobotPose {
@@ -34,14 +36,19 @@ public class EstimatedRobotPose {
/** The estimated time the frame used to derive the robot pose was taken */
public final double timestampSeconds;
/** A list of the targets used to compute this pose */
public final List<PhotonTrackedTarget> targetsUsed;
/**
* Constructs an EstimatedRobotPose
*
* @param estimatedPose estimated pose
* @param timestampSeconds timestamp of the estimate
*/
public EstimatedRobotPose(Pose3d estimatedPose, double timestampSeconds) {
public EstimatedRobotPose(
Pose3d estimatedPose, double timestampSeconds, List<PhotonTrackedTarget> targetsUsed) {
this.estimatedPose = estimatedPose;
this.timestampSeconds = timestampSeconds;
this.targetsUsed = targetsUsed;
}
}

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

@@ -24,9 +24,14 @@
package org.photonvision;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.numbers.*;
import edu.wpi.first.networktables.BooleanPublisher;
import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.DoubleArrayPublisher;
import edu.wpi.first.networktables.DoubleArraySubscriber;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.IntegerEntry;
import edu.wpi.first.networktables.IntegerPublisher;
@@ -39,6 +44,7 @@ import edu.wpi.first.networktables.RawSubscriber;
import edu.wpi.first.networktables.StringSubscriber;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import java.util.Optional;
import java.util.Set;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.hardware.VisionLEDMode;
@@ -64,6 +70,8 @@ public class PhotonCamera {
IntegerPublisher pipelineIndexRequest, ledModeRequest;
IntegerSubscriber pipelineIndexState, ledModeState;
IntegerSubscriber heartbeatEntry;
private DoubleArraySubscriber cameraIntrinsicsSubscriber;
private DoubleArraySubscriber cameraDistortionSubscriber;
public void close() {
rawBytesEntry.close();
@@ -84,6 +92,8 @@ public class PhotonCamera {
ledModeRequest.close();
ledModeState.close();
pipelineIndexRequest.close();
cameraIntrinsicsSubscriber.close();
cameraDistortionSubscriber.close();
}
private final String path;
@@ -133,6 +143,8 @@ public class PhotonCamera {
ledModeRequest = mainTable.getIntegerTopic("ledModeRequest").publish();
ledModeState = mainTable.getIntegerTopic("ledModeState").subscribe(-1);
versionEntry = mainTable.getStringTopic("version").subscribe("");
cameraIntrinsicsSubscriber = mainTable.getDoubleArrayTopic("cameraIntrinsics").subscribe(null);
cameraDistortionSubscriber = mainTable.getDoubleArrayTopic("cameraDistortion").subscribe(null);
m_topicNameSubscriber =
new MultiSubscriber(
@@ -305,6 +317,20 @@ public class PhotonCamera {
return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC;
}
public Optional<Matrix<N3, N3>> getCameraMatrix() {
var cameraMatrix = cameraIntrinsicsSubscriber.get();
if (cameraMatrix != null && cameraMatrix.length == 9) {
return Optional.of(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(cameraMatrix));
} else return Optional.empty();
}
public Optional<Matrix<N5, N1>> getDistCoeffs() {
var distCoeffs = cameraDistortionSubscriber.get();
if (distCoeffs != null && distCoeffs.length == 5) {
return Optional.of(new MatBuilder<>(Nat.N5(), Nat.N1()).fill(distCoeffs));
} else return Optional.empty();
}
private void verifyVersion() {
if (!VERSION_CHECK_ENABLED) return;

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

@@ -24,6 +24,7 @@
package org.photonvision;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose2d;
@@ -37,8 +38,10 @@ import java.util.HashSet;
import java.util.List;
import java.util.Optional;
import java.util.Set;
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
/**
* The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a
@@ -61,11 +64,15 @@ public class PhotonPoseEstimator {
CLOSEST_TO_LAST_POSE,
/** Choose the Pose with the lowest ambiguity. */
AVERAGE_BEST_TARGETS
AVERAGE_BEST_TARGETS,
/** Use all visible tags to compute a single pose estimate.. */
MULTI_TAG_PNP
}
private AprilTagFieldLayout fieldTags;
private PoseStrategy strategy;
private PoseStrategy primaryStrategy;
private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY;
private final PhotonCamera camera;
private Transform3d robotToCamera;
@@ -93,7 +100,7 @@ public class PhotonPoseEstimator {
PhotonCamera camera,
Transform3d robotToCamera) {
this.fieldTags = fieldTags;
this.strategy = strategy;
this.primaryStrategy = strategy;
this.camera = camera;
this.robotToCamera = robotToCamera;
}
@@ -121,8 +128,8 @@ public class PhotonPoseEstimator {
*
* @return the strategy
*/
public PoseStrategy getStrategy() {
return strategy;
public PoseStrategy getPrimaryStrategy() {
return primaryStrategy;
}
/**
@@ -130,8 +137,23 @@ public class PhotonPoseEstimator {
*
* @param strategy the strategy to set
*/
public void setStrategy(PoseStrategy strategy) {
this.strategy = strategy;
public void setPrimaryStrategy(PoseStrategy strategy) {
this.primaryStrategy = strategy;
}
/**
* Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must
* NOT be MULTI_TAG_PNP
*
* @param strategy the strategy to set
*/
public void setMultiTagFallbackStrategy(PoseStrategy strategy) {
if (strategy == PoseStrategy.MULTI_TAG_PNP) {
DriverStation.reportWarning(
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", null);
strategy = PoseStrategy.LOWEST_AMBIGUITY;
}
this.multiTagFallbackStrategy = strategy;
}
/**
@@ -229,8 +251,13 @@ public class PhotonPoseEstimator {
return Optional.empty();
}
return update(cameraResult, this.primaryStrategy);
}
private Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult, PoseStrategy strat) {
Optional<EstimatedRobotPose> estimatedPose;
switch (strategy) {
switch (strat) {
case LOWEST_AMBIGUITY:
estimatedPose = lowestAmbiguityStrategy(cameraResult);
break;
@@ -245,6 +272,9 @@ public class PhotonPoseEstimator {
case AVERAGE_BEST_TARGETS:
estimatedPose = averageBestTargetsStrategy(cameraResult);
break;
case MULTI_TAG_PNP:
estimatedPose = multiTagPNPStrategy(cameraResult);
break;
default:
DriverStation.reportError(
"[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false);
@@ -258,6 +288,55 @@ public class PhotonPoseEstimator {
return estimatedPose;
}
private Optional<EstimatedRobotPose> multiTagPNPStrategy(PhotonPipelineResult result) {
// Arrays we need declared up front
var visCorners = new ArrayList<TargetCorner>();
var knownVisTags = new ArrayList<AprilTag>();
var fieldToCams = new ArrayList<Pose3d>();
var fieldToCamsAlt = new ArrayList<Pose3d>();
if (result.getTargets().size() < 2) {
// Run fallback strategy instead
update(result, this.multiTagFallbackStrategy);
}
for (var target : result.getTargets()) {
visCorners.addAll(target.getDetectedCorners());
Pose3d tagPose = fieldTags.getTagPose(target.getFiducialId()).get();
// actual layout poses of visible tags -- not exposed, so have to recreate
knownVisTags.add(new AprilTag(target.getFiducialId(), tagPose));
fieldToCams.add(tagPose.transformBy(target.getBestCameraToTarget().inverse()));
fieldToCamsAlt.add(tagPose.transformBy(target.getAlternateCameraToTarget().inverse()));
}
var cameraMatrixOpt = camera.getCameraMatrix();
var distCoeffsOpt = camera.getDistCoeffs();
boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
// multi-target solvePNP
if (hasCalibData) {
var cameraMatrix = cameraMatrixOpt.get();
var distCoeffs = distCoeffsOpt.get();
var pnpResults =
VisionEstimation.estimateCamPosePNP(cameraMatrix, distCoeffs, visCorners, knownVisTags);
var best =
new Pose3d()
.plus(pnpResults.best) // field-to-camera
.plus(robotToCamera.inverse()); // field-to-robot
// var alt = new Pose3d()
// .plus(pnpResults.alt) // field-to-camera
// .plus(robotToCamera.inverse()); // field-to-robot
return Optional.of(
new EstimatedRobotPose(best, result.getTimestampSeconds(), result.getTargets()));
} else {
// TODO fallback strategy? Should we just always do solvePNP?
return Optional.empty();
}
}
/**
* Return the estimated position of the robot with the lowest position ambiguity from a List of
* pipeline results.
@@ -299,7 +378,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds()));
result.getTimestampSeconds(),
result.getTargets()));
}
/**
@@ -352,7 +432,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds());
result.getTimestampSeconds(),
result.getTargets());
}
if (bestTransformDelta < smallestHeightDifference) {
@@ -363,7 +444,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds());
result.getTimestampSeconds(),
result.getTargets());
}
}
@@ -424,12 +506,14 @@ public class PhotonPoseEstimator {
if (altDifference < smallestPoseDelta) {
smallestPoseDelta = altDifference;
lowestDeltaPose =
new EstimatedRobotPose(altTransformPosition, result.getTimestampSeconds());
new EstimatedRobotPose(
altTransformPosition, result.getTimestampSeconds(), result.getTargets());
}
if (bestDifference < smallestPoseDelta) {
smallestPoseDelta = bestDifference;
lowestDeltaPose =
new EstimatedRobotPose(bestTransformPosition, result.getTimestampSeconds());
new EstimatedRobotPose(
bestTransformPosition, result.getTimestampSeconds(), result.getTargets());
}
}
return Optional.ofNullable(lowestDeltaPose);
@@ -471,7 +555,8 @@ public class PhotonPoseEstimator {
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds()));
result.getTimestampSeconds(),
result.getTargets()));
}
totalAmbiguity += 1.0 / target.getPoseAmbiguity();
@@ -502,7 +587,8 @@ public class PhotonPoseEstimator {
}
return Optional.of(
new EstimatedRobotPose(new Pose3d(transform, rotation), result.getTimestampSeconds()));
new EstimatedRobotPose(
new Pose3d(transform, rotation), result.getTimestampSeconds(), result.getTargets()));
}
/**

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

@@ -24,6 +24,10 @@
package org.photonvision;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.util.Arrays;
import java.util.List;
@@ -52,6 +56,28 @@ public class SimPhotonCamera {
ts.updateEntries();
}
/**
* Publishes the camera intrinsics matrix. The matrix should be in the form: spotless:off
* fx 0 cx
* 0 fy cy
* 0 0 1
* @param cameraMatrix The cam matrix
* spotless:on
*/
public void setCameraIntrinsicsMat(Matrix<N3, N3> cameraMatrix) {
ts.cameraIntrinsicsPublisher.set(cameraMatrix.getData());
}
/**
* Publishes the camera distortion matrix. The matrix should be in the form [k1 k2 p1 p2 k3]. See
* more: https://docs.opencv.org/3.4/d4/d94/tutorial_camera_calibration.html
*
* @param distortionMat The distortion mat
*/
public void setCameraDistortionMat(Matrix<N5, N1> distortionMat) {
ts.cameraDistortionPublisher.set(distortionMat.getData());
}
/**
* Constructs a Simulated PhotonCamera from the name of the camera.
*

64
photon-lib/src/main/java/org/photonvision/estimation/CameraTargetRelation.java Normal file
View File

@@ -0,0 +1,64 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.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()));
}
}

518
photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java Normal file
View File

@@ -0,0 +1,518 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.estimation;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.geometry.CoordinateSystem;
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 {
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);
}
}
public static MatOfDouble matrixToMat(SimpleMatrix matrix) {
var mat = new Mat(matrix.numRows(), matrix.numCols(), CvType.CV_64F);
mat.put(0, 0, matrix.getDDRM().getData());
var wrappedMat = new MatOfDouble();
mat.convertTo(wrappedMat, CvType.CV_64F);
mat.release();
return wrappedMat;
}
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 =
CoordinateSystem.convert(translations[i], CoordinateSystem.NWU(), CoordinateSystem.EDN());
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 CoordinateSystem.convert(
new Translation3d(data[0], data[1], data[2]),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
}
/**
* 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) {
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();
Vector<N3> axis = new Vector<>(Nat.N3());
axis.set(0, 0, data[0]);
axis.set(1, 0, data[1]);
axis.set(2, 0, data[2]);
return rotationEDNtoNWU(new Rotation3d(axis.div(axis.norm()), axis.norm()));
}
public static TargetCorner averageCorner(List<TargetCorner> corners) {
if (corners == null || corners.size() == 0) return null;
var pointMat = targetCornersToMat(corners);
Core.reduce(pointMat, pointMat, 0, Core.REDUCE_AVG);
var avgPt = matToTargetCorners(pointMat)[0];
pointMat.release();
return avgPt;
}
public static MatOfPoint2f targetCornersToMat(List<TargetCorner> corners) {
return targetCornersToMat(corners.toArray(TargetCorner[]::new));
}
public static MatOfPoint2f targetCornersToMat(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 new MatOfPoint2f(points);
}
public static TargetCorner[] pointsToTargetCorners(Point... points) {
var corners = new TargetCorner[points.length];
for (int i = 0; i < points.length; i++) {
corners[i] = new TargetCorner(points[i].x, points[i].y);
}
return corners;
}
public static TargetCorner[] matToTargetCorners(MatOfPoint2f matInput) {
var corners = new TargetCorner[(int) matInput.total()];
float[] data = new float[(int) matInput.total() * matInput.channels()];
matInput.get(0, 0, data);
for (int i = 0; i < corners.length; i++) {
corners[i] = 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;
}
/**
* Convert a rotation from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0} in EDN,
* this would be XYZ {0, -1, 0} in NWU.
*/
private static Rotation3d rotationEDNtoNWU(Rotation3d rot) {
return CoordinateSystem.convert(
new Rotation3d(), CoordinateSystem.NWU(), CoordinateSystem.EDN())
.plus(CoordinateSystem.convert(rot, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
}
/**
* Convert a rotation from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0} in EDN,
* this would be XYZ {0, -1, 0} in NWU.
*/
private static Rotation3d rotationNWUtoEDN(Rotation3d rot) {
return CoordinateSystem.convert(
new Rotation3d(), CoordinateSystem.EDN(), CoordinateSystem.NWU())
.plus(CoordinateSystem.convert(rot, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
}
/**
* 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 camPose The current camera pose in the 3d world
* @param objectTranslations The 3d points to be projected
* @return The 2d points in pixels which correspond to the image of the 3d points on the camera
*/
public static List<TargetCorner> projectPoints(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
Pose3d camPose,
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 basisChange = RotTrlTransform3d.makeRelativeTo(camPose);
var rvec = rotationToRvec(basisChange.getRotation());
var tvec = translationToTvec(basisChange.getTranslation());
var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
var imagePoints = new MatOfPoint2f();
// project to 2d
Calib3d.projectPoints(objectPoints, rvec, tvec, cameraMatrixMat, distCoeffsMat, imagePoints);
// turn 2d point Mat into TargetCorners
var corners = matToTargetCorners(imagePoints);
// release our Mats from native memory
objectPoints.release();
rvec.release();
tvec.release();
cameraMatrixMat.release();
distCoeffsMat.release();
imagePoints.release();
return Arrays.asList(corners);
}
/**
* Undistort 2d image points using a given camera's intrinsics and distortion.
*
* <p>2d image points from projectPoints(CameraProperties, Pose3d, 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 corners The distorted image points
* @return The undistorted image points
*/
public static List<TargetCorner> undistortPoints(
Matrix<N3, N3> cameraMatrix, Matrix<N5, N1> distCoeffs, List<TargetCorner> corners) {
var points_in = targetCornersToMat(corners);
var points_out = new MatOfPoint2f();
var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
Calib3d.undistortImagePoints(points_in, points_out, cameraMatrixMat, distCoeffsMat);
var corners_out = matToTargetCorners(points_out);
points_in.release();
points_out.release();
cameraMatrixMat.release();
distCoeffsMat.release();
return Arrays.asList(corners_out);
}
/**
* 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 corners The corners/points to be bounded
* @return Rectangle bounding the given corners
*/
public static Rect getBoundingRect(List<TargetCorner> corners) {
var corn = targetCornersToMat(corners);
var rect = Imgproc.boundingRect(corn);
corn.release();
return rect;
}
/**
* Gets the rotated rectangle with minimum area which bounds this contour.
*
* <p>Note that rectangle size and position are stored with doubles and have sub-pixel accuracy.
*
* @param corners The corners/points to be bounded
* @return Rotated rectangle bounding the given corners
*/
public static RotatedRect getMinAreaRect(List<TargetCorner> corners) {
var corn = targetCornersToMat(corners);
var rect = Imgproc.minAreaRect(corn);
corn.release();
return rect;
}
/**
* Get the area in pixels of this target's contour. It's important to note that this may be
* different from the area of the bounding rectangle around the contour.
*
* @param corners The corners defining this contour
* @return Area in pixels (units of corner x/y)
*/
public static double getContourAreaPx(List<TargetCorner> corners) {
var temp = targetCornersToMat(corners);
var corn = new MatOfPoint(temp.toArray());
temp.release();
// outputHull gives us indices (of corn) that make a convex hull contour
var outputHull = new MatOfInt();
Imgproc.convexHull(corn, outputHull);
int[] indices = outputHull.toArray();
outputHull.release();
var tempPoints = corn.toArray();
var points = tempPoints.clone();
for (int i = 0; i < indices.length; i++) {
points[i] = tempPoints[indices[i]];
}
corn.fromArray(points);
// calculate area of the (convex hull) contour
double area = Imgproc.contourArea(corn);
corn.release();
return area;
}
/**
* Finds the transformation(s) that map the camera's pose to the target 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.
*
* <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 imageCorners 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,
List<TargetCorner> imageCorners) {
// IPPE_SQUARE expects our corners in a specific order
modelTrls = reorderCircular(modelTrls, true, -1);
imageCorners = reorderCircular(imageCorners, true, -1);
// translate to opencv classes
var objectPoints = translationToTvec(modelTrls.toArray(new Translation3d[0]));
var imagePoints = targetCornersToMat(imageCorners);
var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
var rvecs = new ArrayList<Mat>();
var tvecs = new ArrayList<Mat>();
var rvec = Mat.zeros(3, 1, CvType.CV_32F);
var tvec = Mat.zeros(3, 1, CvType.CV_32F);
var reprojectionError = new Mat();
// calc rvecs/tvecs and associated reprojection error from image points
Calib3d.solvePnPGeneric(
objectPoints,
imagePoints,
cameraMatrixMat,
distCoeffsMat,
rvecs,
tvecs,
false,
Calib3d.SOLVEPNP_IPPE_SQUARE,
rvec,
tvec,
reprojectionError);
float[] errors = new float[2];
reprojectionError.get(0, 0, errors);
// convert to wpilib coordinates
var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));
Transform3d alt = null;
if (tvecs.size() > 1) {
alt = new Transform3d(tvecToTranslation(tvecs.get(1)), rvecToRotation(rvecs.get(1)));
}
// release our Mats from native memory
objectPoints.release();
imagePoints.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();
for (var v : tvecs) v.release();
rvec.release();
tvec.release();
reprojectionError.release();
if (alt != null) return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]);
else return new PNPResults(best, errors[0]);
}
/**
* Finds the transformation that maps the camera's pose to the target 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.
*
* <p>This method is intended for use with multiple targets and has no alternate solutions. There
* must be at least 3 points.
*
* @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 imageCorners 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,
List<TargetCorner> imageCorners) {
// translate to opencv classes
var objectPoints = translationToTvec(objectTrls.toArray(new Translation3d[0]));
var imagePoints = targetCornersToMat(imageCorners);
var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
var rvecs = new ArrayList<Mat>();
var tvecs = new ArrayList<Mat>();
var rvec = Mat.zeros(3, 1, CvType.CV_32F);
var tvec = Mat.zeros(3, 1, CvType.CV_32F);
var reprojectionError = new Mat();
// calc rvec/tvec from image points
Calib3d.solvePnPGeneric(
objectPoints,
imagePoints,
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
objectPoints.release();
imagePoints.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();
for (var v : tvecs) v.release();
rvec.release();
tvec.release();
reprojectionError.release();
return new PNPResults(best, error[0]);
}
}

72
photon-lib/src/main/java/org/photonvision/estimation/PNPResults.java Normal file
View File

@@ -0,0 +1,72 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.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 {
public final Transform3d best;
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;
public PNPResults() {
this(new Transform3d(), new Transform3d(), 0, 0, 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.best = best;
this.alt = alt;
this.ambiguity = ambiguity;
this.bestReprojErr = bestReprojErr;
this.altReprojErr = altReprojErr;
}
}

114
photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java Normal file
View File

@@ -0,0 +1,114 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.estimation;
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 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;
public RotTrlTransform3d() {
this(new Rotation3d(), new Translation3d());
}
/**
* Creates a rotation-translation transformation from a Transform3d.
*
* <p>Applying this transformation to poses will preserve their current origin-to-pose transform
* as if the origin was transformed by these components.
*
* @param trf The origin transformation
*/
public RotTrlTransform3d(Transform3d trf) {
this(trf.getRotation(), trf.getTranslation());
}
/**
* A rotation-translation transformation.
*
* <p>Applying this transformation to poses will preserve their current origin-to-pose transform
* as if the origin was transformed by these components.
*
* @param rot The rotation component
* @param trl The translation component
*/
public RotTrlTransform3d(Rotation3d rot, Translation3d trl) {
this.rot = rot;
this.trl = trl;
}
/**
* 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);
}
/** 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 apply(new Pose3d(trl, new Rotation3d())).getTranslation();
}
;
public List<Translation3d> applyTrls(List<Translation3d> trls) {
return trls.stream().map(t -> apply(t)).collect(Collectors.toList());
}
public Pose3d apply(Pose3d pose) {
return new Pose3d(pose.getTranslation().rotateBy(rot).plus(trl), pose.getRotation().plus(rot));
}
public List<Pose3d> applyPoses(List<Pose3d> poses) {
return poses.stream().map(p -> apply(p)).collect(Collectors.toList());
}
}

116
photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java Normal file
View File

@@ -0,0 +1,116 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.estimation;
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.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. If this target is spherical, this
* list has one translation with x == radius.
*/
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. */
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 spherical target model which has similar dimensions when viewed from any angle. This
* model will only have one vertex which has x == radius.
*/
public TargetModel(double diameterMeters) {
this.vertices = List.of(new Translation3d(diameterMeters / 2.0, 0, 0));
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.
*
* @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, only one vertex radius meters in front of the pose is
* returned.
*/
public List<Translation3d> getFieldVertices(Pose3d targetPose) {
return vertices.stream()
.map(t -> targetPose.plus(new Transform3d(t, new Rotation3d())).getTranslation())
.collect(Collectors.toList());
}
@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;
}
}

168
photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java Normal file
View File

@@ -0,0 +1,168 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.estimation;
import edu.wpi.first.apriltag.AprilTag;
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 edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.targeting.TargetCorner;
public class VisionEstimation {
public static final TargetModel kTagModel =
new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6));
/**
* Performs solvePNP using 3d-2d point correspondences 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!
*
* @param cameraMatrix the camera intrinsics matrix in standard opencv form
* @param distCoeffs the camera distortion matrix in standard opencv form
* @param corners The visible tag corners in the 2d image
* @param knownTags The known tag field poses corresponding to the visible tag IDs
* @return The transformation that maps the field origin to the camera pose
*/
public static PNPResults estimateCamPosePNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<TargetCorner> corners,
List<AprilTag> knownTags) {
if (knownTags == null
|| corners == null
|| corners.size() != knownTags.size() * 4
|| knownTags.size() == 0) {
return new PNPResults();
}
// single-tag pnp
if (corners.size() == 4) {
var camToTag =
OpenCVHelp.solvePNP_SQUARE(
cameraMatrix, distCoeffs, kTagModel.getFieldVertices(knownTags.get(0).pose), corners);
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 bestTagToCam = camToTag.best.inverse();
SmartDashboard.putNumberArray(
"multiTagBest_internal",
new double[] {
bestTagToCam.getX(),
bestTagToCam.getY(),
bestTagToCam.getZ(),
bestTagToCam.getRotation().getQuaternion().getW(),
bestTagToCam.getRotation().getQuaternion().getX(),
bestTagToCam.getRotation().getQuaternion().getY(),
bestTagToCam.getRotation().getQuaternion().getZ()
});
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(kTagModel.getFieldVertices(tag.pose));
var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners);
// var camToOrigin = OpenCVHelp.solveTagsPNPRansac(prop, objectTrls, corners);
return new PNPResults(
camToOrigin.best.inverse(),
camToOrigin.alt.inverse(),
camToOrigin.ambiguity,
camToOrigin.bestReprojErr,
camToOrigin.altReprojErr);
}
}
// /**
// * The best estimated transformation to the target, 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).
// */
// public static class PNPResults {
// public final Transform3d best;
// public final double bestReprojErr;
// /**
// * Alternate, ambiguous solution from solvepnp. This may be empty
// * if no alternate solution is found.
// */
// public final Transform3d alt;
// /** If no alternate solution is found, this is 0 */
// public final double altReprojErr;
// /** If no alternate solution is found, this is 0 */
// public final double ambiguity;
// public PNPResults() {
// this(new Transform3d(), new Transform3d(), 0, 0, 0);
// }
// public PNPResults(
// Transform3d best, Transform3d alt,
// double ambiguity, double bestReprojErr, double altReprojErr) {
// this.best = best;
// this.alt = alt;
// this.ambiguity = ambiguity;
// this.bestReprojErr = bestReprojErr;
// this.altReprojErr = altReprojErr;
// }
// }
/**
* The best estimated transformation (Rotation-translation composition) that maps a set of
* translations onto another with point correspondences, and its RMSE.
*/
public static class SVDResults {
public final RotTrlTransform3d trf;
/** If the result is invalid, this value is -1 */
public final double rmse;
public SVDResults() {
this(new RotTrlTransform3d(), -1);
}
public SVDResults(RotTrlTransform3d trf, double rmse) {
this.trf = trf;
this.rmse = rmse;
}
}
}

23
photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp
View File

@@ -26,6 +26,7 @@
#include <frc/Errors.h>
#include <frc/Timer.h>
#include <opencv2/core/mat.hpp>
#include "PhotonVersion.h"
#include "photonlib/Packet.h"
@@ -57,6 +58,10 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
ledModePub(mainTable->GetIntegerTopic("ledMode").Publish()),
ledModeSub(mainTable->GetIntegerTopic("ledMode").Subscribe(0)),
versionEntry(mainTable->GetStringTopic("version").Subscribe("")),
cameraIntrinsicsSubscriber(
rootTable->GetDoubleArrayTopic("cameraIntrinsics").Subscribe({})),
cameraDistortionSubscriber(
rootTable->GetDoubleArrayTopic("cameraDistortion").Subscribe({})),
driverModeSubscriber(
rootTable->GetBooleanTopic("driverMode").Subscribe(false)),
driverModePublisher(
@@ -119,12 +124,26 @@ LEDMode PhotonCamera::GetLEDMode() const {
return static_cast<LEDMode>(static_cast<int>(ledModeSub.Get()));
}
std::optional<cv::Mat> PhotonCamera::GetCameraMatrix() {
auto camCoeffs = cameraIntrinsicsSubscriber.Get();
if (camCoeffs.size() == 9) {
// clone should deal with ownership concerns? not sure
return cv::Mat(3, 3, CV_64FC1, camCoeffs.data()).clone();
}
return std::nullopt;
}
void PhotonCamera::SetLEDMode(LEDMode mode) {
ledModePub.Set(static_cast<double>(static_cast<int>(mode)));
}
const std::string_view PhotonCamera::GetCameraName() const {
return m_cameraName;
std::optional<cv::Mat> PhotonCamera::GetDistCoeffs() {
auto distCoeffs = cameraDistortionSubscriber.Get();
if (distCoeffs.size() == 5) {
// clone should deal with ownership concerns? not sure
return cv::Mat(5, 1, CV_64FC1, distCoeffs.data()).clone();
}
return std::nullopt;
}
void PhotonCamera::VerifyVersion() {

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

@@ -36,6 +36,9 @@
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/types.hpp>
#include <units/time.h>
#include "photonlib/PhotonCamera.h"
@@ -43,6 +46,16 @@
#include "photonlib/PhotonTrackedTarget.h"
namespace photonlib {
namespace detail {
cv::Point3d ToPoint3d(const frc::Translation3d& translation);
std::optional<std::array<cv::Point3d, 4>> CalcTagCorners(
int tagID, const frc::AprilTagFieldLayout& aprilTags);
frc::Pose3d ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec);
cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX,
units::meter_t cornerY, frc::Pose3d tagPose);
} // namespace detail
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
PoseStrategy strat, PhotonCamera&& cam,
frc::Transform3d robotToCamera)
@@ -53,6 +66,17 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()) {}
void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
if (strategy == MULTI_TAG_PNP) {
FRC_ReportError(
frc::warn::Warning,
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity",
"");
strategy = LOWEST_AMBIGUITY;
}
multiTagFallbackStrategy = strategy;
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
auto result = camera.GetLatestResult();
return Update(result);
@@ -64,6 +88,11 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
return std::nullopt;
}
return Update(result, this->strategy);
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
PhotonPipelineResult result, PoseStrategy strategy) {
std::optional<EstimatedRobotPose> ret = std::nullopt;
switch (strategy) {
@@ -83,6 +112,9 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
case AVERAGE_BEST_TARGETS:
ret = AverageBestTargetsStrategy(result);
break;
case ::photonlib::MULTI_TAG_PNP:
ret = MultiTagPnpStrategy(result);
break;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
"");
@@ -127,7 +159,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
fiducialPose.value()
.TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp()};
result.GetTimestamp(), result.GetTargets()};
}
std::optional<EstimatedRobotPose>
@@ -147,14 +179,14 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
frc::Pose3d const targetPose = fiducialPose.value();
units::meter_t alternativeDifference = units::math::abs(
units::meter_t const alternativeDifference = units::math::abs(
m_robotToCamera.Z() -
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.Z());
units::meter_t bestDifference = units::math::abs(
units::meter_t const bestDifference = units::math::abs(
m_robotToCamera.Z() -
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z());
@@ -163,14 +195,14 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp()};
result.GetTimestamp(), result.GetTargets()};
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp()};
result.GetTimestamp(), result.GetTargets()};
}
}
@@ -204,9 +236,9 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse());
units::meter_t alternativeDifference = units::math::abs(
units::meter_t const alternativeDifference = units::math::abs(
referencePose.Translation().Distance(altPose.Translation()));
units::meter_t bestDifference = units::math::abs(
units::meter_t const bestDifference = units::math::abs(
referencePose.Translation().Distance(bestPose.Translation()));
if (alternativeDifference < smallestDifference) {
smallestDifference = alternativeDifference;
@@ -221,7 +253,113 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
}
}
return EstimatedRobotPose{pose, stateTimestamp};
return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets()};
}
std::optional<std::array<cv::Point3d, 4>> detail::CalcTagCorners(
int tagID, const frc::AprilTagFieldLayout& aprilTags) {
if (auto tagPose = aprilTags.GetTagPose(tagID); tagPose.has_value()) {
return std::array{TagCornerToObjectPoint(-3_in, -3_in, *tagPose),
TagCornerToObjectPoint(+3_in, -3_in, *tagPose),
TagCornerToObjectPoint(+3_in, +3_in, *tagPose),
TagCornerToObjectPoint(-3_in, +3_in, *tagPose)};
} else {
return std::nullopt;
}
}
cv::Point3d detail::ToPoint3d(const frc::Translation3d& translation) {
return cv::Point3d(-translation.Y().value(), -translation.Z().value(),
+translation.X().value());
}
cv::Point3d detail::TagCornerToObjectPoint(units::meter_t cornerX,
units::meter_t cornerY,
frc::Pose3d tagPose) {
frc::Translation3d cornerTrans =
tagPose.Translation() +
frc::Translation3d(0.0_m, cornerX, cornerY).RotateBy(tagPose.Rotation());
return ToPoint3d(cornerTrans);
}
frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
using namespace frc;
using namespace units;
cv::Mat R;
cv::Rodrigues(rvec, R); // R is 3x3
R = R.t(); // rotation of inverse
cv::Mat tvecI = -R * tvec; // translation of inverse
Vectord<3> tv;
tv[0] = +tvecI.at<double>(2, 0);
tv[1] = -tvecI.at<double>(0, 0);
tv[2] = -tvecI.at<double>(1, 0);
Vectord<3> rv;
rv[0] = +rvec.at<double>(2, 0);
rv[1] = -rvec.at<double>(0, 0);
rv[2] = +rvec.at<double>(1, 0);
return Pose3d(Translation3d(meter_t{tv[0]}, meter_t{tv[1]}, meter_t{tv[2]}),
Rotation3d(
// radian_t{rv[0]},
// radian_t{rv[1]},
// radian_t{rv[2]}
rv, radian_t{rv.norm()}));
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
PhotonPipelineResult result) {
using namespace frc;
if (!result.HasTargets() || result.GetTargets().size() < 2) {
return Update(result, this->multiTagFallbackStrategy);
}
auto const targets = result.GetTargets();
// List of corners mapped from 3d space (meters) to the 2d camera screen
// (pixels).
std::vector<cv::Point3f> objectPoints;
std::vector<cv::Point2f> imagePoints;
// Add all target corners to main list of corners
for (auto target : targets) {
int id = target.GetFiducialId();
if (auto const tagCorners = detail::CalcTagCorners(id, aprilTags);
tagCorners.has_value()) {
auto const targetCorners = target.GetDetectedCorners();
for (size_t cornerIdx = 0; cornerIdx < 4; ++cornerIdx) {
imagePoints.emplace_back(targetCorners[cornerIdx].first,
targetCorners[cornerIdx].second);
objectPoints.emplace_back((*tagCorners)[cornerIdx]);
}
}
}
if (imagePoints.empty()) {
return std::nullopt;
}
// Use OpenCV ITERATIVE solver
cv::Mat const rvec(3, 1, cv::DataType<double>::type);
cv::Mat const tvec(3, 1, cv::DataType<double>::type);
auto const camMat = camera.GetCameraMatrix();
auto const distCoeffs = camera.GetDistCoeffs();
if (!camMat || !distCoeffs) {
return std::nullopt;
}
cv::solvePnP(objectPoints, imagePoints, camMat.value(), distCoeffs.value(),
rvec, tvec, false, cv::SOLVEPNP_SQPNP);
Pose3d const pose = detail::ToPose3d(tvec, rvec);
return photonlib::EstimatedRobotPose(
pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(),
result.GetTargets());
}
std::optional<EstimatedRobotPose>
@@ -247,7 +385,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
return EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetLatency()};
result.GetTimestamp(), result.GetTargets()};
}
totalAmbiguity += 1. / target.GetPoseAmbiguity();
@@ -261,12 +399,12 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
tempPoses) {
double weight = (1. / pair.second.first) / totalAmbiguity;
double const weight = (1. / pair.second.first) / totalAmbiguity;
transform = transform + pair.first.Translation() * weight;
rotation = rotation + pair.first.Rotation() * weight;
}
return EstimatedRobotPose{frc::Pose3d(transform, rotation),
result.GetTimestamp()};
result.GetTimestamp(), result.GetTargets()};
}
} // namespace photonlib

11
photon-lib/src/main/native/include/photonlib/PhotonCamera.h
View File

@@ -28,6 +28,7 @@
#include <string>
#include <networktables/BooleanTopic.h>
#include <networktables/DoubleArrayTopic.h>
#include <networktables/DoubleTopic.h>
#include <networktables/IntegerTopic.h>
#include <networktables/MultiSubscriber.h>
@@ -40,6 +41,10 @@
#include "photonlib/PhotonPipelineResult.h"
namespace cv {
class Mat;
} // namespace cv
namespace photonlib {
enum LEDMode : int { kDefault = -1, kOff = 0, kOn = 1, kBlink = 2 };
@@ -144,6 +149,9 @@ class PhotonCamera {
*/
const std::string_view GetCameraName() const;
std::optional<cv::Mat> GetCameraMatrix();
std::optional<cv::Mat> GetDistCoeffs();
/**
* Returns whether the latest target result has targets.
* This method is deprecated; {@link PhotonPipelineResult#hasTargets()} should
@@ -178,6 +186,9 @@ class PhotonCamera {
nt::IntegerSubscriber ledModeSub;
nt::StringSubscriber versionEntry;
nt::DoubleArraySubscriber cameraIntrinsicsSubscriber;
nt::DoubleArraySubscriber cameraDistortionSubscriber;
nt::BooleanSubscriber driverModeSubscriber;
nt::BooleanPublisher driverModePublisher;
nt::IntegerSubscriber ledModeSubscriber;

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

@@ -33,13 +33,18 @@
#include "photonlib/PhotonCamera.h"
#include "photonlib/PhotonPipelineResult.h"
namespace cv {
class Mat;
} // namespace cv
namespace photonlib {
enum PoseStrategy : int {
LOWEST_AMBIGUITY,
enum PoseStrategy {
LOWEST_AMBIGUITY = 0,
CLOSEST_TO_CAMERA_HEIGHT,
CLOSEST_TO_REFERENCE_POSE,
CLOSEST_TO_LAST_POSE,
AVERAGE_BEST_TARGETS
AVERAGE_BEST_TARGETS,
MULTI_TAG_PNP
};
struct EstimatedRobotPose {
@@ -49,8 +54,14 @@ struct EstimatedRobotPose {
* the same timebase as the RoboRIO FPGA Timestamp */
units::second_t timestamp;
EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_)
: estimatedPose(pose_), timestamp(time_) {}
/** A list of the targets used to compute this pose */
wpi::SmallVector<PhotonTrackedTarget, 10> targetsUsed;
EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_,
std::span<const PhotonTrackedTarget> targets)
: estimatedPose(pose_),
timestamp(time_),
targetsUsed(targets.data(), targets.data() + targets.size()) {}
};
/**
@@ -100,6 +111,14 @@ class PhotonPoseEstimator {
*/
inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
/**
* Set the Position Estimation Strategy used in multi-tag mode when
* only one tag can be seen. Must NOT be MULTI_TAG_PNP
*
* @param strategy the strategy to set
*/
void SetMultiTagFallbackStrategy(PoseStrategy strategy);
/**
* Return the reference position that is being used by the estimator.
*
@@ -159,6 +178,7 @@ class PhotonPoseEstimator {
private:
frc::AprilTagFieldLayout aprilTags;
PoseStrategy strategy;
PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY;
PhotonCamera camera;
frc::Transform3d m_robotToCamera;
@@ -166,6 +186,9 @@ class PhotonPoseEstimator {
frc::Pose3d lastPose;
frc::Pose3d referencePose;
std::optional<EstimatedRobotPose> Update(PhotonPipelineResult result,
PoseStrategy strategy);
/**
* Return the estimated position of the robot with the lowest position
* ambiguity from a List of pipeline results.
@@ -199,12 +222,21 @@ class PhotonPoseEstimator {
std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
PhotonPipelineResult result);
/**
* 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(
PhotonPipelineResult result);
/**
* 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.
timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> AverageBestTargetsStrategy(
PhotonPipelineResult result);

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

@@ -29,7 +29,6 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.hal.JNIWrapper;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
@@ -157,8 +156,6 @@ class PhotonPoseEstimatorTest {
@Test
void testClosestToCameraHeightStrategy() {
ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
cameraOne.result =
new PhotonPipelineResult(
@@ -474,8 +471,6 @@ class PhotonPoseEstimatorTest {
@Test
void averageBestPoses() {
ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
cameraOne.result =
new PhotonPipelineResult(

101
photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java Normal file
View File

@@ -0,0 +1,101 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.estimation;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.cscore.CameraServerCvJNI;
import edu.wpi.first.hal.JNIWrapper;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.net.WPINetJNI;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.NetworkTablesJNI;
import edu.wpi.first.util.CombinedRuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.io.IOException;
import org.junit.jupiter.api.BeforeAll;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonPoseEstimator;
public class ApriltagWorkbenchTest {
@BeforeAll
public static void setUp() {
JNIWrapper.Helper.setExtractOnStaticLoad(false);
WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
WPINetJNI.Helper.setExtractOnStaticLoad(false);
CameraServerCvJNI.Helper.setExtractOnStaticLoad(false);
try {
CombinedRuntimeLoader.loadLibraries(
ApriltagWorkbenchTest.class,
"wpiutiljni",
"ntcorejni",
"wpinetjni",
"wpiHaljni",
"cscorejnicvstatic");
} catch (Exception e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
// No version check for testing
PhotonCamera.setVersionCheckEnabled(false);
}
// @Test
public void testMeme() throws IOException, InterruptedException {
NetworkTableInstance instance = NetworkTableInstance.getDefault();
instance.stopServer();
// set the NT server if simulating this code.
// "localhost" for photon on desktop, or "photonvision.local" / "[ip-address]"
// for coprocessor
instance.setServer("localhost");
instance.startClient4("myRobot");
var robotToCamera = new Transform3d();
var cam = new PhotonCamera("WPI2023");
var tagLayout =
AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile);
var pe =
new PhotonPoseEstimator(
tagLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP, cam, robotToCamera);
var field = new Field2d();
SmartDashboard.putData(field);
while (!Thread.interrupted()) {
Thread.sleep(500);
var ret = pe.update();
System.out.println(ret);
field.setRobotPose(ret.get().estimatedPose.toPose2d());
}
}
}

1
photon-server/networktables.json Normal file
View File

@@ -0,0 +1 @@
[]

9
photon-targeting/build.gradle
View File

@@ -5,6 +5,11 @@ apply from: "${rootDir}/shared/common.gradle"
dependencies {
implementation wpilibTools.deps.wpilibJava("wpimath")
implementation "org.ejml:ejml-simple:0.41"
// Junit
testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2")
testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2")
testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.8.2")
}
tasks.withType(JavaCompile) {
@@ -19,4 +24,8 @@ java {
withSourcesJar()
}
test {
useJUnitPlatform()
}
apply from: "publish.gradle"

14
photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java
View File

@@ -182,4 +182,18 @@ public class Packet {
}
return packetData[readPos++] == 1;
}
public void encode(double[] data) {
for (double d : data) {
encode(d);
}
}
public double[] decodeDoubleArray(int len) {
double[] ret = new double[len];
for (int i = 0; i < len; i++) {
ret[i] = decodeDouble();
}
return ret;
}
}

12
photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java
View File

@@ -64,6 +64,10 @@ public class NTTopicSet {
public IntegerTopic heartbeatTopic;
public IntegerPublisher heartbeatPublisher;
// Camera Calibration
public DoubleArrayPublisher cameraIntrinsicsPublisher;
public DoubleArrayPublisher cameraDistortionPublisher;
public void updateEntries() {
rawBytesEntry =
subTable
@@ -93,6 +97,9 @@ public class NTTopicSet {
heartbeatTopic = subTable.getIntegerTopic("heartbeat");
heartbeatPublisher = heartbeatTopic.publish();
cameraIntrinsicsPublisher = subTable.getDoubleArrayTopic("cameraIntrinsics").publish();
cameraDistortionPublisher = subTable.getDoubleArrayTopic("cameraDistortion").publish();
}
@SuppressWarnings("DuplicatedCode")
@@ -113,5 +120,10 @@ public class NTTopicSet {
if (targetSkewEntry != null) targetSkewEntry.close();
if (bestTargetPosX != null) bestTargetPosX.close();
if (bestTargetPosY != null) bestTargetPosY.close();
if (heartbeatPublisher != null) heartbeatPublisher.close();
if (cameraIntrinsicsPublisher != null) cameraIntrinsicsPublisher.close();
if (cameraDistortionPublisher != null) cameraDistortionPublisher.close();
}
}

45
photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
View File

@@ -19,7 +19,6 @@ package org.photonvision.targeting;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import org.photonvision.common.dataflow.structures.Packet;
/** Represents a pipeline result from a PhotonCamera. */
@@ -123,21 +122,6 @@ public class PhotonPipelineResult {
return new ArrayList<>(targets);
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
PhotonPipelineResult that = (PhotonPipelineResult) o;
boolean latencyMatch = Double.compare(that.latencyMillis, latencyMillis) == 0;
boolean targetsMatch = that.targets.equals(targets);
return latencyMatch && targetsMatch;
}
@Override
public int hashCode() {
return Objects.hash(latencyMillis, targets);
}
/**
* Populates the fields of the pipeline result from the packet.
*
@@ -178,4 +162,33 @@ public class PhotonPipelineResult {
// Return the packet.
return packet;
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + ((targets == null) ? 0 : targets.hashCode());
long temp;
temp = Double.doubleToLongBits(latencyMillis);
result = prime * result + (int) (temp ^ (temp >>> 32));
temp = Double.doubleToLongBits(timestampSeconds);
result = prime * result + (int) (temp ^ (temp >>> 32));
return result;
}
@Override
public boolean equals(Object obj) {
if (this == obj) return true;
if (obj == null) return false;
if (getClass() != obj.getClass()) return false;
PhotonPipelineResult other = (PhotonPipelineResult) obj;
if (targets == null) {
if (other.targets != null) return false;
} else if (!targets.equals(other.targets)) return false;
if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis))
return false;
if (Double.doubleToLongBits(timestampSeconds)
!= Double.doubleToLongBits(other.timestampSeconds)) return false;
return true;
}
}

6
photonlib-cpp-examples/apriltagExample/.wpilib/wpilib_preferences.json Normal file
View File

@@ -0,0 +1,6 @@
{
"enableCppIntellisense": true,
"currentLanguage": "cpp",
"projectYear": "2023",
"teamNumber": 5
}

1
photonlib-cpp-examples/apriltagExample/networktables.json Normal file
View File

@@ -0,0 +1 @@
[]

81
photonlib-cpp-examples/apriltagExample/simgui-window.json Normal file
View File

@@ -0,0 +1,81 @@
{
"Docking": {
"Data": []
},
"MainWindow": {
"GLOBAL": {
"fps": "120",
"height": "880",
"maximized": "0",
"style": "0",
"userScale": "2",
"width": "1652",
"xpos": "268",
"ypos": "82"
}
},
"Table": {
"0x542B5671,2": {
"Column 0 Width": "391",
"Column 1 Width": "156",
"RefScale": "13"
}
},
"Window": {
"###/SmartDashboard/Field": {
"Collapsed": "0",
"Pos": "514,2",
"Size": "517,341"
},
"###FMS": {
"Collapsed": "0",
"Pos": "36,663",
"Size": "283,146"
},
"###Joysticks": {
"Collapsed": "0",
"Pos": "373,500",
"Size": "796,206"
},
"###Keyboard 0 Settings": {
"Collapsed": "0",
"Pos": "149,98",
"Size": "300,560"
},
"###NetworkTables": {
"Collapsed": "0",
"Pos": "663,464",
"Size": "750,365"
},
"###NetworkTables Info": {
"Collapsed": "0",
"Pos": "520,330",
"Size": "750,145"
},
"###Other Devices": {
"Collapsed": "0",
"Pos": "1025,20",
"Size": "250,695"
},
"###System Joysticks": {
"Collapsed": "0",
"Pos": "5,350",
"Size": "192,218"
},
"###Timing": {
"Collapsed": "0",
"Pos": "5,150",
"Size": "135,127"
},
"Debug##Default": {
"Collapsed": "0",
"Pos": "60,60",
"Size": "400,400"
},
"Robot State": {
"Collapsed": "0",
"Pos": "5,20",
"Size": "92,99"
}
}
}

40
photonlib-cpp-examples/apriltagExample/simgui.json Normal file
View File

@@ -0,0 +1,40 @@
{
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo",
"/SmartDashboard/Field": "Field2d"
},
"windows": {
"/SmartDashboard/Field": {
"window": {
"visible": true
}
}
}
},
"NetworkTables": {
"transitory": {
"CameraPublisher": {
"open": true
},
"SmartDashboard": {
"open": true
},
"photonvision": {
"open": true,
"photonvision": {
"open": true
}
}
}
},
"NetworkTables Info": {
"Clients": {
"open": true
},
"Server": {
"open": true
},
"visible": true
}
}

5
photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h
View File

@@ -23,6 +23,7 @@
*/
#pragma once
#include <photonlib/PhotonCamera.h>
#include <photonlib/PhotonPoseEstimator.h>
@@ -35,8 +36,8 @@ class PhotonCameraWrapper {
public:
photonlib::PhotonPoseEstimator m_poseEstimator{
frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp),
photonlib::LOWEST_AMBIGUITY,
std::move(photonlib::PhotonCamera{"WPI2023"}), frc::Transform3d{}};
photonlib::MULTI_TAG_PNP, std::move(photonlib::PhotonCamera{"WPI2023"}),
frc::Transform3d{}};
inline std::optional<photonlib::EstimatedRobotPose> Update(
frc::Pose2d estimatedPose) {

4
shared/config.gradle
View File

@@ -9,8 +9,8 @@ nativeUtils.wpi.configureDependencies {
wpiVersion = wpilibVersion
wpimathVersion = wpilibVersion
niLibVersion = "2023.3.0"
opencvVersion = opencvVersion
googleTestVersion = "1.11.0-4"
opencvVersion = "4.6.0-4"
googleTestVersion = "1.12.1-1"
imguiVersion = "1.86-1"
}