diff --git a/.gitignore b/.gitignore index 4c59f92df..ca5a93289 100644 --- a/.gitignore +++ b/.gitignore @@ -145,6 +145,7 @@ build/spotlessJava build/* build photon-lib/src/main/java/org/photonvision/PhotonVersion.java +photon-lib/bin/main/images/* /photonlib-java-examples/bin/ photon-lib/src/generate/native/include/PhotonVersion.h .gitattributes diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 741117401..a14f09c39 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -34,7 +34,10 @@ nativeUtils.platformConfigs.linuxathena.cppCompiler.args.add("-Wno-deprecated-an dependencies { implementation project(":photon-targeting") + // WPILib deps implementation wpilibTools.deps.wpilibJava("wpiutil") + implementation wpilibTools.deps.wpilibJava("cameraserver") + implementation wpilibTools.deps.wpilibJava("cscore") implementation wpilibTools.deps.wpilibJava("wpimath") implementation wpilibTools.deps.wpilibJava("wpinet") implementation wpilibTools.deps.wpilibJava("hal") @@ -42,12 +45,18 @@ dependencies { implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") - testImplementation wpilibTools.deps.wpilibJava("cscore") + // Jackson + implementation "com.fasterxml.jackson.core:jackson-annotations:2.12.4" + implementation "com.fasterxml.jackson.core:jackson-core:2.12.4" + implementation "com.fasterxml.jackson.core:jackson-databind:2.12.4" + 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 wpilibTools.deps.wpilibJava("cscore") 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") @@ -157,6 +166,7 @@ def testNativeTasks = wpilibTools.createExtractionTasks { testNativeTasks.addToSourceSetResources(sourceSets.test) +testNativeConfig.dependencies.add wpilibTools.deps.cscore() testNativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore") testNativeConfig.dependencies.add wpilibTools.deps.wpilib("wpinet") testNativeConfig.dependencies.add wpilibTools.deps.wpilib("hal") diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index dc0052912..b9978a802 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -51,10 +51,10 @@ import org.photonvision.common.hardware.VisionLEDMode; import org.photonvision.targeting.PhotonPipelineResult; /** Represents a camera that is connected to PhotonVision. */ -public class PhotonCamera { - static final String kTableName = "photonvision"; +public class PhotonCamera implements AutoCloseable { + public static final String kTableName = "photonvision"; - protected final NetworkTable cameraTable; + private final NetworkTable cameraTable; RawSubscriber rawBytesEntry; BooleanPublisher driverModePublisher; BooleanSubscriber driverModeSubscriber; @@ -73,6 +73,7 @@ public class PhotonCamera { private DoubleArraySubscriber cameraIntrinsicsSubscriber; private DoubleArraySubscriber cameraDistortionSubscriber; + @Override public void close() { rawBytesEntry.close(); driverModePublisher.close(); @@ -151,9 +152,7 @@ public class PhotonCamera { m_topicNameSubscriber = new MultiSubscriber( - instance, - new String[] {"/photonvision/"}, - new PubSubOption[] {PubSubOption.topicsOnly(true)}); + instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true)); } /** @@ -186,7 +185,7 @@ public class PhotonCamera { ret.createFromPacket(packet); // Set the timestamp of the result. - // getLatestChange returns in microseconds so we divide by 1e6 to convert to seconds. + // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds. ret.setTimestampSeconds((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3); // Return result. @@ -334,6 +333,14 @@ public class PhotonCamera { } else return Optional.empty(); } + /** + * Gets the NetworkTable representing this camera's subtable. You probably don't ever need to call + * this. + */ + public final NetworkTable getCameraTable() { + return cameraTable; + } + private void verifyVersion() { if (!VERSION_CHECK_ENABLED) return; diff --git a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java index acd9a71c2..b834ad445 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -285,7 +285,7 @@ public final class OpenCVHelp { /** * Undistort 2d image points using a given camera's intrinsics and distortion. * - *

2d image points from projectPoints(CameraProperties, Pose3d, List) projectPoints} will + *

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). * diff --git a/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java b/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java index 549614de8..41c3e36d9 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java @@ -106,7 +106,7 @@ public class RotTrlTransform3d { ; public List applyTrls(List trls) { - return trls.stream().map(t -> apply(t)).collect(Collectors.toList()); + return trls.stream().map(this::apply).collect(Collectors.toList()); } public Pose3d apply(Pose3d pose) { @@ -114,6 +114,6 @@ public class RotTrlTransform3d { } public List applyPoses(List poses) { - return poses.stream().map(p -> apply(p)).collect(Collectors.toList()); + return poses.stream().map(this::apply).collect(Collectors.toList()); } } diff --git a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java index bc3a735e9..9cdc1811f 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -25,20 +25,34 @@ 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 edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.ArrayList; import java.util.List; +import java.util.Objects; +import java.util.stream.Collectors; +import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; public class VisionEstimation { - public static final TargetModel kTagModel = - new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6)); + /** Get the visible {@link AprilTag}s according the tag layout using the visible tag IDs. */ + public static List getVisibleLayoutTags( + List 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 to estimate the field-to-camera @@ -47,29 +61,37 @@ public class VisionEstimation { *

Note: The returned transformation is from the field origin to the camera pose! * (Unless you only feed this one tag??) * - * @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 + * @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 + * @param tagLayout The known tag layout on the field * @return The transformation that maps the field origin to the camera pose */ @Deprecated public static PNPResults estimateCamPosePNP( Matrix cameraMatrix, Matrix distCoeffs, - List corners, - List knownTags) { - if (knownTags == null - || corners == null - || corners.size() != knownTags.size() * 4 - || knownTags.size() == 0) { + List visTags, + AprilTagFieldLayout tagLayout) { + if (tagLayout == null + || visTags == null + || tagLayout.getTags().size() == 0 + || visTags.size() == 0) { return new PNPResults(); } + + var corners = new ArrayList(); + for (var tag : visTags) corners.addAll(tag.getDetectedCorners()); + var knownTags = getVisibleLayoutTags(visTags, tagLayout); + if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) { + return new PNPResults(); + } + // single-tag pnp - if (corners.size() == 4) { + if (visTags.size() == 1) { var camToTag = OpenCVHelp.solvePNP_SQUARE( - cameraMatrix, distCoeffs, kTagModel.getFieldVertices(knownTags.get(0).pose), corners); + cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, corners); var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse()); var altPose = new Pose3d(); if (camToTag.ambiguity != 0) @@ -99,9 +121,8 @@ public class VisionEstimation { // multi-tag pnp else { var objectTrls = new ArrayList(); - for (var tag : knownTags) objectTrls.addAll(kTagModel.getFieldVertices(tag.pose)); + for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.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(), @@ -135,7 +156,7 @@ public class VisionEstimation { return new PNPResults(); } var objectTrls = new ArrayList(); - for (var tag : knownTags) objectTrls.addAll(kTagModel.getFieldVertices(tag.pose)); + for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.vertices); var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners); // var camToOrigin = OpenCVHelp.solveTagsPNPRansac(prop, objectTrls, corners); return new PNPResults( diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java new file mode 100644 index 000000000..710d7f49b --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -0,0 +1,464 @@ +/* + * 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.simulation; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.CvSource; +import edu.wpi.first.cscore.VideoMode.PixelFormat; +import edu.wpi.first.cscore.VideoSource.ConnectionStrategy; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.util.RuntimeLoader; +import edu.wpi.first.util.WPIUtilJNI; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import org.opencv.core.Core; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonTargetSortMode; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.networktables.NTTopicSet; +import org.photonvision.estimation.CameraTargetRelation; +import org.photonvision.estimation.OpenCVHelp; +import org.photonvision.estimation.PNPResults; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; + +/** + * A handle for simulating {@link PhotonCamera} values. Processing simulated targets through this + * class will change the associated PhotonCamera's results. + */ +@SuppressWarnings("unused") +public class PhotonCameraSim implements AutoCloseable { + private final PhotonCamera cam; + + NTTopicSet ts = new NTTopicSet(); + private long heartbeatCounter = 0; + + /** This simulated camera's {@link SimCameraProperties} */ + public final SimCameraProperties prop; + + private long nextNTEntryTime = WPIUtilJNI.now(); + + private double maxSightRangeMeters = Double.MAX_VALUE; + private static final double kDefaultMinAreaPx = 90; + private double minTargetAreaPercent; + private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest; + + // video stream simulation + private final CvSource videoSimRaw; + private final Mat videoSimFrameRaw = new Mat(); + private boolean videoSimRawEnabled = true; + private final CvSource videoSimProcessed; + private final Mat videoSimFrameProcessed = new Mat(); + private boolean videoSimProcEnabled = true; + + 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); + } + } + + @Override + public void close() { + videoSimRaw.close(); + videoSimFrameRaw.release(); + videoSimProcessed.close(); + videoSimFrameProcessed.release(); + } + + /** + * Constructs a handle for simulating {@link PhotonCamera} values. Processing simulated targets + * through this class will change the associated PhotonCamera's results. + * + *

This constructor's camera has a 90 deg FOV with no simulated lag! + * + *

By default, the minimum target area is 100 pixels and there is no maximum sight range. + * + * @param camera The camera to be simulated + */ + public PhotonCameraSim(PhotonCamera camera) { + this(camera, SimCameraProperties.PERFECT_90DEG()); + } + + /** + * Constructs a handle for simulating {@link PhotonCamera} values. Processing simulated targets + * through this class will change the associated PhotonCamera's results. + * + *

By default, the minimum target area is 100 pixels and there is no maximum sight range. + * + * @param camera The camera to be simulated + * @param prop Properties of this camera such as FOV and FPS + */ + public PhotonCameraSim(PhotonCamera camera, SimCameraProperties prop) { + this.cam = camera; + this.prop = prop; + setMinTargetAreaPixels(kDefaultMinAreaPx); + + videoSimRaw = + CameraServer.putVideo(camera.getName() + "-raw", prop.getResWidth(), prop.getResHeight()); + videoSimRaw.setPixelFormat(PixelFormat.kGray); + videoSimProcessed = + CameraServer.putVideo( + camera.getName() + "-processed", prop.getResWidth(), prop.getResHeight()); + + ts.removeEntries(); + ts.subTable = camera.getCameraTable(); + ts.updateEntries(); + } + + /** + * Constructs a handle for simulating {@link PhotonCamera} values. Processing simulated targets + * through this class will change the associated PhotonCamera's results. + * + * @param camera The camera to be simulated + * @param prop Properties of this camera such as FOV and FPS + * @param minTargetAreaPercent The minimum percentage(0 - 100) a detected target must take up of + * the camera's image to be processed. Match this with your contour filtering settings in the + * PhotonVision GUI. + * @param maxSightRangeMeters Maximum distance at which the target is illuminated to your camera. + * Note that minimum target area of the image is separate from this. + */ + public PhotonCameraSim( + PhotonCamera camera, + SimCameraProperties prop, + double minTargetAreaPercent, + double maxSightRangeMeters) { + this(camera, prop); + this.minTargetAreaPercent = minTargetAreaPercent; + this.maxSightRangeMeters = maxSightRangeMeters; + } + + public PhotonCamera getCamera() { + return cam; + } + + public double getMinTargetAreaPercent() { + return minTargetAreaPercent; + } + + public double getMinTargetAreaPixels() { + return minTargetAreaPercent / 100.0 * prop.getResArea(); + } + + public double getMaxSightRangeMeters() { + return maxSightRangeMeters; + } + + public PhotonTargetSortMode getTargetSortMode() { + return sortMode; + } + + public CvSource getVideoSimRaw() { + return videoSimRaw; + } + + public Mat getVideoSimFrameRaw() { + return videoSimFrameRaw; + } + + /** + * Determines if this target's pose should be visible to the camera without considering its + * projected image points. Does not account for image area. + * + * @param camPose Camera's 3d pose + * @param target Vision target containing pose and shape + * @return If this vision target can be seen before image projection. + */ + public boolean canSeeTargetPose(Pose3d camPose, VisionTargetSim target) { + var rel = new CameraTargetRelation(camPose, target.getPose()); + return ( + // target translation is outside of camera's FOV + (Math.abs(rel.camToTargYaw.getDegrees()) < prop.getHorizFOV().getDegrees() / 2) + && (Math.abs(rel.camToTargPitch.getDegrees()) < prop.getVertFOV().getDegrees() / 2) + && (!target.getModel().isPlanar + || Math.abs(rel.targToCamAngle.getDegrees()) + < 90) // camera is behind planar target and it should be occluded + && (rel.camToTarg.getTranslation().getNorm() <= maxSightRangeMeters)); // target is too far + } + + /** + * Determines if all target corners are inside the camera's image. + * + * @param corners The corners of the target as image points(x,y) + */ + public boolean canSeeCorners(List corners) { + // corner is outside of resolution + for (var corner : corners) { + if (MathUtil.clamp(corner.x, 0, prop.getResWidth()) != corner.x + || MathUtil.clamp(corner.y, 0, prop.getResHeight()) != corner.y) { + return false; + } + } + return true; + } + + /** + * Determine if this camera should process a new frame based on performance metrics and the time + * since the last update. This returns an Optional which is either empty if no update should occur + * or a Long of the timestamp in microseconds of when the frame which should be received by NT. If + * a timestamp is returned, the last frame update time becomes that timestamp. + * + * @return Optional long which is empty while blocked or the NT entry timestamp in microseconds if + * ready + */ + public Optional consumeNextEntryTime() { + // check if this camera is ready for another frame update + long now = WPIUtilJNI.now(); + long timestamp = -1; + int iter = 0; + // prepare next latest update + while (now >= nextNTEntryTime) { + timestamp = nextNTEntryTime; + long frameTime = (long) (prop.estMsUntilNextFrame() * 1e3); + nextNTEntryTime += frameTime; + + // if frame time is very small, avoid blocking + if (iter++ > 50) { + timestamp = now; + nextNTEntryTime = now + frameTime; + break; + } + } + // return the timestamp of the latest update + if (timestamp >= 0) return Optional.of(timestamp); + // or this camera isn't ready to process yet + else return Optional.empty(); + } + + /** + * The minimum percentage(0 - 100) a detected target must take up of the camera's image to be + * processed. + */ + public void setMinTargetAreaPercent(double areaPercent) { + this.minTargetAreaPercent = areaPercent; + } + + /** + * The minimum number of pixels a detected target must take up in the camera's image to be + * processed. + */ + public void setMinTargetAreaPixels(double areaPx) { + this.minTargetAreaPercent = areaPx / prop.getResArea() * 100; + } + + /** + * Maximum distance at which the target is illuminated to your camera. Note that minimum target + * area of the image is separate from this. + */ + public void setMaxSightRange(double rangeMeters) { + this.maxSightRangeMeters = rangeMeters; + } + + /** Defines the order the targets are sorted in the pipeline result. */ + public void setTargetSortMode(PhotonTargetSortMode sortMode) { + if (sortMode != null) this.sortMode = sortMode; + } + + /** Sets whether the raw video stream simulation is enabled. */ + public void enableRawStream(boolean enabled) { + videoSimRawEnabled = enabled; + } + + /** Sets whether the processed video stream simulation is enabled. */ + public void enableProcessedStream(boolean enabled) { + videoSimProcEnabled = enabled; + } + + public PhotonPipelineResult process( + double latencyMillis, Pose3d cameraPose, List targets) { + // sort targets by distance to camera + targets = new ArrayList<>(targets); + targets.sort( + (t1, t2) -> { + double dist1 = t1.getPose().getTranslation().getDistance(cameraPose.getTranslation()); + double dist2 = t2.getPose().getTranslation().getDistance(cameraPose.getTranslation()); + if (dist1 == dist2) return 0; + return dist1 < dist2 ? 1 : -1; + }); + // all targets visible (in FOV) + var visibleTags = new ArrayList>>(); + // all targets actually detectable to the camera + var detectableTgts = new ArrayList(); + + // reset our frame + VideoSimUtil.updateVideoProp(videoSimRaw, prop); + VideoSimUtil.updateVideoProp(videoSimProcessed, prop); + Size videoFrameSize = new Size(prop.getResWidth(), prop.getResHeight()); + Mat.zeros(videoFrameSize, CvType.CV_8UC1).assignTo(videoSimFrameRaw); + + for (var tgt : targets) { + // pose isn't visible, skip to next + if (!canSeeTargetPose(cameraPose, tgt)) continue; + + // find target's 3d corner points + // TODO: Handle spherical targets + var fieldCorners = tgt.getFieldVertices(); + + // project 3d target points into 2d image points + var targetCorners = + OpenCVHelp.projectPoints( + prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, fieldCorners); + // save visible tags for stream simulation + if (tgt.fiducialID >= 0) { + visibleTags.add(new Pair<>(tgt.fiducialID, targetCorners)); + } + // estimate pixel noise + var noisyTargetCorners = prop.estPixelNoise(targetCorners); + // find the (naive) 2d yaw/pitch + var centerPt = OpenCVHelp.getMinAreaRect(noisyTargetCorners).center; + var centerRot = prop.getPixelRot(new TargetCorner(centerPt.x, centerPt.y)); + // find contour area + double areaPercent = prop.getContourAreaPercent(noisyTargetCorners); + + // projected target can't be detected, skip to next + if (!(canSeeCorners(noisyTargetCorners) && areaPercent >= minTargetAreaPercent)) continue; + + var pnpSim = new PNPResults(); + if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP + pnpSim = + OpenCVHelp.solvePNP_SQUARE( + prop.getIntrinsics(), + prop.getDistCoeffs(), + tgt.getModel().vertices, + noisyTargetCorners); + centerRot = + prop.getPixelRot( + OpenCVHelp.projectPoints( + prop.getIntrinsics(), + prop.getDistCoeffs(), + new Pose3d(), + List.of(pnpSim.best.getTranslation())) + .get(0)); + } + + Point[] minAreaRectPts = new Point[noisyTargetCorners.size()]; + OpenCVHelp.getMinAreaRect(noisyTargetCorners).points(minAreaRectPts); + + detectableTgts.add( + new PhotonTrackedTarget( + Math.toDegrees(centerRot.getZ()), + -Math.toDegrees(centerRot.getY()), + areaPercent, + Math.toDegrees(centerRot.getX()), + tgt.fiducialID, + pnpSim.best, + pnpSim.alt, + pnpSim.ambiguity, + List.of(OpenCVHelp.pointsToTargetCorners(minAreaRectPts)), + noisyTargetCorners)); + } + // render visible tags to raw video frame + if (videoSimRawEnabled) { + for (var tag : visibleTags) { + VideoSimUtil.warp16h5TagImage( + tag.getFirst(), OpenCVHelp.targetCornersToMat(tag.getSecond()), videoSimFrameRaw, true); + } + videoSimRaw.putFrame(videoSimFrameRaw); + } else videoSimRaw.setConnectionStrategy(ConnectionStrategy.kForceClose); + // draw/annotate tag detection outline on processed view + if (videoSimProcEnabled) { + Imgproc.cvtColor(videoSimFrameRaw, videoSimFrameProcessed, Imgproc.COLOR_GRAY2BGR); + for (var tgt : detectableTgts) { + if (tgt.getFiducialId() >= 0) { + VideoSimUtil.drawTagDetection( + tgt.getFiducialId(), + OpenCVHelp.targetCornersToMat(tgt.getDetectedCorners()), + videoSimFrameProcessed); + } + } + videoSimProcessed.putFrame(videoSimFrameProcessed); + } else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose); + + // put this simulated data to NT + if (sortMode != null) { + detectableTgts.sort(sortMode.getComparator()); + } + return new PhotonPipelineResult(latencyMillis, detectableTgts); + } + + /** + * Simulate one processed frame of vision data, putting one result to NT at current timestamp. + * Image capture time is assumed be (current time - latency). + * + * @param result The pipeline result to submit + */ + public void submitProcessedFrame(PhotonPipelineResult result) { + submitProcessedFrame(result, WPIUtilJNI.now()); + } + + /** + * Simulate one processed frame of vision data, putting one result to NT. Image capture timestamp + * overrides {@link PhotonPipelineResult#getTimestampSeconds() getTimestampSeconds()} for more + * precise latency simulation. + * + * @param result The pipeline result to submit + * @param receiveTimestamp The (sim) timestamp when this result was read by NT in microseconds + */ + public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimestamp) { + ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp); + + var newPacket = new Packet(result.getPacketSize()); + result.populatePacket(newPacket); + ts.rawBytesEntry.set(newPacket.getData(), receiveTimestamp); + + boolean hasTargets = result.hasTargets(); + ts.hasTargetEntry.set(hasTargets, receiveTimestamp); + if (!hasTargets) { + ts.targetPitchEntry.set(0.0, receiveTimestamp); + ts.targetYawEntry.set(0.0, receiveTimestamp); + ts.targetAreaEntry.set(0.0, receiveTimestamp); + ts.targetPoseEntry.set(new double[] {0.0, 0.0, 0.0}, receiveTimestamp); + ts.targetSkewEntry.set(0.0, receiveTimestamp); + } else { + var bestTarget = result.getBestTarget(); + + ts.targetPitchEntry.set(bestTarget.getPitch(), receiveTimestamp); + ts.targetYawEntry.set(bestTarget.getYaw(), receiveTimestamp); + ts.targetAreaEntry.set(bestTarget.getArea(), receiveTimestamp); + ts.targetSkewEntry.set(bestTarget.getSkew(), receiveTimestamp); + + var transform = bestTarget.getBestCameraToTarget(); + double[] poseData = { + transform.getX(), transform.getY(), transform.getRotation().toRotation2d().getDegrees() + }; + ts.targetPoseEntry.set(poseData, receiveTimestamp); + } + + ts.heartbeatPublisher.set(heartbeatCounter++, receiveTimestamp); + } +} diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java new file mode 100644 index 000000000..336c7d502 --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java @@ -0,0 +1,601 @@ +/* + * 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.simulation; + +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.numbers.*; +import edu.wpi.first.wpilibj.DriverStation; +import java.io.IOException; +import java.nio.file.Path; +import java.util.List; +import java.util.Random; +import java.util.stream.Collectors; +import org.photonvision.estimation.OpenCVHelp; +import org.photonvision.targeting.TargetCorner; + +/** + * Calibration and performance values for this camera. + * + *

The resolution will affect the accuracy of projected(3d to 2d) target corners and similarly + * the severity of image noise on estimation(2d to 3d). + * + *

The camera intrinsics and distortion coefficients describe the results of calibration, and how + * to map between 3d field points and 2d image points. + * + *

The performance values (framerate/exposure time, latency) determine how often results should + * be updated and with how much latency in simulation. High exposure time causes motion blur which + * can inhibit target detection while moving. Note that latency estimation does not account for + * network latency and the latency reported will always be perfect. + */ +public class SimCameraProperties { + private final Random rand = new Random(); + // calibration + private int resWidth; + private int resHeight; + private Matrix camIntrinsics; + private Matrix distCoeffs; + private double avgErrorPx; + private double errorStdDevPx; + // performance + private double frameSpeedMs = 0; + private double exposureTimeMs = 0; + private double avgLatencyMs = 0; + private double latencyStdDevMs = 0; + + /** Default constructor which is the same as {@link #PERFECT_90DEG} */ + public SimCameraProperties() { + setCalibration(960, 720, Rotation2d.fromDegrees(90)); + } + + /** + * Reads camera properties from a photonvision config.json file. This is only the + * resolution, camera intrinsics, distortion coefficients, and average/std. dev. pixel error. + * Other camera properties must be set. + * + * @param path Path to the config.json file + * @param width The width of the desired resolution in the JSON + * @param height The height of the desired resolution in the JSON + * @throws IOException If reading the JSON fails, either from an invalid path or a missing/invalid + * calibrated resolution. + */ + public SimCameraProperties(String path, int width, int height) throws IOException { + this(Path.of(path), width, height); + } + + /** + * Reads camera properties from a photonvision config.json file. This is only the + * resolution, camera intrinsics, distortion coefficients, and average/std. dev. pixel error. + * Other camera properties must be set. + * + * @param path Path to the config.json file + * @param width The width of the desired resolution in the JSON + * @param height The height of the desired resolution in the JSON + * @throws IOException If reading the JSON fails, either from an invalid path or a missing/invalid + * calibrated resolution. + */ + public SimCameraProperties(Path path, int width, int height) throws IOException { + var mapper = new ObjectMapper(); + var json = mapper.readTree(path.toFile()); + json = json.get("calibrations"); + boolean success = false; + try { + for (int i = 0; i < json.size() && !success; i++) { + // check if this calibration entry is our desired resolution + var calib = json.get(i); + int jsonWidth = calib.get("resolution").get("width").asInt(); + int jsonHeight = calib.get("resolution").get("height").asInt(); + if (jsonWidth != width || jsonHeight != height) continue; + // get the relevant calibration values + var jsonIntrinsicsNode = calib.get("cameraIntrinsics").get("data"); + double[] jsonIntrinsics = new double[jsonIntrinsicsNode.size()]; + for (int j = 0; j < jsonIntrinsicsNode.size(); j++) { + jsonIntrinsics[j] = jsonIntrinsicsNode.get(j).asDouble(); + } + var jsonDistortNode = calib.get("cameraExtrinsics").get("data"); + double[] jsonDistortion = new double[jsonDistortNode.size()]; + for (int j = 0; j < jsonDistortNode.size(); j++) { + jsonDistortion[j] = jsonDistortNode.get(j).asDouble(); + } + var jsonViewErrors = calib.get("perViewErrors"); + double jsonAvgError = 0; + for (int j = 0; j < jsonViewErrors.size(); j++) { + jsonAvgError += jsonViewErrors.get(j).asDouble(); + } + jsonAvgError /= jsonViewErrors.size(); + double jsonErrorStdDev = calib.get("standardDeviation").asDouble(); + // assign the read JSON values to this CameraProperties + setCalibration( + jsonWidth, + jsonHeight, + Matrix.mat(Nat.N3(), Nat.N3()).fill(jsonIntrinsics), + Matrix.mat(Nat.N5(), Nat.N1()).fill(jsonDistortion)); + setCalibError(jsonAvgError, jsonErrorStdDev); + success = true; + } + } catch (Exception e) { + throw new IOException("Invalid calibration JSON"); + } + if (!success) throw new IOException("Requested resolution not found in calibration"); + } + + public void setRandomSeed(long seed) { + rand.setSeed(seed); + } + + public void setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) { + double s = Math.sqrt(resWidth * resWidth + resHeight * resHeight); + var fovWidth = new Rotation2d(fovDiag.getRadians() * (resWidth / s)); + var fovHeight = new Rotation2d(fovDiag.getRadians() * (resHeight / s)); + + double maxFovDeg = Math.max(fovWidth.getDegrees(), fovHeight.getDegrees()); + if (maxFovDeg > 179) { + double scale = 179.0 / maxFovDeg; + fovWidth = new Rotation2d(fovWidth.getRadians() * scale); + fovHeight = new Rotation2d(fovHeight.getRadians() * scale); + DriverStation.reportError( + "Requested FOV width/height too large! Scaling below 180 degrees...", false); + } + + // assume no distortion + var distCoeff = VecBuilder.fill(0, 0, 0, 0, 0); + + // assume centered principal point (pixels) + double cx = resWidth / 2.0; + double cy = resHeight / 2.0; + + // use given fov to determine focal point (pixels) + double fx = cx / Math.tan(fovWidth.getRadians() / 2.0); + double fy = cy / Math.tan(fovHeight.getRadians() / 2.0); + + // create camera intrinsics matrix + var camIntrinsics = Matrix.mat(Nat.N3(), Nat.N3()).fill(fx, 0, cx, 0, fy, cy, 0, 0, 1); + setCalibration(resWidth, resHeight, camIntrinsics, distCoeff); + } + + public void setCalibration( + int resWidth, int resHeight, Matrix camIntrinsics, Matrix distCoeffs) { + this.resWidth = resWidth; + this.resHeight = resHeight; + this.camIntrinsics = camIntrinsics; + this.distCoeffs = distCoeffs; + } + + public void setCameraIntrinsics(Matrix camIntrinsics) { + this.camIntrinsics = camIntrinsics; + } + + public void setDistortionCoeffs(Matrix distCoeffs) { + this.distCoeffs = distCoeffs; + } + + public void setCalibError(double avgErrorPx, double errorStdDevPx) { + this.avgErrorPx = avgErrorPx; + this.errorStdDevPx = errorStdDevPx; + } + + /** + * @param fps The average frames per second the camera should process at. Exposure time limits + * FPS if set! + */ + public void setFPS(double fps) { + frameSpeedMs = Math.max(1000.0 / fps, exposureTimeMs); + } + + /** + * @param exposureTimeMs The amount of time the "shutter" is open for one frame. Affects motion + * blur. Frame speed(from FPS) is limited to this! + */ + public void setExposureTimeMs(double exposureTimeMs) { + this.exposureTimeMs = exposureTimeMs; + frameSpeedMs = Math.max(frameSpeedMs, exposureTimeMs); + } + + /** + * @param avgLatencyMs The average latency (from image capture to data published) in milliseconds + * a frame should have + */ + public void setAvgLatencyMs(double avgLatencyMs) { + this.avgLatencyMs = avgLatencyMs; + } + + /** + * @param latencyStdDevMs The standard deviation in milliseconds of the latency + */ + public void setLatencyStdDevMs(double latencyStdDevMs) { + this.latencyStdDevMs = latencyStdDevMs; + } + + public int getResWidth() { + return resWidth; + } + + public int getResHeight() { + return resHeight; + } + + public int getResArea() { + return resWidth * resHeight; + } + + public Matrix getIntrinsics() { + return camIntrinsics.copy(); + } + + public Vector getDistCoeffs() { + return new Vector<>(distCoeffs); + } + + public double getFPS() { + return 1000.0 / frameSpeedMs; + } + + public double getFrameSpeedMs() { + return frameSpeedMs; + } + + public double getExposureTimeMs() { + return exposureTimeMs; + } + + public double getAvgLatencyMs() { + return avgLatencyMs; + } + + public double getLatencyStdDevMs() { + return latencyStdDevMs; + } + + public SimCameraProperties copy() { + var newProp = new SimCameraProperties(); + newProp.setCalibration(resWidth, resHeight, camIntrinsics, distCoeffs); + newProp.setCalibError(avgErrorPx, errorStdDevPx); + newProp.setFPS(getFPS()); + newProp.setExposureTimeMs(exposureTimeMs); + newProp.setAvgLatencyMs(avgLatencyMs); + newProp.setLatencyStdDevMs(latencyStdDevMs); + return newProp; + } + + public List undistort(List points) { + return OpenCVHelp.undistortPoints(camIntrinsics, distCoeffs, points); + } + + /** + * The percentage(0 - 100) of this camera's resolution the contour takes up in pixels of the + * image. + * + * @param corners Corners of the contour + */ + public double getContourAreaPercent(List corners) { + return OpenCVHelp.getContourAreaPx(corners) / getResArea() * 100; + } + + /** The yaw from the principal point of this camera to the pixel x value. Positive values left. */ + public Rotation2d getPixelYaw(double pixelX) { + double fx = camIntrinsics.get(0, 0); + // account for principal point not being centered + double cx = camIntrinsics.get(0, 2); + double xOffset = cx - pixelX; + return new Rotation2d(fx, xOffset); + } + + /** + * The pitch from the principal point of this camera to the pixel y value. Pitch is positive down. + * + *

Note that this angle is naively computed and may be incorrect. See {@link + * #getCorrectedPixelRot(TargetCorner)}. + */ + public Rotation2d getPixelPitch(double pixelY) { + double fy = camIntrinsics.get(1, 1); + // account for principal point not being centered + double cy = camIntrinsics.get(1, 2); + double yOffset = cy - pixelY; + return new Rotation2d(fy, -yOffset); + } + + /** + * Finds the yaw and pitch to the given image point. Yaw is positive left, and pitch is positive + * down. + * + *

Note that pitch is naively computed and may be incorrect. See {@link + * #getCorrectedPixelRot(TargetCorner)}. + */ + public Rotation3d getPixelRot(TargetCorner point) { + return new Rotation3d( + 0, getPixelPitch(point.y).getRadians(), getPixelYaw(point.x).getRadians()); + } + + /** + * Gives the yaw and pitch of the line intersecting the camera lens and the given pixel + * coordinates on the sensor. Yaw is positive left, and pitch positive down. + * + *

The pitch traditionally calculated from pixel offsets do not correctly account for non-zero + * values of yaw because of perspective distortion (not to be confused with lens distortion)-- for + * example, the pitch angle is naively calculated as: + * + * 

pitch = arctan(pixel y offset / focal length y)
+ * + * However, using focal length as a side of the associated right triangle is not correct when the + * pixel x value is not 0, because the distance from this pixel (projected on the x-axis) to the + * camera lens increases. Projecting a line back out of the camera with these naive angles will + * not intersect the 3d point that was originally projected into this 2d pixel. Instead, this + * length should be: + * + * 
focal length y ⟶ (focal length y / cos(arctan(pixel x offset / focal length x)))
+ * + * @return Rotation3d with yaw and pitch of the line projected out of the camera from the given + * pixel (roll is zero). + */ + public Rotation3d getCorrectedPixelRot(TargetCorner point) { + double fx = camIntrinsics.get(0, 0); + double cx = camIntrinsics.get(0, 2); + double xOffset = cx - point.x; + + double fy = camIntrinsics.get(1, 1); + double cy = camIntrinsics.get(1, 2); + double yOffset = cy - point.y; + + // calculate yaw normally + var yaw = new Rotation2d(fx, xOffset); + // correct pitch based on yaw + var pitch = new Rotation2d(fy / Math.cos(Math.atan(xOffset / fx)), -yOffset); + + return new Rotation3d(0, pitch.getRadians(), yaw.getRadians()); + } + + public Rotation2d getHorizFOV() { + // sum of FOV left and right principal point + var left = getPixelYaw(0); + var right = getPixelYaw(resWidth); + return left.minus(right); + } + + public Rotation2d getVertFOV() { + // sum of FOV above and below principal point + var above = getPixelPitch(0); + var below = getPixelPitch(resHeight); + return below.minus(above); + } + + public Rotation2d getDiagFOV() { + return new Rotation2d(Math.hypot(getHorizFOV().getRadians(), getVertFOV().getRadians())); + } + + /** Width:height */ + public double getAspectRatio() { + return (double) resWidth / resHeight; + } + + /** + * Returns these pixel points as fractions of a 1x1 square image. This means the camera's aspect + * ratio and resolution will be used, and the points' x and y may not reach all portions(e.g. a + * wide aspect ratio means some of the top and bottom of the square image is unreachable). + * + * @param points Pixel points on this camera's image + * @return Points mapped to an image of 1x1 resolution + */ + public List getPixelFraction(List points) { + double resLarge = getAspectRatio() > 1 ? resWidth : resHeight; + + return points.stream() + .map( + p -> { + // offset to account for aspect ratio + return new TargetCorner( + (p.x + (resLarge - resWidth) / 2.0) / resLarge, + (p.y + (resLarge - resHeight) / 2.0) / resLarge); + }) + .collect(Collectors.toList()); + } + + /** Returns these points after applying this camera's estimated noise. */ + public List estPixelNoise(List points) { + if (avgErrorPx == 0 && errorStdDevPx == 0) return points; + + return points.stream() + .map( + p -> { + // error pixels in random direction + double error = avgErrorPx + rand.nextGaussian() * errorStdDevPx; + double errorAngle = rand.nextDouble() * 2 * Math.PI - Math.PI; + return new TargetCorner( + p.x + error * Math.cos(errorAngle), p.y + error * Math.sin(errorAngle)); + }) + .collect(Collectors.toList()); + } + + /** + * @return Noisy estimation of a frame's processing latency in milliseconds + */ + public double estLatencyMs() { + return Math.max(avgLatencyMs + rand.nextGaussian() * latencyStdDevMs, 0); + } + + /** + * @return Estimate how long until the next frame should be processed in milliseconds + */ + public double estMsUntilNextFrame() { + // exceptional processing latency blocks the next frame + return frameSpeedMs + Math.max(0, estLatencyMs() - frameSpeedMs); + } + + // pre-calibrated example cameras + + /** 960x720 resolution, 90 degree FOV, "perfect" lagless camera */ + public static SimCameraProperties PERFECT_90DEG() { + return new SimCameraProperties(); + } + + public static SimCameraProperties PI4_LIFECAM_320_240() { + var prop = new SimCameraProperties(); + prop.setCalibration( + 320, + 240, + Matrix.mat(Nat.N3(), Nat.N3()) + .fill( // intrinsic + 328.2733242048587, + 0.0, + 164.8190261141906, + 0.0, + 318.0609794305216, + 123.8633838438093, + 0.0, + 0.0, + 1.0), + VecBuilder.fill( // distort + 0.09957946553445934, + -0.9166265114485799, + 0.0019519890627236526, + -0.0036071725380870333, + 1.5627234622420942)); + prop.setCalibError(0.21, 0.0124); + prop.setFPS(30); + prop.setAvgLatencyMs(30); + prop.setLatencyStdDevMs(10); + return prop; + } + + public static SimCameraProperties PI4_LIFECAM_640_480() { + var prop = new SimCameraProperties(); + prop.setCalibration( + 640, + 480, + Matrix.mat(Nat.N3(), Nat.N3()) + .fill( // intrinsic + 669.1428078983059, + 0.0, + 322.53377249329213, + 0.0, + 646.9843137061716, + 241.26567383784163, + 0.0, + 0.0, + 1.0), + VecBuilder.fill( // distort + 0.12788470750464645, + -1.2350335805796528, + 0.0024990767286192732, + -0.0026958287600230705, + 2.2951386729115537)); + prop.setCalibError(0.26, 0.046); + prop.setFPS(15); + prop.setAvgLatencyMs(65); + prop.setLatencyStdDevMs(15); + return prop; + } + + public static SimCameraProperties LL2_640_480() { + var prop = new SimCameraProperties(); + prop.setCalibration( + 640, + 480, + Matrix.mat(Nat.N3(), Nat.N3()) + .fill( // intrinsic + 511.22843367007755, + 0.0, + 323.62049380211096, + 0.0, + 514.5452336723849, + 261.8827920543568, + 0.0, + 0.0, + 1.0), + VecBuilder.fill( // distort + 0.1917469998873756, + -0.5142936883324216, + 0.012461562046896614, + 0.0014084973492408186, + 0.35160648971214437)); + prop.setCalibError(0.25, 0.05); + prop.setFPS(15); + prop.setAvgLatencyMs(35); + prop.setLatencyStdDevMs(8); + return prop; + } + + public static SimCameraProperties LL2_960_720() { + var prop = new SimCameraProperties(); + prop.setCalibration( + 960, + 720, + Matrix.mat(Nat.N3(), Nat.N3()) + .fill( // intrinsic + 769.6873145148892, + 0.0, + 486.1096609458122, + 0.0, + 773.8164483705323, + 384.66071662358354, + 0.0, + 0.0, + 1.0), + VecBuilder.fill( // distort + 0.189462064814501, + -0.49903003669627627, + 0.007468423590519429, + 0.002496885298683693, + 0.3443122090208624)); + prop.setCalibError(0.35, 0.10); + prop.setFPS(10); + prop.setAvgLatencyMs(50); + prop.setLatencyStdDevMs(15); + return prop; + } + + public static SimCameraProperties LL2_1280_720() { + var prop = new SimCameraProperties(); + prop.setCalibration( + 1280, + 720, + Matrix.mat(Nat.N3(), Nat.N3()) + .fill( // intrinsic + 1011.3749416937393, + 0.0, + 645.4955139388737, + 0.0, + 1008.5391755084075, + 508.32877656020196, + 0.0, + 0.0, + 1.0), + VecBuilder.fill( // distort + 0.13730101577061535, + -0.2904345656989261, + 8.32475714507539E-4, + -3.694397782014239E-4, + 0.09487962227027584)); + prop.setCalibError(0.37, 0.06); + prop.setFPS(7); + prop.setAvgLatencyMs(60); + prop.setLatencyStdDevMs(20); + return prop; + } +} diff --git a/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java similarity index 97% rename from photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java rename to photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java index a0ce8f37b..9f301486a 100644 --- a/photon-lib/src/main/java/org/photonvision/SimPhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java @@ -22,7 +22,7 @@ * SOFTWARE. */ -package org.photonvision; +package org.photonvision.simulation; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; @@ -31,11 +31,17 @@ import edu.wpi.first.math.numbers.N5; import edu.wpi.first.networktables.NetworkTableInstance; import java.util.Arrays; import java.util.List; +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.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +/** + * @deprecated Use {@link PhotonCameraSim} instead + */ +@Deprecated @SuppressWarnings("unused") public class SimPhotonCamera { NTTopicSet ts = new NTTopicSet(); diff --git a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java similarity index 99% rename from photon-lib/src/main/java/org/photonvision/SimVisionSystem.java rename to photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java index 4ae091475..52756f472 100644 --- a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java @@ -22,7 +22,7 @@ * SOFTWARE. */ -package org.photonvision; +package org.photonvision.simulation; import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; @@ -41,6 +41,10 @@ import java.util.List; import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; +/** + * @deprecated Use {@link VisionSystemSim} instead + */ +@Deprecated public class SimVisionSystem { SimPhotonCamera cam; diff --git a/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionTarget.java similarity index 95% rename from photon-lib/src/main/java/org/photonvision/SimVisionTarget.java rename to photon-lib/src/main/java/org/photonvision/simulation/SimVisionTarget.java index 2b5c2ceab..0d66154d9 100644 --- a/photon-lib/src/main/java/org/photonvision/SimVisionTarget.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimVisionTarget.java @@ -22,10 +22,14 @@ * SOFTWARE. */ -package org.photonvision; +package org.photonvision.simulation; import edu.wpi.first.math.geometry.Pose3d; +/** + * @deprecated Use {@link VisionTargetSim} instead + */ +@Deprecated public class SimVisionTarget { Pose3d targetPose; double targetWidthMeters; diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java new file mode 100644 index 000000000..cec341b4d --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java @@ -0,0 +1,337 @@ +/* + * 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.simulation; + +import edu.wpi.first.cscore.CvSource; +import edu.wpi.first.util.RuntimeLoader; +import java.awt.image.BufferedImage; +import java.io.IOException; +import java.util.Arrays; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import javax.imageio.ImageIO; +import org.opencv.core.Core; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.core.Size; +import org.opencv.imgcodecs.Imgcodecs; +import org.opencv.imgproc.Imgproc; +import org.photonvision.estimation.OpenCVHelp; + +public class VideoSimUtil { + public static final String kLocalTagImagesPath = "./src/main/resources/images/apriltags/"; + public static final String kResourceTagImagesPath = "/images/apriltags/"; + public static final String kTag16h5ImageName = "tag16_05_00000"; + public static final int kNumTags16h5 = 30; + + // All 16h5 tag images + private static final Map kTag16h5Images = new HashMap<>(); + // Points corresponding to marker(black square) corners of 8x8 16h5 tag images + public static final Point[] kTag16h5MarkerPts; + + 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); + } + + // create Mats of 8x8 apriltag images + for (int i = 0; i < VideoSimUtil.kNumTags16h5; i++) { + Mat tagImage = VideoSimUtil.get16h5TagImage(i); + kTag16h5Images.put(i, tagImage); + } + + kTag16h5MarkerPts = get16h5MarkerPts(); + } + + /** Updates the properties of this CvSource video stream with the given camera properties. */ + public static void updateVideoProp(CvSource video, SimCameraProperties prop) { + video.setResolution(prop.getResWidth(), prop.getResHeight()); + video.setFPS((int) prop.getFPS()); + } + + /** + * Gets the points representing the corners of this image. Because image pixels are accessed + * through a Mat, the point (0,0) actually represents the center of the top-left pixel and not the + * actual top-left corner. + * + * @param size Size of image + */ + public static Point[] getImageCorners(Size size) { + return new Point[] { + new Point(-0.5, -0.5), + new Point(size.width - 0.5, -0.5), + new Point(size.width - 0.5, size.height - 0.5), + new Point(-0.5, size.height - 0.5) + }; + } + + /** + * Gets the 8x8 (grayscale) image of a specific 16h5 AprilTag. + * + * @param id The fiducial id of the desired tag + */ + public static Mat get16h5TagImage(int id) { + String name = kTag16h5ImageName; + String idString = String.valueOf(id); + name = name.substring(0, name.length() - idString.length()) + idString; + + var resource = VideoSimUtil.class.getResource(kResourceTagImagesPath + name + ".png"); + // local IDE tests + String path = kLocalTagImagesPath + name + ".png"; + // gradle tests + if (resource != null) { + path = resource.getPath(); + + // TODO why did we have this previously? + // if (path.startsWith("/")) path = path.substring(1); + } + Mat result = new Mat(); + if (!path.startsWith("file")) result = Imgcodecs.imread(path, Imgcodecs.IMREAD_GRAYSCALE); + // reading jar file + if (result.empty()) { + BufferedImage buf; + try { + buf = ImageIO.read(resource); + } catch (IOException e) { + System.err.println("Couldn't read tag image!"); + return result; + } + + result = new Mat(buf.getHeight(), buf.getWidth(), CvType.CV_8UC1); + + byte[] px = new byte[1]; + for (int y = 0; y < result.height(); y++) { + for (int x = 0; x < result.width(); x++) { + px[0] = (byte) (buf.getRGB(x, y) & 0xFF); + result.put(y, x, px); + } + } + } + return result; + } + + /** Gets the points representing the marker(black square) corners. */ + public static Point[] get16h5MarkerPts() { + return get16h5MarkerPts(1); + } + + /** + * Gets the points representing the marker(black square) corners. + * + * @param scale The scale of the tag image (8*scale x 8*scale image) + */ + public static Point[] get16h5MarkerPts(int scale) { + var roi16h5 = new Rect(new Point(1, 1), new Size(6, 6)); + roi16h5.x *= scale; + roi16h5.y *= scale; + roi16h5.width *= scale; + roi16h5.height *= scale; + var pts = getImageCorners(roi16h5.size()); + for (int i = 0; i < pts.length; i++) { + var pt = pts[i]; + pts[i] = new Point(roi16h5.tl().x + pt.x, roi16h5.tl().y + pt.y); + } + return pts; + } + + /** + * Warps the image of a specific 16h5 AprilTag onto the destination image at the given points. + * + * @param tagId The id of the specific tag to warp onto the destination image + * @param dstPoints Points(4) in destination image where the tag marker(black square) corners + * should be warped onto. + * @param destination The destination image to place the warped tag image onto. + * @param antialiasing If antialiasing should be performed by automatically + * supersampling/interpolating the warped image. This should be used if better stream quality + * is desired or target detection is being done on the stream, but can hurt performance. + * @see OpenCVHelp#targetCornersToMat(org.photonvision.targeting.TargetCorner...) + */ + public static void warp16h5TagImage( + int tagId, MatOfPoint2f dstPoints, Mat destination, boolean antialiasing) { + Mat tagImage = kTag16h5Images.get(tagId); + if (tagImage == null || tagImage.empty()) return; + var tagPoints = new MatOfPoint2f(kTag16h5MarkerPts); + // points of tag image corners + var tagImageCorners = new MatOfPoint2f(getImageCorners(tagImage.size())); + // the rectangle describing the rectangle-of-interest(ROI) + var boundingRect = Imgproc.boundingRect(dstPoints); + // find the perspective transform from the tag image to the warped destination points + Mat perspecTrf = Imgproc.getPerspectiveTransform(tagPoints, dstPoints); + // check extreme image corners after transform to check if we need to expand bounding rect + var extremeCorners = new MatOfPoint2f(); + Core.perspectiveTransform(tagImageCorners, extremeCorners, perspecTrf); + // dilate ROI to fit full tag + boundingRect = Imgproc.boundingRect(extremeCorners); + + // adjust interpolation strategy based on size of warped tag compared to tag image + var warpedContourArea = Imgproc.contourArea(extremeCorners); + double warpedTagUpscale = Math.sqrt(warpedContourArea) / Math.sqrt(tagImage.size().area()); + int warpStrategy = Imgproc.INTER_NEAREST; + // automatically determine the best supersampling of warped image and scale of tag image + /* + (warpPerspective does not properly resample, so this is used to avoid aliasing in the + warped image. Supersampling is used when the warped tag is small, but is very slow + when the warped tag is large-- scaling the tag image up and using linear interpolation + instead can be performant while still effectively antialiasing. Some combination of these + two can be used in between those extremes.) + + TODO: Simplify magic numbers to one or two variables, or use a more proper approach? + */ + int supersampling = 6; + supersampling = (int) Math.ceil(supersampling / warpedTagUpscale); + supersampling = Math.max(Math.min(supersampling, 8), 1); + + Mat scaledTagImage = new Mat(); + if (warpedTagUpscale > 2.0) { + warpStrategy = Imgproc.INTER_LINEAR; + int scaleFactor = (int) (warpedTagUpscale / 3.0) + 2; + scaleFactor = Math.max(Math.min(scaleFactor, 40), 1); + scaleFactor *= supersampling; + Imgproc.resize( + tagImage, scaledTagImage, new Size(), scaleFactor, scaleFactor, Imgproc.INTER_NEAREST); + tagPoints.fromArray(get16h5MarkerPts(scaleFactor)); + } else tagImage.assignTo(scaledTagImage); + + // constrain the bounding rect inside of the destination image + boundingRect.x -= 1; + boundingRect.y -= 1; + boundingRect.width += 2; + boundingRect.height += 2; + if (boundingRect.x < 0) { + boundingRect.width += boundingRect.x; + boundingRect.x = 0; + } + if (boundingRect.y < 0) { + boundingRect.height += boundingRect.y; + boundingRect.y = 0; + } + boundingRect.width = Math.min(destination.width() - boundingRect.x, boundingRect.width); + boundingRect.height = Math.min(destination.height() - boundingRect.y, boundingRect.height); + if (boundingRect.width <= 0 || boundingRect.height <= 0) return; + + // upscale if supersampling + Mat scaledDstPts = new Mat(); + if (supersampling > 1) { + Core.multiply(dstPoints, new Scalar(supersampling, supersampling), scaledDstPts); + boundingRect.x *= supersampling; + boundingRect.y *= supersampling; + boundingRect.width *= supersampling; + boundingRect.height *= supersampling; + } else dstPoints.assignTo(scaledDstPts); + + // update transform relative to expanded, scaled bounding rect + Core.subtract(scaledDstPts, new Scalar(boundingRect.tl().x, boundingRect.tl().y), scaledDstPts); + perspecTrf = Imgproc.getPerspectiveTransform(tagPoints, scaledDstPts); + + // warp (scaled) tag image onto (scaled) ROI image representing the portion of + // the destination image encapsulated by boundingRect + Mat tempROI = new Mat(); + Imgproc.warpPerspective(scaledTagImage, tempROI, perspecTrf, boundingRect.size(), warpStrategy); + + // downscale ROI with interpolation if supersampling + if (supersampling > 1) { + boundingRect.x /= supersampling; + boundingRect.y /= supersampling; + boundingRect.width /= supersampling; + boundingRect.height /= supersampling; + Imgproc.resize(tempROI, tempROI, boundingRect.size(), 0, 0, Imgproc.INTER_AREA); + } + + // we want to copy ONLY the transformed tag to the result image, not the entire bounding rect + // using a mask only copies the source pixels which have an associated non-zero value in the + // mask + Mat tempMask = Mat.zeros(tempROI.size(), CvType.CV_8UC1); + Core.subtract( + extremeCorners, new Scalar(boundingRect.tl().x, boundingRect.tl().y), extremeCorners); + Point tempCenter = new Point(); + tempCenter.x = + Arrays.stream(extremeCorners.toArray()).mapToDouble(p -> p.x).average().getAsDouble(); + tempCenter.y = + Arrays.stream(extremeCorners.toArray()).mapToDouble(p -> p.y).average().getAsDouble(); + // dilate tag corners + Arrays.stream(extremeCorners.toArray()) + .forEach( + p -> { + double xdiff = p.x - tempCenter.x; + double ydiff = p.y - tempCenter.y; + xdiff += 1 * Math.signum(xdiff); + ydiff += 1 * Math.signum(ydiff); + new Point(tempCenter.x + xdiff, tempCenter.y + ydiff); + }); + // (make inside of tag completely white in mask) + Imgproc.fillConvexPoly(tempMask, new MatOfPoint(extremeCorners.toArray()), new Scalar(255)); + + // copy transformed tag onto result image + tempROI.copyTo(destination.submat(boundingRect), tempMask); + } + + /** + * Draws a contour around the given points and text of the id onto the destination image. + * + * @param id Fiducial ID number to draw + * @param dstPoints Points representing the four corners of the tag marker(black square) in the + * destination image. + * @param destination The destination image to draw onto. The image should be in the BGR color + * space. + */ + public static void drawTagDetection(int id, MatOfPoint2f dstPoints, Mat destination) { + var dstPointsd = new MatOfPoint(dstPoints.toArray()); + double scaleX = destination.width() / 640.0; + double scaleY = destination.height() / 480.0; + double minScale = Math.min(scaleX, scaleY); + int thickness = (int) (1 * minScale); + // for(var pt : dstPoints.toArray()) { + // Imgproc.circle(destination, pt, 4, new Scalar(255), 1, Imgproc.LINE_AA); + // } + // Imgproc.rectangle(destination, extremeRect, new Scalar(255), 1, Imgproc.LINE_AA); + // Imgproc.rectangle(destination, Imgproc.boundingRect(dstPoints), new Scalar(255), 1, + // Imgproc.LINE_AA); + Imgproc.polylines( + destination, List.of(dstPointsd), true, new Scalar(0, 0, 255), thickness, Imgproc.LINE_AA); + var textPt = Imgproc.boundingRect(dstPoints).tl(); + textPt.x -= 10.0 * scaleX; + textPt.y -= 12.0 * scaleY; + Imgproc.putText( + destination, + String.valueOf(id), + textPt, + Imgproc.FONT_HERSHEY_PLAIN, + 1.5 * minScale, + new Scalar(0, 0, 255), + thickness, + Imgproc.LINE_AA); + } +} diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java new file mode 100644 index 000000000..6a1c36cf3 --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java @@ -0,0 +1,377 @@ +/* + * 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.simulation; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.ArrayList; +import java.util.Collection; +import java.util.HashMap; +import java.util.HashSet; +import java.util.List; +import java.util.Map; +import java.util.Optional; +import java.util.Set; +import java.util.stream.Collectors; +import org.photonvision.PhotonCamera; +import org.photonvision.estimation.TargetModel; + +/** + * A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot + * running PhotonVision, detecting targets placed on the field. {@link VisionTargetSim}s added to + * this class will be detected by the {@link PhotonCameraSim}s added to this class. This class + * should be updated periodically with the robot's current pose in order to publish the simulated + * camera target info. + */ +public class VisionSystemSim { + private final Map camSimMap = new HashMap<>(); + private static final double kBufferLengthSeconds = 1.5; + // save robot-to-camera for each camera over time (Pose3d is easily interpolatable) + private final Map> camTrfMap = new HashMap<>(); + + // interpolate drivetrain with twists + private final TimeInterpolatableBuffer robotPoseBuffer = + TimeInterpolatableBuffer.createBuffer(kBufferLengthSeconds); + + private final Map> targetSets = new HashMap<>(); + + private final Field2d dbgField; + + /** + * A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot + * running PhotonVision, detecting targets placed on the field. {@link VisionTargetSim}s added to + * this class will be detected by the {@link PhotonCameraSim}s added to this class. This class + * should be updated periodically with the robot's current pose in order to publish the simulated + * camera target info. + * + * @param visionSystemName The specific identifier for this vision system in NetworkTables. + */ + public VisionSystemSim(String visionSystemName) { + dbgField = new Field2d(); + String tableName = "VisionSystemSim-" + visionSystemName; + SmartDashboard.putData(tableName + "/Sim Field", dbgField); + } + + /** Get one of the simulated cameras. */ + public Optional getCameraSim(String name) { + return Optional.ofNullable(camSimMap.get(name)); + } + + /** Get all the simulated cameras. */ + public Collection getCameraSims() { + return camSimMap.values(); + } + + /** + * Adds a simulated camera to this vision system with a specified robot-to-camera transformation. + * The vision targets registered with this vision system simulation will be observed by the + * simulated {@link PhotonCamera}. + * + * @param cameraSim The camera simulation + * @param robotToCamera The transform from the robot pose to the camera pose + */ + public void addCamera(PhotonCameraSim cameraSim, Transform3d robotToCamera) { + var existing = camSimMap.putIfAbsent(cameraSim.getCamera().getName(), cameraSim); + if (existing == null) { + camTrfMap.put(cameraSim, TimeInterpolatableBuffer.createBuffer(kBufferLengthSeconds)); + camTrfMap + .get(cameraSim) + .addSample(Timer.getFPGATimestamp(), new Pose3d().plus(robotToCamera)); + } + } + + /** Remove all simulated cameras from this vision system. */ + public void clearCameras() { + camSimMap.clear(); + camTrfMap.clear(); + } + + /** + * Remove a simulated camera from this vision system. + * + * @return If the camera was present and removed + */ + public boolean removeCamera(PhotonCameraSim cameraSim) { + boolean success = camSimMap.remove(cameraSim.getCamera().getName()) != null; + camTrfMap.remove(cameraSim); + return success; + } + + /** + * Get a simulated camera's position relative to the robot. If the requested camera is invalid, an + * empty optional is returned. + * + * @return The transform of this cameraSim, or an empty optional if it is invalid + */ + public Optional getRobotToCamera(PhotonCameraSim cameraSim) { + return getRobotToCamera(cameraSim, Timer.getFPGATimestamp()); + } + + /** + * Get a simulated camera's position relative to the robot. If the requested camera is invalid, an + * empty optional is returned. + * + * @param cameraSim Specific camera to get the robot-to-camera transform of + * @param timeSeconds Timestamp in seconds of when the transform should be observed + * @return The transform of this cameraSim, or an empty optional if it is invalid + */ + public Optional getRobotToCamera(PhotonCameraSim cameraSim, double timeSeconds) { + var trfBuffer = camTrfMap.get(cameraSim); + if (trfBuffer == null) return Optional.empty(); + var sample = trfBuffer.getSample(timeSeconds); + if (sample.isEmpty()) return Optional.empty(); + return Optional.of(new Transform3d(new Pose3d(), sample.orElse(new Pose3d()))); + } + + /** + * Adjust a camera's position relative to the robot. Use this if your camera is on a gimbal or + * turret or some other mobile platform. + * + * @param cameraSim The simulated camera to change the relative position of + * @param robotToCamera New transform from the robot to the camera + * @return If the cameraSim was valid and transform was adjusted + */ + public boolean adjustCamera(PhotonCameraSim cameraSim, Transform3d robotToCamera) { + var trfBuffer = camTrfMap.get(cameraSim); + if (trfBuffer == null) return false; + trfBuffer.addSample(Timer.getFPGATimestamp(), new Pose3d().plus(robotToCamera)); + return true; + } + + /** Reset the previous transforms for all cameras to their current transform. */ + public void resetCameraTransforms() { + for (var cam : camTrfMap.keySet()) resetCameraTransforms(cam); + } + + /** + * Reset the transform history for this camera to just the current transform. + * + * @return If the cameraSim was valid and transforms were reset + */ + public boolean resetCameraTransforms(PhotonCameraSim cameraSim) { + double now = Timer.getFPGATimestamp(); + var trfBuffer = camTrfMap.get(cameraSim); + if (trfBuffer == null) return false; + var lastTrf = new Transform3d(new Pose3d(), trfBuffer.getSample(now).orElse(new Pose3d())); + trfBuffer.clear(); + adjustCamera(cameraSim, lastTrf); + return true; + } + + public Set getVisionTargets() { + var all = new HashSet(); + for (var entry : targetSets.entrySet()) { + all.addAll(entry.getValue()); + } + return all; + } + + public Set getVisionTargets(String type) { + return targetSets.get(type); + } + + /** + * Adds targets on the field which your vision system is designed to detect. The {@link + * PhotonCamera}s simulated from this system will report the location of the camera relative to + * the subset of these targets which are visible from the given camera position. + * + *

By default these are added under the type "targets". + * + * @param targets Targets to add to the simulated field + */ + public void addVisionTargets(VisionTargetSim... targets) { + addVisionTargets("targets", targets); + } + + /** + * Adds targets on the field which your vision system is designed to detect. The {@link + * PhotonCamera}s simulated from this system will report the location of the camera relative to + * the subset of these targets which are visible from the given camera position. + * + *

The AprilTags from this layout will be added as vision targets under the type "apriltags". + * The poses added preserve the tag layout's current alliance origin. + * + * @param tagLayout The field tag layout to get Apriltag poses and IDs from + */ + public void addVisionTargets(AprilTagFieldLayout tagLayout) { + for (AprilTag tag : tagLayout.getTags()) { + addVisionTargets( + "apriltags", + new VisionTargetSim( + tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation + TargetModel.kTag16h5, + tag.ID)); + } + } + + /** + * Adds targets on the field which your vision system is designed to detect. The {@link + * PhotonCamera}s simulated from this system will report the location of the camera relative to + * the subset of these targets which are visible from the given camera position. + * + * @param type Type of target (e.g. "cargo"). + * @param targets Targets to add to the simulated field + */ + public void addVisionTargets(String type, VisionTargetSim... targets) { + targetSets.computeIfAbsent(type, k -> new HashSet<>()); + for (var tgt : targets) { + targetSets.get(type).add(tgt); + } + } + + public void clearVisionTargets() { + targetSets.clear(); + } + + public Set removeVisionTargets(String type) { + return targetSets.remove(type); + } + + public Set removeVisionTargets(VisionTargetSim... targets) { + var removeList = List.of(targets); + var removedSet = new HashSet(); + for (var entry : targetSets.entrySet()) { + entry + .getValue() + .removeIf( + t -> { + if (removeList.contains(t)) { + removedSet.add(t); + return true; + } else return false; + }); + } + return removedSet; + } + + /** Get the latest robot pose in meters saved by the vision system. */ + public Pose3d getRobotPose() { + return getRobotPose(Timer.getFPGATimestamp()); + } + + /** + * Get the robot pose in meters saved by the vision system at this timestamp. + * + * @param timestamp Timestamp of the desired robot pose + */ + public Pose3d getRobotPose(double timestamp) { + return robotPoseBuffer.getSample(timestamp).orElse(new Pose3d()); + } + + /** Clears all previous robot poses and sets robotPose at current time. */ + public void resetRobotPose(Pose2d robotPose) { + resetRobotPose(new Pose3d(robotPose)); + } + + /** Clears all previous robot poses and sets robotPose at current time. */ + public void resetRobotPose(Pose3d robotPose) { + robotPoseBuffer.clear(); + robotPoseBuffer.addSample(Timer.getFPGATimestamp(), robotPose); + } + + public Field2d getDebugField() { + return dbgField; + } + + /** + * Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically + * determine if a new frame should be submitted. + * + * @param robotPoseMeters The current robot pose in meters + */ + public void update(Pose2d robotPoseMeters) { + update(new Pose3d(robotPoseMeters)); + } + + /** + * Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically + * determine if a new frame should be submitted. + * + * @param robotPoseMeters The current robot pose in meters + */ + public void update(Pose3d robotPoseMeters) { + var targetTypes = targetSets.entrySet(); + // update vision targets on field + targetTypes.forEach( + entry -> + dbgField + .getObject(entry.getKey()) + .setPoses( + entry.getValue().stream() + .map(t -> t.getPose().toPose2d()) + .collect(Collectors.toList()))); + + if (robotPoseMeters == null) return; + + // save "real" robot poses over time + double now = Timer.getFPGATimestamp(); + robotPoseBuffer.addSample(now, robotPoseMeters); + dbgField.setRobotPose(robotPoseMeters.toPose2d()); + + var allTargets = new ArrayList(); + targetTypes.forEach((entry) -> allTargets.addAll(entry.getValue())); + var visibleTargets = new ArrayList(); + var cameraPose2ds = new ArrayList(); + // process each camera + for (var camSim : camSimMap.values()) { + // check if this camera is ready to process and get latency + var optTimestamp = camSim.consumeNextEntryTime(); + if (optTimestamp.isEmpty()) continue; + // when this result "was" read by NT + long timestampNT = optTimestamp.get(); + // this result's processing latency in milliseconds + double latencyMillis = camSim.prop.estLatencyMs(); + // the image capture timestamp in seconds of this result + double timestampCapture = timestampNT / 1e6 - latencyMillis / 1e3; + + // use camera pose from the image capture timestamp + Pose3d lateRobotPose = getRobotPose(timestampCapture); + Pose3d lateCameraPose = lateRobotPose.plus(getRobotToCamera(camSim).get()); + cameraPose2ds.add(lateCameraPose.toPose2d()); + + // process a PhotonPipelineResult with visible targets + var camResult = camSim.process(latencyMillis, lateCameraPose, allTargets); + // publish this info to NT at estimated timestamp of receive + camSim.submitProcessedFrame(camResult, timestampNT); + // display debug results + for (var target : camResult.getTargets()) { + visibleTargets.add(lateCameraPose.transformBy(target.getBestCameraToTarget())); + } + } + if (visibleTargets.size() != 0) { + dbgField + .getObject("visibleTargetPoses") + .setPoses(visibleTargets.stream().map(Pose3d::toPose2d).collect(Collectors.toList())); + } + if (cameraPose2ds.size() != 0) dbgField.getObject("cameras").setPoses(cameraPose2ds); + } +} diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionTargetSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionTargetSim.java new file mode 100644 index 000000000..88d19756c --- /dev/null +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionTargetSim.java @@ -0,0 +1,94 @@ +/* + * 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.simulation; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation3d; +import java.util.List; +import org.photonvision.estimation.TargetModel; + +/** Describes a vision target located somewhere on the field that your vision system can detect. */ +public class VisionTargetSim { + private Pose3d pose; + private TargetModel model; + + public final int fiducialID; + + /** + * Describes a vision target located somewhere on the field that your vision system can detect. + * + * @param pose Pose3d of the tag in field-relative coordinates + * @param model TargetModel which describes the shape of the target + */ + public VisionTargetSim(Pose3d pose, TargetModel model) { + this.pose = pose; + this.model = model; + this.fiducialID = -1; + } + + /** + * Describes a fiducial tag located somewhere on the field that your vision system can detect. + * + * @param pose Pose3d of the tag in field-relative coordinates + * @param model TargetModel which describes the shape of the target(tag) + * @param id The ID of this fiducial tag + */ + public VisionTargetSim(Pose3d pose, TargetModel model, int id) { + this.pose = pose; + this.model = model; + this.fiducialID = id; + } + + public void setPose(Pose3d pose) { + this.pose = pose; + } + + public void setModel(TargetModel model) { + this.model = model; + } + + public Pose3d getPose() { + return pose; + } + + public TargetModel getModel() { + return model; + } + + /** This target's vertices offset from its field pose. */ + public List getFieldVertices() { + return model.getFieldVertices(pose); + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj instanceof VisionTargetSim) { + var o = (VisionTargetSim) obj; + return pose.equals(o.pose) && model.equals(o.model); + } + return false; + } +} diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00000.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00000.png new file mode 100644 index 000000000..f6f22a9c9 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00000.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00001.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00001.png new file mode 100644 index 000000000..9b66b7d59 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00001.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00002.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00002.png new file mode 100644 index 000000000..856d4f163 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00002.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00003.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00003.png new file mode 100644 index 000000000..df0cc4ab3 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00003.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00004.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00004.png new file mode 100644 index 000000000..b5537da0b Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00004.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00005.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00005.png new file mode 100644 index 000000000..13574f8d5 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00005.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00006.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00006.png new file mode 100644 index 000000000..6c8b797ca Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00006.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00007.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00007.png new file mode 100644 index 000000000..e0d18241d Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00007.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00008.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00008.png new file mode 100644 index 000000000..30827b6f9 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00008.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00009.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00009.png new file mode 100644 index 000000000..702990646 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00009.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00010.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00010.png new file mode 100644 index 000000000..71beaa962 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00010.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00011.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00011.png new file mode 100644 index 000000000..06e0c94a3 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00011.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00012.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00012.png new file mode 100644 index 000000000..11a9a0016 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00012.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00013.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00013.png new file mode 100644 index 000000000..7443d8030 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00013.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00014.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00014.png new file mode 100644 index 000000000..bad5efedc Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00014.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00015.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00015.png new file mode 100644 index 000000000..3b4f564d4 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00015.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00016.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00016.png new file mode 100644 index 000000000..cf5fa68c2 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00016.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00017.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00017.png new file mode 100644 index 000000000..13ed6b2cc Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00017.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00018.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00018.png new file mode 100644 index 000000000..27ccc3324 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00018.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00019.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00019.png new file mode 100644 index 000000000..97a521c47 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00019.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00020.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00020.png new file mode 100644 index 000000000..f7c1f0d4e Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00020.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00021.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00021.png new file mode 100644 index 000000000..aade7b332 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00021.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00022.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00022.png new file mode 100644 index 000000000..ae9c0be5e Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00022.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00023.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00023.png new file mode 100644 index 000000000..d4122e600 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00023.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00024.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00024.png new file mode 100644 index 000000000..46f9c9812 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00024.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00025.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00025.png new file mode 100644 index 000000000..29d3bf8aa Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00025.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00026.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00026.png new file mode 100644 index 000000000..4e8654129 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00026.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00027.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00027.png new file mode 100644 index 000000000..a14c4a733 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00027.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00028.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00028.png new file mode 100644 index 000000000..5adc09082 Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00028.png differ diff --git a/photon-lib/src/main/resources/images/apriltags/tag16_05_00029.png b/photon-lib/src/main/resources/images/apriltags/tag16_05_00029.png new file mode 100644 index 000000000..9639d1cec Binary files /dev/null and b/photon-lib/src/main/resources/images/apriltags/tag16_05_00029.png differ diff --git a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java new file mode 100644 index 000000000..594439e84 --- /dev/null +++ b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java @@ -0,0 +1,229 @@ +/* + * 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 static org.junit.jupiter.api.Assertions.*; + +import edu.wpi.first.apriltag.jni.AprilTagJNI; +import edu.wpi.first.cscore.CameraServerCvJNI; +import edu.wpi.first.cscore.CameraServerJNI; +import edu.wpi.first.hal.JNIWrapper; +import edu.wpi.first.math.MathUtil; +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.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +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.RuntimeLoader; +import edu.wpi.first.util.WPIUtilJNI; +import java.util.List; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.Test; +import org.opencv.core.Core; +import org.photonvision.estimation.CameraTargetRelation; +import org.photonvision.estimation.OpenCVHelp; +import org.photonvision.estimation.TargetModel; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.simulation.VisionTargetSim; + +public class OpenCVTest { + private static final double kTrlDelta = 0.005; + private static final double kRotDeltaDeg = 0.25; + + public static void assertSame(Translation3d trl1, Translation3d trl2) { + assertEquals(0, trl1.getX() - trl2.getX(), kTrlDelta, "Trl X Diff"); + assertEquals(0, trl1.getY() - trl2.getY(), kTrlDelta, "Trl Y Diff"); + assertEquals(0, trl1.getZ() - trl2.getZ(), kTrlDelta, "Trl Z Diff"); + } + + public static void assertSame(Rotation3d rot1, Rotation3d rot2) { + assertEquals(0, MathUtil.angleModulus(rot1.getX() - rot2.getX()), kRotDeltaDeg, "Rot X Diff"); + assertEquals(0, MathUtil.angleModulus(rot1.getY() - rot2.getY()), kRotDeltaDeg, "Rot Y Diff"); + assertEquals(0, MathUtil.angleModulus(rot1.getZ() - rot2.getZ()), kRotDeltaDeg, "Rot Z Diff"); + assertEquals( + 0, MathUtil.angleModulus(rot1.getAngle() - rot2.getAngle()), kRotDeltaDeg, "Rot W Diff"); + } + + public static void assertSame(Pose3d pose1, Pose3d pose2) { + assertSame(pose1.getTranslation(), pose2.getTranslation()); + assertSame(pose1.getRotation(), pose2.getRotation()); + } + + public static void assertSame(Transform3d trf1, Transform3d trf2) { + assertSame(trf1.getTranslation(), trf2.getTranslation()); + assertSame(trf1.getRotation(), trf2.getRotation()); + } + + private static final SimCameraProperties prop = new SimCameraProperties(); + + @BeforeAll + public static void setUp() { + JNIWrapper.Helper.setExtractOnStaticLoad(false); + WPIUtilJNI.Helper.setExtractOnStaticLoad(false); + NetworkTablesJNI.Helper.setExtractOnStaticLoad(false); + WPINetJNI.Helper.setExtractOnStaticLoad(false); + CameraServerJNI.Helper.setExtractOnStaticLoad(false); + CameraServerCvJNI.Helper.setExtractOnStaticLoad(false); + AprilTagJNI.Helper.setExtractOnStaticLoad(false); + + try { + CombinedRuntimeLoader.loadLibraries( + VisionSystemSim.class, + "wpiutiljni", + "ntcorejni", + "wpinetjni", + "wpiHaljni", + "cscorejni", + "cscorejnicvstatic"); + + var loader = + new RuntimeLoader<>( + Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class); + loader.loadLibrary(); + } catch (Exception e) { + e.printStackTrace(); + } + + // NT live for debug purposes + NetworkTableInstance.getDefault().startServer(); + + // No version check for testing + PhotonCamera.setVersionCheckEnabled(false); + } + + @Test + public void testTrlConvert() { + var trl = new Translation3d(0.75, -0.4, 0.1); + var tvec = OpenCVHelp.translationToTvec(trl); + var result = OpenCVHelp.tvecToTranslation(tvec); + tvec.release(); + assertSame(trl, result); + } + + @Test + public void testRotConvert() { + var rot = new Rotation3d(0.5, 1, -1); + var rvec = OpenCVHelp.rotationToRvec(rot); + var result = OpenCVHelp.rvecToRotation(rvec); + rvec.release(); + var diff = result.minus(rot); + assertSame(new Rotation3d(), diff); + } + + @Test + public void testProjection() { + var target = + new VisionTargetSim( + new Pose3d(1, 0, 0, new Rotation3d(0, 0, Math.PI)), TargetModel.kTag16h5, 0); + var cameraPose = new Pose3d(0, 0, 0, new Rotation3d()); + var targetCorners = + OpenCVHelp.projectPoints( + prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices()); + // find circulation (counter/clockwise-ness) + double circulation = 0; + for (int i = 0; i < targetCorners.size(); i++) { + double xDiff = targetCorners.get((i + 1) % 4).x - targetCorners.get(i).x; + double ySum = targetCorners.get((i + 1) % 4).y + targetCorners.get(i).y; + circulation += xDiff * ySum; + } + assertTrue(circulation > 0, "2d fiducial points aren't counter-clockwise"); + // undo projection distortion + targetCorners = prop.undistort(targetCorners); + var avgCenterRot1 = prop.getPixelRot(OpenCVHelp.averageCorner(targetCorners)); + cameraPose = + cameraPose.plus(new Transform3d(new Translation3d(), new Rotation3d(0, 0.25, 0.25))); + targetCorners = + OpenCVHelp.projectPoints( + prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices()); + var avgCenterRot2 = prop.getPixelRot(OpenCVHelp.averageCorner(targetCorners)); + var yaw2d = new Rotation2d(avgCenterRot2.getZ()); + var pitch2d = new Rotation2d(avgCenterRot2.getY()); + var yawDiff = yaw2d.minus(new Rotation2d(avgCenterRot1.getZ())); + var pitchDiff = pitch2d.minus(new Rotation2d(avgCenterRot1.getY())); + assertTrue(yawDiff.getRadians() < 0, "2d points don't follow yaw"); + assertTrue(pitchDiff.getRadians() < 0, "2d points don't follow pitch"); + var actualRelation = new CameraTargetRelation(cameraPose, target.getPose()); + assertEquals( + actualRelation.camToTargPitch.getDegrees(), + pitchDiff.getDegrees() * Math.cos(yaw2d.getRadians()), // adjust for perpsective distortion + kRotDeltaDeg, + "2d pitch doesn't match 3d"); + assertEquals( + actualRelation.camToTargYaw.getDegrees(), + yawDiff.getDegrees(), + kRotDeltaDeg, + "2d yaw doesn't match 3d"); + } + + @Test + public void testSolvePNP_SQUARE() { + var target = + new VisionTargetSim( + new Pose3d(5, 0.5, 1, new Rotation3d(0, 0, Math.PI)), TargetModel.kTag16h5, 0); + var cameraPose = new Pose3d(0, 0, 0, new Rotation3d()); + var actualRelation = new CameraTargetRelation(cameraPose, target.getPose()); + var targetCorners = + OpenCVHelp.projectPoints( + prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices()); + var pnpSim = + OpenCVHelp.solvePNP_SQUARE( + prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners); + var estRelation = new CameraTargetRelation(cameraPose, cameraPose.plus(pnpSim.best)); + assertSame(actualRelation.camToTarg, estRelation.camToTarg); + } + + @Test + public void testSolvePNP_SQPNP() { + var target = + new VisionTargetSim( + new Pose3d(5, 0.5, 1, new Rotation3d(0, 0, Math.PI)), + new TargetModel( + List.of( + new Translation3d(0, 0, 0), + new Translation3d(1, 0, 0), + new Translation3d(0, 1, 0), + new Translation3d(0, 0, 1), + new Translation3d(0.125, 0.25, 0.5), + new Translation3d(0, 0, -1), + new Translation3d(0, -1, 0), + new Translation3d(-1, 0, 0))), + 0); + var cameraPose = new Pose3d(0, 0, 0, new Rotation3d()); + var actualRelation = new CameraTargetRelation(cameraPose, target.getPose()); + var targetCorners = + OpenCVHelp.projectPoints( + prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, target.getFieldVertices()); + var pnpSim = + OpenCVHelp.solvePNP_SQPNP( + prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners); + var estRelation = new CameraTargetRelation(cameraPose, cameraPose.plus(pnpSim.best)); + assertSame(actualRelation.camToTarg, estRelation.camToTarg); + } +} diff --git a/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java similarity index 53% rename from photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java rename to photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index 524e8eff5..f4d5806e4 100644 --- a/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -28,6 +28,9 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertTrue; +import edu.wpi.first.apriltag.jni.AprilTagJNI; +import edu.wpi.first.cscore.CameraServerCvJNI; +import edu.wpi.first.cscore.CameraServerJNI; import edu.wpi.first.hal.JNIWrapper; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -41,6 +44,7 @@ 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.RuntimeLoader; import edu.wpi.first.util.WPIUtilJNI; import java.util.List; import java.util.stream.Stream; @@ -52,18 +56,26 @@ import org.junit.jupiter.params.ParameterizedTest; import org.junit.jupiter.params.provider.Arguments; import org.junit.jupiter.params.provider.MethodSource; import org.junit.jupiter.params.provider.ValueSource; +import org.opencv.core.Core; +import org.photonvision.estimation.TargetModel; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.simulation.VisionTargetSim; import org.photonvision.targeting.PhotonTrackedTarget; -class SimVisionSystemTest { +class VisionSystemSimTest { + private static final double kTrlDelta = 0.005; + private static final double kRotDeltaDeg = 0.25; + @Test public void testEmpty() { Assertions.assertDoesNotThrow( () -> { - var sysUnderTest = - new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 320, 240, 0); - sysUnderTest.addSimVisionTarget(new SimVisionTarget(new Pose3d(), 1.0, 1.0, 42)); + var sysUnderTest = new VisionSystemSim("Test"); + sysUnderTest.addVisionTargets( + new VisionTargetSim(new Pose3d(), new TargetModel(1.0, 1.0))); for (int loopIdx = 0; loopIdx < 100; loopIdx++) { - sysUnderTest.processFrame(new Pose2d()); + sysUnderTest.update(new Pose2d()); } }); } @@ -74,12 +86,25 @@ class SimVisionSystemTest { WPIUtilJNI.Helper.setExtractOnStaticLoad(false); NetworkTablesJNI.Helper.setExtractOnStaticLoad(false); WPINetJNI.Helper.setExtractOnStaticLoad(false); + CameraServerJNI.Helper.setExtractOnStaticLoad(false); + CameraServerCvJNI.Helper.setExtractOnStaticLoad(false); + AprilTagJNI.Helper.setExtractOnStaticLoad(false); try { CombinedRuntimeLoader.loadLibraries( - SimVisionSystem.class, "wpiutiljni", "ntcorejni", "wpinetjni", "wpiHaljni"); + VisionSystemSim.class, + "wpiutiljni", + "ntcorejni", + "wpinetjni", + "wpiHaljni", + "cscorejni", + "cscorejnicvstatic"); + + var loader = + new RuntimeLoader<>( + Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class); + loader.loadLibrary(); } catch (Exception e) { - // TODO Auto-generated catch block e.printStackTrace(); } @@ -114,66 +139,74 @@ class SimVisionSystemTest { public void testVisibilityCupidShuffle() { final var targetPose = new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI)); - var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 0); - sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3)); + var visionSysSim = new VisionSystemSim("Test"); + var camera = new PhotonCamera("camera"); + var cameraSim = new PhotonCameraSim(camera); + visionSysSim.addCamera(cameraSim, new Transform3d()); + cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(1.0, 3.0), 3)); // To the right, to the right var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-70)); - sysUnderTest.processFrame(robotPose); - assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // To the right, to the right robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-95)); - sysUnderTest.processFrame(robotPose); - assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // To the left, to the left robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(90)); - sysUnderTest.processFrame(robotPose); - assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // To the left, to the left robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(65)); - sysUnderTest.processFrame(robotPose); - assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // now kick, now kick robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(5)); - sysUnderTest.processFrame(robotPose); - assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); // now kick, now kick robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-5)); - sysUnderTest.processFrame(robotPose); - assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); // now walk it by yourself robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-179)); - sysUnderTest.processFrame(robotPose); - assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // now walk it by yourself - sysUnderTest.moveCamera(new Transform3d(new Translation3d(), new Rotation3d(0, 0, Math.PI))); - sysUnderTest.processFrame(robotPose); - assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.adjustCamera( + cameraSim, new Transform3d(new Translation3d(), new Rotation3d(0, 0, Math.PI))); + visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); } @Test public void testNotVisibleVert1() { final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI)); - var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 0); - sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3)); + var visionSysSim = new VisionSystemSim("Test"); + var camera = new PhotonCamera("camera"); + var cameraSim = new PhotonCameraSim(camera); + visionSysSim.addCamera(cameraSim, new Transform3d()); + cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(1.0, 3.0), 3)); var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(5)); - sysUnderTest.processFrame(robotPose); - assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); - sysUnderTest.moveCamera( - new Transform3d( - new Translation3d(0, 0, 5000), new Rotation3d(0, 0, Math.PI))); // vooop selfie stick - sysUnderTest.processFrame(robotPose); - assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.adjustCamera( // vooop selfie stick + cameraSim, new Transform3d(new Translation3d(0, 0, 5000), new Rotation3d(0, 0, Math.PI))); + visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); } @Test @@ -181,50 +214,65 @@ class SimVisionSystemTest { final var targetPose = new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI)); var robotToCamera = - new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, Math.PI / 4, 0)); - var sysUnderTest = new SimVisionSystem("Test", 80.0, robotToCamera, 99999, 1234, 1234, 0); - sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 3.0, 0.5, 1736)); + new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, -Math.PI / 4, 0)); + var visionSysSim = new VisionSystemSim("Test"); + var camera = new PhotonCamera("camera"); + var cameraSim = new PhotonCameraSim(camera); + visionSysSim.addCamera(cameraSim, robotToCamera); + cameraSim.prop.setCalibration(1234, 1234, Rotation2d.fromDegrees(80)); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(1.0, 0.5), 1736)); - var robotPose = new Pose2d(new Translation2d(14.98, 0), Rotation2d.fromDegrees(5)); - sysUnderTest.processFrame(robotPose); - assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); + var robotPose = new Pose2d(new Translation2d(13.98, 0), Rotation2d.fromDegrees(5)); + visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); // Pitched back camera should mean target goes out of view below the robot as distance increases robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5)); - sysUnderTest.processFrame(robotPose); - assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); } @Test public void testNotVisibleTgtSize() { final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI)); - var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 20.0); - sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.1, 0.025, 24)); + var visionSysSim = new VisionSystemSim("Test"); + var camera = new PhotonCamera("camera"); + var cameraSim = new PhotonCameraSim(camera); + visionSysSim.addCamera(cameraSim, new Transform3d()); + cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); + cameraSim.setMinTargetAreaPixels(20.0); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.1, 0.025), 24)); var robotPose = new Pose2d(new Translation2d(12, 0), Rotation2d.fromDegrees(5)); - sysUnderTest.processFrame(robotPose); - assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5)); - sysUnderTest.processFrame(robotPose); - assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); } @Test public void testNotVisibleTooFarForLEDs() { final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI)); - var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 10, 640, 480, 1.0); - sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 0.25, 78)); + var visionSysSim = new VisionSystemSim("Test"); + var camera = new PhotonCamera("camera"); + var cameraSim = new PhotonCameraSim(camera); + visionSysSim.addCamera(cameraSim, new Transform3d()); + cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); + cameraSim.setMaxSightRange(10); + cameraSim.setMinTargetAreaPixels(1.0); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(1.0, 0.25), 78)); var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(5)); - sysUnderTest.processFrame(robotPose); - assertTrue(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5)); - sysUnderTest.processFrame(robotPose); - assertFalse(sysUnderTest.cam.getLatestResult().hasTargets()); + visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); } @ParameterizedTest @@ -232,34 +280,43 @@ class SimVisionSystemTest { public void testYawAngles(double testYaw) { final var targetPose = new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, 3 * Math.PI / 4)); - var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 0); - sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 3)); + var visionSysSim = new VisionSystemSim("Test"); + var camera = new PhotonCamera("camera"); + var cameraSim = new PhotonCameraSim(camera); + visionSysSim.addCamera(cameraSim, new Transform3d()); + cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); + cameraSim.setMinTargetAreaPixels(0.0); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.5, 0.5), 3)); var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(-1.0 * testYaw)); - sysUnderTest.processFrame(robotPose); - var res = sysUnderTest.cam.getLatestResult(); + visionSysSim.update(robotPose); + var res = camera.getLatestResult(); assertTrue(res.hasTargets()); var tgt = res.getBestTarget(); - assertEquals(tgt.getYaw(), testYaw, 0.0001); + assertEquals(tgt.getYaw(), testYaw, kRotDeltaDeg); } @ParameterizedTest @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999}) public void testCameraPitch(double testPitch) { - testPitch = testPitch * -1; - final var targetPose = new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, 3 * Math.PI / 4)); final var robotPose = new Pose2d(new Translation2d(10, 0), new Rotation2d(0)); - var sysUnderTest = new SimVisionSystem("Test", 120.0, new Transform3d(), 99999, 640, 480, 0); - sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 23)); + var visionSysSim = new VisionSystemSim("Test"); + var camera = new PhotonCamera("camera"); + var cameraSim = new PhotonCameraSim(camera); + visionSysSim.addCamera(cameraSim, new Transform3d()); + cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(120)); + cameraSim.setMinTargetAreaPixels(0.0); + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.5, 0.5), 23)); // Transform is now robot -> camera - sysUnderTest.moveCamera( + visionSysSim.adjustCamera( + cameraSim, new Transform3d( new Translation3d(), new Rotation3d(0, Units.degreesToRadians(testPitch), 0))); - sysUnderTest.processFrame(robotPose); - var res = sysUnderTest.cam.getLatestResult(); + visionSysSim.update(robotPose); + var res = camera.getLatestResult(); assertTrue(res.hasTargets()); var tgt = res.getBestTarget(); @@ -267,29 +324,29 @@ class SimVisionSystemTest { // the // lower half of the image // which should produce negative pitch. - assertEquals(testPitch * -1, tgt.getPitch(), 0.0001); + assertEquals(testPitch, tgt.getPitch(), kRotDeltaDeg); } private static Stream distCalCParamProvider() { // Arbitrary and fairly random assortment of distances, camera pitches, and heights return Stream.of( - Arguments.of(5, 15.98, 0), - Arguments.of(6, 15.98, 1), - Arguments.of(10, 15.98, 0), - Arguments.of(15, 15.98, 2), - Arguments.of(19.95, 15.98, 0), - Arguments.of(20, 15.98, 0), - Arguments.of(5, 42, 1), - Arguments.of(6, 42, 0), - Arguments.of(10, 42, 2), - Arguments.of(15, 42, 0.5), - Arguments.of(19.42, 15.98, 0), - Arguments.of(20, 42, 0), - Arguments.of(5, 55, 2), - Arguments.of(6, 55, 0), - Arguments.of(10, 54, 2.2), - Arguments.of(15, 53, 0), - Arguments.of(19.52, 15.98, 1.1)); + Arguments.of(5, -15.98, 0), + Arguments.of(6, -15.98, 1), + Arguments.of(10, -15.98, 0), + Arguments.of(15, -15.98, 2), + Arguments.of(19.95, -15.98, 0), + Arguments.of(20, -15.98, 0), + Arguments.of(5, -42, 1), + Arguments.of(6, -42, 0), + Arguments.of(10, -42, 2), + Arguments.of(15, -42, 0.5), + Arguments.of(19.42, -15.98, 0), + Arguments.of(20, -42, 0), + Arguments.of(5, -35, 2), + Arguments.of(6, -35, 0), + Arguments.of(10, -34, 3.2), + Arguments.of(15, -33, 0), + Arguments.of(19.52, -15.98, 1.1)); } @ParameterizedTest @@ -306,29 +363,32 @@ class SimVisionSystemTest { new Translation3d(0, 0, Units.feetToMeters(testHeight)), new Rotation3d(0, Units.degreesToRadians(testPitch), 0)); - var sysUnderTest = - new SimVisionSystem( - "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!", - 160.0, - robotToCamera, - 99999, - 640, - 480, - 0); - sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 0)); + var visionSysSim = + new VisionSystemSim( + "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!"); + var camera = new PhotonCamera("camera"); + var cameraSim = new PhotonCameraSim(camera); + visionSysSim.addCamera(cameraSim, new Transform3d()); + cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(160)); + cameraSim.setMinTargetAreaPixels(0.0); + visionSysSim.adjustCamera(cameraSim, robotToCamera); + // note that non-fiducial targets have different center point calculation and will + // return slightly inaccurate yaw/pitch values + visionSysSim.addVisionTargets(new VisionTargetSim(targetPose, new TargetModel(0.5, 0.5), 0)); - sysUnderTest.processFrame(robotPose); - var res = sysUnderTest.cam.getLatestResult(); + visionSysSim.update(robotPose); + var res = camera.getLatestResult(); assertTrue(res.hasTargets()); var tgt = res.getBestTarget(); - assertEquals(tgt.getYaw(), 0.0, 0.0001); + assertEquals(0.0, tgt.getYaw(), kRotDeltaDeg); + double distMeas = PhotonUtils.calculateDistanceToTargetMeters( robotToCamera.getZ(), targetPose.getZ(), - Units.degreesToRadians(testPitch), + Units.degreesToRadians(-testPitch), Units.degreesToRadians(tgt.getPitch())); - assertEquals(Units.feetToMeters(testDist), distMeas, 0.001); + assertEquals(Units.feetToMeters(testDist), distMeas, kTrlDelta); } @Test @@ -339,92 +399,87 @@ class SimVisionSystemTest { new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, Math.PI)); final var targetPoseR = new Pose3d(new Translation3d(15.98, -2, 0), new Rotation3d(0, 0, Math.PI)); - var sysUnderTest = new SimVisionSystem("Test", 160.0, new Transform3d(), 99999, 640, 480, 20.0); - sysUnderTest.addSimVisionTarget( - new SimVisionTarget( + var visionSysSim = new VisionSystemSim("Test"); + var camera = new PhotonCamera("camera"); + var cameraSim = new PhotonCameraSim(camera); + visionSysSim.addCamera(cameraSim, new Transform3d()); + cameraSim.prop.setCalibration(640, 480, Rotation2d.fromDegrees(80)); + cameraSim.setMinTargetAreaPixels(20.0); + + visionSysSim.addVisionTargets( + new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), - 0.25, - 0.25, + TargetModel.kTag16h5, 1)); - sysUnderTest.addSimVisionTarget( - new SimVisionTarget( + visionSysSim.addVisionTargets( + new VisionTargetSim( targetPoseC.transformBy( new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), - 0.25, - 0.25, + TargetModel.kTag16h5, 2)); - sysUnderTest.addSimVisionTarget( - new SimVisionTarget( + visionSysSim.addVisionTargets( + new VisionTargetSim( targetPoseR.transformBy( new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())), - 0.25, - 0.25, + TargetModel.kTag16h5, 3)); - sysUnderTest.addSimVisionTarget( - new SimVisionTarget( + visionSysSim.addVisionTargets( + new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), - 0.25, - 0.25, + TargetModel.kTag16h5, 4)); - sysUnderTest.addSimVisionTarget( - new SimVisionTarget( + visionSysSim.addVisionTargets( + new VisionTargetSim( targetPoseC.transformBy( new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), - 0.25, - 0.25, + TargetModel.kTag16h5, 5)); - sysUnderTest.addSimVisionTarget( - new SimVisionTarget( - targetPoseR.transformBy( + visionSysSim.addVisionTargets( + new VisionTargetSim( + targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())), - 0.25, - 0.25, + TargetModel.kTag16h5, 6)); - sysUnderTest.addSimVisionTarget( - new SimVisionTarget( + visionSysSim.addVisionTargets( + new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 0.50), new Rotation3d())), - 0.25, - 0.25, + TargetModel.kTag16h5, 7)); - sysUnderTest.addSimVisionTarget( - new SimVisionTarget( + visionSysSim.addVisionTargets( + new VisionTargetSim( targetPoseC.transformBy( new Transform3d(new Translation3d(0, 0, 0.50), new Rotation3d())), - 0.25, - 0.25, + TargetModel.kTag16h5, 8)); - sysUnderTest.addSimVisionTarget( - new SimVisionTarget( + visionSysSim.addVisionTargets( + new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 0.75), new Rotation3d())), - 0.25, - 0.25, + TargetModel.kTag16h5, 9)); - sysUnderTest.addSimVisionTarget( - new SimVisionTarget( + visionSysSim.addVisionTargets( + new VisionTargetSim( targetPoseR.transformBy( new Transform3d(new Translation3d(0, 0, 0.75), new Rotation3d())), - 0.25, - 0.25, + TargetModel.kTag16h5, 10)); - sysUnderTest.addSimVisionTarget( - new SimVisionTarget( + visionSysSim.addVisionTargets( + new VisionTargetSim( targetPoseL.transformBy( new Transform3d(new Translation3d(0, 0, 0.25), new Rotation3d())), - 0.25, - 0.25, + TargetModel.kTag16h5, 11)); var robotPose = new Pose2d(new Translation2d(6.0, 0), Rotation2d.fromDegrees(0.25)); - sysUnderTest.processFrame(robotPose); - var res = sysUnderTest.cam.getLatestResult(); + visionSysSim.update(robotPose); + var res = camera.getLatestResult(); assertTrue(res.hasTargets()); List tgtList; tgtList = res.getTargets(); - assertEquals(tgtList.size(), 11); + assertEquals(11, tgtList.size()); } } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 908ca24e6..a15fca87d 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -118,14 +118,14 @@ public class PhotonTrackedTarget { * Return a list of the n corners in image space (origin top left, x right, y down), in no * particular order, detected for this target. * - *

For fiducials, the order is known and is always counter-clock wise around the tag, like so + *

For fiducials, the order is known and is always counter-clock wise around the tag, like so: * - *

spotless:off - * -> +X 3 ----- 2 + * 

​
+     * ⟶ +X  3 ----- 2
      * |      |       |
      * V      |       |
      * +Y     0 ----- 1
-     * spotless:on
+     *
*/ public List getDetectedCorners() { return detectedCorners; diff --git a/photonlib-cpp-examples/apriltagExample/gradlew b/photonlib-cpp-examples/apriltagExample/gradlew old mode 100755 new mode 100644 diff --git a/photonlib-java-examples/apriltagExample/simgui-ds.json b/photonlib-java-examples/apriltagExample/simgui-ds.json index 69b1a3cbc..25e298be7 100644 --- a/photonlib-java-examples/apriltagExample/simgui-ds.json +++ b/photonlib-java-examples/apriltagExample/simgui-ds.json @@ -1,11 +1,13 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, "keyboardJoysticks": [ { "axisConfig": [ - { - "decKey": 65, - "incKey": 68 - }, + {}, { "decKey": 87, "incKey": 83 @@ -15,9 +17,14 @@ "decayRate": 0.0, "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 65, + "incKey": 68 } ], - "axisCount": 3, + "axisCount": 5, "buttonCount": 4, "buttonKeys": [ 90, diff --git a/photonlib-java-examples/apriltagExample/simgui.json b/photonlib-java-examples/apriltagExample/simgui.json index ec51dae2b..5c06de417 100644 --- a/photonlib-java-examples/apriltagExample/simgui.json +++ b/photonlib-java-examples/apriltagExample/simgui.json @@ -1,20 +1,46 @@ { + "HALProvider": { + "Other Devices": { + "AnalogGyro[0]": { + "header": { + "open": true + } + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", - "/SmartDashboard/Field": "Field2d" + "/LiveWindow/Ungrouped/AnalogGyro[0]": "Gyro", + "/LiveWindow/Ungrouped/MotorControllerGroup[1]": "Motor Controller", + "/LiveWindow/Ungrouped/MotorControllerGroup[2]": "Motor Controller", + "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", + "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/VisionSystemSim-Test/Sim Field": "Field2d", + "/SmartDashboard/VisionSystemSim-YOUR CAMERA NAME/Sim Field": "Field2d" }, "windows": { "/SmartDashboard/Field": { "window": { "visible": true } + }, + "/SmartDashboard/VisionSystemSim-Test/Sim Field": { + "window": { + "visible": true + } + }, + "/SmartDashboard/VisionSystemSim-YOUR CAMERA NAME/Sim Field": { + "window": { + "visible": true + } } } }, "NetworkTables": { "transitory": { - "Shuffleboard": { + "LiveWindow": { "open": true }, "photonvision": { @@ -24,6 +50,9 @@ "USB_Camera": { "open": true }, + "YOUR CAMERA NAME": { + "open": true + }, "open": true, "testCamera": { "open": true diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java index b470437ee..f33d9f051 100644 --- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java +++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java @@ -38,6 +38,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; @@ -50,6 +51,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.DriveTrainConstants; import java.util.Optional; import org.photonvision.EstimatedRobotPose; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; /** Represents a differential drive style drivetrain. */ public class Drivetrain { @@ -94,7 +98,7 @@ public class Drivetrain { private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder); private final Field2d m_fieldSim = new Field2d(); private final LinearSystem m_drivetrainSystem = - LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); + LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.8); private final DifferentialDrivetrainSim m_drivetrainSimulator = new DifferentialDrivetrainSim( m_drivetrainSystem, @@ -104,6 +108,9 @@ public class Drivetrain { DriveTrainConstants.kWheelRadius, null); + // Simulated PhotonCamera -- only used in sim! + VisionSystemSim m_visionSystemSim; + /** * Constructs a differential drive object. Sets the encoder distance per pulse and resets the * gyro. @@ -128,6 +135,15 @@ public class Drivetrain { m_rightEncoder.reset(); SmartDashboard.putData("Field", m_fieldSim); + + // Only simulate our PhotonCamera in simulation + if (RobotBase.isSimulation()) { + m_visionSystemSim = new VisionSystemSim(Constants.VisionConstants.cameraName); + var cameraSim = + new PhotonCameraSim(pcw.photonCamera, SimCameraProperties.PI4_LIFECAM_640_480()); + m_visionSystemSim.addCamera(cameraSim, Constants.VisionConstants.robotToCam); + m_visionSystemSim.addVisionTargets(pcw.photonPoseEstimator.getFieldTags()); + } } /** @@ -174,6 +190,9 @@ public class Drivetrain { m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees()); + + // Update results from vision as well + m_visionSystemSim.update(m_drivetrainSimulator.getPose()); } /** Updates the field-relative position. */ diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java index a29737a0f..d46957886 100644 --- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java +++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java @@ -37,8 +37,8 @@ import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; public class PhotonCameraWrapper { - private PhotonCamera photonCamera; - private PhotonPoseEstimator photonPoseEstimator; + PhotonCamera photonCamera; + PhotonPoseEstimator photonPoseEstimator; public PhotonCameraWrapper() { // Change the name of your camera here to whatever it is in the PhotonVision UI. diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java index 8f9327457..78f0f50ab 100644 --- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Robot.java @@ -42,6 +42,9 @@ public class Robot extends TimedRobot { public void robotInit() { if (Robot.isSimulation()) { NetworkTableInstance instance = NetworkTableInstance.getDefault(); + + // We have to have Photon running and set to NT server mode for it to connect + // to our computer instead of to a roboRIO. instance.stopServer(); // set the NT server if simulating this code. // "localhost" for photon on desktop, or "photonvision.local" / "[ip-address]" for coprocessor @@ -51,6 +54,10 @@ public class Robot extends TimedRobot { m_controller = new XboxController(0); m_drive = new Drivetrain(); + + // Flush NetworkTables every loop. This ensures that robot pose and other values + // are sent during every iteration. This only applies to local NT connections! + setNetworkTablesFlushEnabled(true); } @Override diff --git a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java index 6675b89ca..2686a202b 100644 --- a/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java +++ b/photonlib-java-examples/simaimandrange/src/main/java/frc/robot/sim/DrivetrainSim.java @@ -41,8 +41,8 @@ import edu.wpi.first.wpilibj.simulation.PWMSim; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; -import org.photonvision.SimVisionSystem; -import org.photonvision.SimVisionTarget; +import org.photonvision.simulation.SimVisionSystem; +import org.photonvision.simulation.SimVisionTarget; /** * Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated diff --git a/photonlib-java-examples/simposeest/src/main/java/frc/robot/Constants.java b/photonlib-java-examples/simposeest/src/main/java/frc/robot/Constants.java index d3d7fdf7a..e5e8ca9ca 100644 --- a/photonlib-java-examples/simposeest/src/main/java/frc/robot/Constants.java +++ b/photonlib-java-examples/simposeest/src/main/java/frc/robot/Constants.java @@ -30,7 +30,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.util.Units; -import org.photonvision.SimVisionTarget; +import org.photonvision.simulation.SimVisionTarget; /** * Holding class for all physical constants that must be used throughout the codebase. These values diff --git a/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java b/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java index 2eeaaa106..7d1eee2d4 100644 --- a/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java +++ b/photonlib-java-examples/simposeest/src/main/java/frc/robot/DrivetrainSim.java @@ -38,7 +38,7 @@ import edu.wpi.first.wpilibj.simulation.AnalogGyroSim; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; import edu.wpi.first.wpilibj.simulation.EncoderSim; import edu.wpi.first.wpilibj.simulation.PWMSim; -import org.photonvision.SimVisionSystem; +import org.photonvision.simulation.SimVisionSystem; /** * Implementation of a simulation of robot physics, sensors, motor controllers Includes a Simulated