diff --git a/build.gradle b/build.gradle index 4349be993..9421d4428 100644 --- a/build.gradle +++ b/build.gradle @@ -112,5 +112,6 @@ subprojects { options.addStringOption("charset", "utf-8") options.addStringOption("docencoding", "utf-8") options.addStringOption("encoding", "utf-8") + options.addBooleanOption("Xdoclint/package:-org.photonvision.proto,-org.photonvision.struct,-org.photonvision.targeting.proto,-org.photonvision.jni", true) } } diff --git a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py index 347bda12c..8f3472732 100644 --- a/photon-lib/py/photonlibpy/simulation/photonCameraSim.py +++ b/photon-lib/py/photonlibpy/simulation/photonCameraSim.py @@ -51,7 +51,7 @@ class PhotonCameraSim: :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 + :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. @@ -206,7 +206,7 @@ class PhotonCameraSim: return None def setMinTargetAreaPercent(self, areaPercent: float) -> None: - """The minimum percentage(0 - 100) a detected target must take up of the camera's image to be + """The minimum percentage (0 - 100) a detected target must take up of the camera's image to be processed. """ self.minTargetAreaPercent = areaPercent diff --git a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py index e33a1ce7a..ee6b5276e 100644 --- a/photon-lib/py/photonlibpy/simulation/simCameraProperties.py +++ b/photon-lib/py/photonlibpy/simulation/simCameraProperties.py @@ -195,7 +195,7 @@ class SimCameraProperties: return self.latencyStdDev def getContourAreaPercent(self, points: np.ndarray) -> float: - """The percentage(0 - 100) of this camera's resolution the contour takes up in pixels of the + """The percentage (0 - 100) of this camera's resolution the contour takes up in pixels of the image. :param points: Points of the contour diff --git a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java index b3da87c59..3a4a5209d 100644 --- a/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java +++ b/photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java @@ -48,6 +48,8 @@ public class EstimatedRobotPose { * * @param estimatedPose estimated pose * @param timestampSeconds timestamp of the estimate + * @param targetsUsed list of targets used + * @param strategy pose strategy */ public EstimatedRobotPose( Pose3d estimatedPose, double timestampSeconds, List targetsUsed) { diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 45885ab3b..752b3f530 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -115,6 +115,14 @@ public class PhotonCamera implements AutoCloseable { private final Alert disconnectAlert; private final Alert timesyncAlert; + /** + * Sets whether or not coprocessor version checks will occur. Setting this to true will silence + * all console warnings about coproccessor connection, so be careful when enabling this and ensure + * all your coprocessors are communicating to the robot properly and everything has matching + * versions. + * + * @param enabled Whether or not to enable coprocessor version checks + */ public static void setVersionCheckEnabled(boolean enabled) { VERSION_CHECK_ENABLED = enabled; } @@ -179,7 +187,7 @@ public class PhotonCamera implements AutoCloseable { verifyDependencies(); } - public static void verifyDependencies() { + static void verifyDependencies() { // spotless:off if (!WPILibVersion.Version.equals(PhotonVersion.wpilibTargetVersion)) { String bfw = """ @@ -287,6 +295,8 @@ public class PhotonCamera implements AutoCloseable { * getAllUnreadResults() will return different (potentially empty) result arrays. Be careful to * call this exactly ONCE per loop of your robot code! FIFO depth is limited to 20 changes, so * make sure to call this frequently enough to avoid old results being discarded, too! + * + * @return The list of pipeline results */ public List getAllUnreadResults() { verifyVersion(); @@ -311,6 +321,8 @@ public class PhotonCamera implements AutoCloseable { * *

Replaced by {@link #getAllUnreadResults()} over getLatestResult, as this function can miss * results, or provide duplicate ones! + * + * @return The latest pipeline result */ @Deprecated(since = "2024", forRemoval = true) public PhotonPipelineResult getLatestResult() { @@ -501,8 +513,11 @@ public class PhotonCamera implements AutoCloseable { } /** - * The camera calibration's distortion coefficients, in OPENCV8 form. Higher-order terms are set - * to 0 + * Returns the camera calibration's distortion coefficients, in OPENCV8 form. Higher-order terms + * are set to 0 + * + * @return The distortion coefficients in a 8x1 matrix, if they are published by the camera. Empty + * otherwise. */ public Optional> getDistCoeffs() { var distCoeffs = cameraDistortionSubscriber.get(); diff --git a/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java b/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java index bf2264464..7c7a42f88 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonTargetSortMode.java @@ -27,6 +27,7 @@ package org.photonvision; import java.util.Comparator; import org.photonvision.targeting.PhotonTrackedTarget; +@SuppressWarnings("doclint") public enum PhotonTargetSortMode { Smallest(Comparator.comparingDouble(PhotonTrackedTarget::getArea)), Largest(Smallest.m_comparator.reversed()), diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index 3493d3a92..d20af6958 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -69,7 +69,9 @@ import org.photonvision.targeting.PnpResult; public class PhotonCameraSim implements AutoCloseable { private final PhotonCamera cam; + @SuppressWarnings("doclint") protected NTTopicSet ts = new NTTopicSet(); + private long heartbeatCounter = 1; /** This simulated camera's {@link SimCameraProperties} */ @@ -168,7 +170,7 @@ public class PhotonCameraSim implements AutoCloseable { * * @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 + * @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. @@ -190,7 +192,7 @@ public class PhotonCameraSim implements AutoCloseable { * * @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 + * @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. @@ -208,22 +210,50 @@ public class PhotonCameraSim implements AutoCloseable { this.maxSightRangeMeters = maxSightRangeMeters; } + /** + * Returns the camera being simulated. + * + * @return The camera + */ public PhotonCamera getCamera() { return cam; } + /** + * Returns the minimum percentage (0 - 100) a detected target must take up of the camera's image + * to be processed. + * + * @return The percentage + */ public double getMinTargetAreaPercent() { return minTargetAreaPercent; } + /** + * Returns the minimum number of pixels a detected target must take up in the camera's image to be + * processed. + * + * @return The number of pixels + */ public double getMinTargetAreaPixels() { return minTargetAreaPercent / 100.0 * prop.getResArea(); } + /** + * Returns the maximum distance at which the target is illuminated to your camera. Note that + * minimum target area of the image is separate from this. + * + * @return The distance in meters + */ public double getMaxSightRangeMeters() { return maxSightRangeMeters; } + /** + * Returns the order the targets are sorted in the pipeline result. + * + * @return The target sorting order + */ public PhotonTargetSortMode getTargetSortMode() { return sortMode; } @@ -261,6 +291,7 @@ public class PhotonCameraSim implements AutoCloseable { * Determines if all target points are inside the camera's image. * * @param points The target's 2d image points + * @return True if all the target points are inside the camera's image, false otherwise. */ public boolean canSeeCorners(Point[] points) { for (var point : points) { @@ -306,30 +337,40 @@ public class PhotonCameraSim implements AutoCloseable { } /** - * The minimum percentage(0 - 100) a detected target must take up of the camera's image to be - * processed. + * Sets the minimum percentage (0 - 100) a detected target must take up of the camera's image to + * be processed. + * + * @param areaPercent The percentage */ 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 + * Sets the minimum number of pixels a detected target must take up in the camera's image to be * processed. + * + * @param areaPx The number of pixels */ 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. + * Sets the maximum distance at which the target is illuminated to your camera. Note that minimum + * target area of the image is separate from this. + * + * @param rangeMeters The distance in meters */ public void setMaxSightRange(double rangeMeters) { this.maxSightRangeMeters = rangeMeters; } - /** Defines the order the targets are sorted in the pipeline result. */ + /** + * Defines the order the targets are sorted in the pipeline result. + * + * @param sortMode The target sorting order + */ public void setTargetSortMode(PhotonTargetSortMode sortMode) { if (sortMode != null) this.sortMode = sortMode; } @@ -338,6 +379,8 @@ public class PhotonCameraSim implements AutoCloseable { * Sets whether the raw video stream simulation is enabled. * *

Note: This may increase loop times. + * + * @param enabled Whether or not to enable the raw video stream */ public void enableRawStream(boolean enabled) { videoSimRawEnabled = enabled; @@ -347,6 +390,8 @@ public class PhotonCameraSim implements AutoCloseable { * Sets whether a wireframe of the field is drawn to the raw video stream. * *

Note: This will dramatically increase loop times. + * + * @param enabled Whether or not to enable the wireframe in the raw video stream */ public void enableDrawWireframe(boolean enabled) { videoSimWireframeEnabled = enabled; @@ -363,7 +408,11 @@ public class PhotonCameraSim implements AutoCloseable { videoSimWireframeResolution = resolution; } - /** Sets whether the processed video stream simulation is enabled. */ + /** + * Sets whether the processed video stream simulation is enabled. + * + * @param enabled Whether or not to enable the processed video stream + */ public void enableProcessedStream(boolean enabled) { videoSimProcEnabled = enabled; } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java index 7e806e159..620a076f7 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java @@ -89,7 +89,7 @@ public class SimCameraProperties { } /** - * Reads camera properties from a photonvision config.json file. This is only the + * 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. * @@ -104,7 +104,7 @@ public class SimCameraProperties { } /** - * Reads camera properties from a photonvision config.json file. This is only the + * 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. * @@ -232,8 +232,11 @@ public class SimCameraProperties { } /** + * Sets the simulated FPS for this camera. + * * @param fps The average frames per second the camera should process at. Exposure time limits * FPS if set! + * @return This camera properties object for use in chaining */ public SimCameraProperties setFPS(double fps) { frameSpeedMs = Math.max(1000.0 / fps, exposureTimeMs); @@ -242,8 +245,11 @@ public class SimCameraProperties { } /** + * Sets the simulated exposure time for this camera. + * * @param exposureTimeMs The amount of time the "shutter" is open for one frame. Affects motion * blur. Frame speed(from FPS) is limited to this! + * @return This camera properties object for use in chaining */ public SimCameraProperties setExposureTimeMs(double exposureTimeMs) { this.exposureTimeMs = exposureTimeMs; @@ -253,8 +259,11 @@ public class SimCameraProperties { } /** + * Sets the simulated latency for this camera. + * * @param avgLatencyMs The average latency (from image capture to data published) in milliseconds * a frame should have + * @return This camera properties object for use in chaining */ public SimCameraProperties setAvgLatencyMs(double avgLatencyMs) { this.avgLatencyMs = avgLatencyMs; @@ -263,7 +272,10 @@ public class SimCameraProperties { } /** + * Sets the simulated latency variation for this camera. + * * @param latencyStdDevMs The standard deviation in milliseconds of the latency + * @return This camera properties object for use in chaining */ public SimCameraProperties setLatencyStdDevMs(double latencyStdDevMs) { this.latencyStdDevMs = latencyStdDevMs; @@ -271,14 +283,29 @@ public class SimCameraProperties { return this; } + /** + * Gets the width of the simulated camera image. + * + * @return The width in pixels + */ public int getResWidth() { return resWidth; } + /** + * Gets the height of the simulated camera image. + * + * @return The height in pixels + */ public int getResHeight() { return resHeight; } + /** + * Gets the area of the simulated camera image. + * + * @return The area in pixels + */ public int getResArea() { return resWidth * resHeight; } @@ -292,30 +319,66 @@ public class SimCameraProperties { return camIntrinsics.copy(); } + /** + * Returns the camera calibration's distortion coefficients, in OPENCV8 form. Higher-order terms + * are set to 0 + * + * @return The distortion coefficients in an 8d vector + */ public Vector getDistCoeffs() { return new Vector<>(distCoeffs); } + /** + * Gets the FPS of the simulated camera. + * + * @return The FPS + */ public double getFPS() { return 1000.0 / frameSpeedMs; } + /** + * Gets the time per frame of the simulated camera. + * + * @return The time per frame in milliseconds + */ public double getFrameSpeedMs() { return frameSpeedMs; } + /** + * Gets the exposure time of the simulated camera. + * + * @return The exposure time in milliseconds + */ public double getExposureTimeMs() { return exposureTimeMs; } + /** + * Gets the average latency of the simulated camera. + * + * @return The average latency in milliseconds + */ public double getAvgLatencyMs() { return avgLatencyMs; } + /** + * Gets the time per frame of the simulated camera. + * + * @return The time per frame in milliseconds + */ public double getLatencyStdDevMs() { return latencyStdDevMs; } + /** + * Returns a copy of the camera properties. + * + * @return The copied camera properties + */ public SimCameraProperties copy() { var newProp = new SimCameraProperties(); newProp.setCalibration(resWidth, resHeight, camIntrinsics, distCoeffs); @@ -328,10 +391,11 @@ public class SimCameraProperties { } /** - * The percentage(0 - 100) of this camera's resolution the contour takes up in pixels of the + * The percentage (0 - 100) of this camera's resolution the contour takes up in pixels of the * image. * * @param points Points of the contour + * @return The percentage */ public double getContourAreaPercent(Point[] points) { return Imgproc.contourArea(new MatOfPoint2f(OpenCVHelp.getConvexHull(points))) @@ -567,7 +631,12 @@ public class SimCameraProperties { else return new Pair<>(null, null); } - /** Returns these points after applying this camera's estimated noise. */ + /** + * Returns these points after applying this camera's estimated noise. + * + * @param points The points to add noise to + * @return The points with noise + */ public Point[] estPixelNoise(Point[] points) { if (avgErrorPx == 0 && errorStdDevPx == 0) return points; @@ -584,14 +653,18 @@ public class SimCameraProperties { } /** - * @return Noisy estimation of a frame's processing latency in milliseconds + * Returns an estimation of a frame's processing latency with noise added. + * + * @return The latency estimate 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 + * Estimates how long until the next frame should be processed. + * + * @return The estimated time until the next frame in milliseconds */ public double estMsUntilNextFrame() { // exceptional processing latency blocks the next frame @@ -600,11 +673,28 @@ public class SimCameraProperties { // pre-calibrated example cameras - /** 960x720 resolution, 90 degree FOV, "perfect" lagless camera */ + /** + * Creates a set of camera properties where the camera has a 960x720 resolution, 90 degree FOV, + * and is a "perfect" lagless camera. + * + * @return The properties for this theoretical camera + */ public static SimCameraProperties PERFECT_90DEG() { return new SimCameraProperties(); } + /** + * Creates a set of camera properties matching those of Microsoft Lifecam running on a Raspberry + * Pi 4 at 320x240 resolution. + * + *

Note that this set of properties represents a camera setup, not your camera + * setup. Do not use these camera properties for any non-sim vision calculations, especially + * the calibration data. Always use your camera's calibration data to do vision calculations in + * non-sim environments. These properties exist as a sample that may be used to get representative + * data in sim. + * + * @return The properties for this camera setup + */ public static SimCameraProperties PI4_LIFECAM_320_240() { var prop = new SimCameraProperties(); prop.setCalibration( @@ -639,6 +729,18 @@ public class SimCameraProperties { return prop; } + /** + * Creates a set of camera properties matching those of Microsoft Lifecam running on a Raspberry + * Pi 4 at 640x480 resolution. + * + *

Note that this set of properties represents a camera setup, not your camera + * setup. Do not use these camera properties for any non-sim vision calculations, especially + * the calibration data. Always use your camera's calibration data to do vision calculations in + * non-sim environments. These properties exist as a sample that may be used to get representative + * data in sim. + * + * @return The properties for this camera setup + */ public static SimCameraProperties PI4_LIFECAM_640_480() { var prop = new SimCameraProperties(); prop.setCalibration( @@ -673,6 +775,18 @@ public class SimCameraProperties { return prop; } + /** + * Creates a set of camera properties matching those of a Limelight 2 running at 640x480 + * resolution. + * + *

Note that this set of properties represents a camera setup, not your camera + * setup. Do not use these camera properties for any non-sim vision calculations, especially + * the calibration data. Always use your camera's calibration data to do vision calculations in + * non-sim environments. These properties exist as a sample that may be used to get representative + * data in sim. + * + * @return The properties for this camera setup + */ public static SimCameraProperties LL2_640_480() { var prop = new SimCameraProperties(); prop.setCalibration( @@ -706,6 +820,18 @@ public class SimCameraProperties { return prop; } + /** + * Creates a set of camera properties matching those of a Limelight 2 running at 960x720 + * resolution. + * + *

Note that this set of properties represents a camera setup, not your camera + * setup. Do not use these camera properties for any non-sim vision calculations, especially + * the calibration data. Always use your camera's calibration data to do vision calculations in + * non-sim environments. These properties exist as a sample that may be used to get representative + * data in sim. + * + * @return The properties for this camera setup + */ public static SimCameraProperties LL2_960_720() { var prop = new SimCameraProperties(); prop.setCalibration( @@ -740,6 +866,18 @@ public class SimCameraProperties { return prop; } + /** + * Creates a set of camera properties matching those of a Limelight 2 running at 1280x720 + * resolution. + * + *

Note that this set of properties represents a camera setup, not your camera + * setup. Do not use these camera properties for any non-sim vision calculations, especially + * the calibration data. Always use your camera's calibration data to do vision calculations in + * non-sim environments. These properties exist as a sample that may be used to get representative + * data in sim. + * + * @return The properties for this camera setup + */ public static SimCameraProperties LL2_1280_720() { var prop = new SimCameraProperties(); prop.setCalibration( diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java index ceaabe4ec..636599ae8 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java @@ -73,6 +73,8 @@ public class VideoSimUtil { kTag36h11MarkerPts = get36h11MarkerPts(); } + private VideoSimUtil() {} + /** 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()); @@ -87,6 +89,7 @@ public class VideoSimUtil { *

Order of corners returned is: [BL, BR, TR, TL] * * @param size Size of image + * @return The corners */ public static Point[] getImageCorners(Size size) { return new Point[] { @@ -116,7 +119,11 @@ public class VideoSimUtil { frame.getHeight(), frame.getWidth(), CvType.CV_8UC1, frame.getData(), frame.getStride()); } - /** Gets the points representing the marker(black square) corners. */ + /** + * Gets the points representing the marker(black square) corners. + * + * @return The points + */ public static Point[] get36h11MarkerPts() { return get36h11MarkerPts(1); } @@ -125,6 +132,7 @@ public class VideoSimUtil { * Gets the points representing the marker(black square) corners. * * @param scale The scale of the tag image (10*scale x 10*scale image) + * @return The points */ public static Point[] get36h11MarkerPts(int scale) { var roi36h11 = new Rect(new Point(1, 1), new Size(8, 8)); @@ -276,12 +284,12 @@ public class VideoSimUtil { * resolution. * * @param thickness480p A hypothetical line thickness in a 640x480 image - * @param destinationImg The destination image to scale to + * @param destination The destination image to scale to * @return Scaled thickness which cannot be less than 1 */ - public static double getScaledThickness(double thickness480p, Mat destinationImg) { - double scaleX = destinationImg.width() / 640.0; - double scaleY = destinationImg.height() / 480.0; + public static double getScaledThickness(double thickness480p, Mat destination) { + double scaleX = destination.width() / 640.0; + double scaleY = destination.height() / 480.0; double minScale = Math.min(scaleX, scaleY); return Math.max(thickness480p * minScale, 1.0); } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java index 2f8ff2ef6..29b79a8f0 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java @@ -119,6 +119,7 @@ public class VisionSystemSim { /** * Remove a simulated camera from this vision system. * + * @param cameraSim The camera to remove * @return If the camera was present and removed */ public boolean removeCamera(PhotonCameraSim cameraSim) { @@ -202,6 +203,7 @@ public class VisionSystemSim { /** * Reset the transform history for this camera to just the current transform. * + * @param cameraSim The camera to reset * @return If the cameraSim was valid and transforms were reset */ public boolean resetCameraTransforms(PhotonCameraSim cameraSim) { @@ -214,6 +216,11 @@ public class VisionSystemSim { return true; } + /** + * Returns all the vision targets on the field. + * + * @return The vision targets + */ public Set getVisionTargets() { var all = new HashSet(); for (var entry : targetSets.entrySet()) { @@ -222,6 +229,12 @@ public class VisionSystemSim { return all; } + /** + * Returns all the vision targets of the specified type on the field. + * + * @param type The type of vision targets to return + * @return The vision targets + */ public Set getVisionTargets(String type) { return targetSets.get(type); } @@ -276,18 +289,33 @@ public class VisionSystemSim { } } + /** Removes every {@link VisionTargetSim} from the simulated field. */ public void clearVisionTargets() { targetSets.clear(); } + /** Removes all simulated AprilTag targets from the simulated field. */ public void clearAprilTags() { removeVisionTargets("apriltag"); } + /** + * Removes every {@link VisionTargetSim} of the specified type from the simulated field. + * + * @param type Type of target (e.g. "cargo"). Same as the type passed into {@link + * #addVisionTargets(String, VisionTargetSim...)} + * @return The removed targets, or null if no targets of the specified type exist + */ public Set removeVisionTargets(String type) { return targetSets.remove(type); } + /** + * Removes the specified {@link VisionTargetSim}s from the simulated field. + * + * @param targets The targets to remove + * @return The targets that were actually removed + */ public Set removeVisionTargets(VisionTargetSim... targets) { var removeList = List.of(targets); var removedSet = new HashSet(); @@ -305,7 +333,11 @@ public class VisionSystemSim { return removedSet; } - /** Get the latest robot pose in meters saved by the vision system. */ + /** + * Get the latest robot pose in meters saved by the vision system. + * + * @return The latest robot pose + */ public Pose3d getRobotPose() { return getRobotPose(Timer.getFPGATimestamp()); } @@ -314,17 +346,26 @@ public class VisionSystemSim { * Get the robot pose in meters saved by the vision system at this timestamp. * * @param timestamp Timestamp of the desired robot pose + * @return The 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. */ + /** + * Clears all previous robot poses and sets robotPose at current time. + * + * @param robotPose The robot pose + */ public void resetRobotPose(Pose2d robotPose) { resetRobotPose(new Pose3d(robotPose)); } - /** Clears all previous robot poses and sets robotPose at current time. */ + /** + * Clears all previous robot poses and sets robotPose at current time. + * + * @param robotPose The robot pose + */ public void resetRobotPose(Pose3d robotPose) { robotPoseBuffer.clear(); robotPoseBuffer.addSample(Timer.getFPGATimestamp(), robotPose); diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionTargetSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionTargetSim.java index 95739131e..5e148fc7b 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VisionTargetSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionTargetSim.java @@ -40,7 +40,7 @@ public class VisionTargetSim { * 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 + * @param model TargetModel which describes the geometry of the target */ public VisionTargetSim(Pose3d pose, TargetModel model) { this.pose = pose; @@ -52,7 +52,7 @@ public class VisionTargetSim { * 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 model TargetModel which describes the geometry of the target(tag) * @param id The ID of this fiducial tag */ public VisionTargetSim(Pose3d pose, TargetModel model, int id) { @@ -61,18 +61,38 @@ public class VisionTargetSim { this.fiducialID = id; } + /** + * Sets the pose of this target on the field. + * + * @param pose The pose in field-relative coordinates + */ public void setPose(Pose3d pose) { this.pose = pose; } + /** + * Sets the model describing this target's geometry. + * + * @param model The model of the target + */ public void setModel(TargetModel model) { this.model = model; } + /** + * Returns the pose of this target on the field. + * + * @return The pose in field-relative coordinates + */ public Pose3d getPose() { return pose; } + /** + * Returns the model describing this target's geometry. + * + * @return The model of the target + */ public TargetModel getModel() { return model; } diff --git a/photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java b/photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java index 968ec8e97..bf64412f7 100644 --- a/photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java +++ b/photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java @@ -32,6 +32,8 @@ import org.photonvision.jni.TimeSyncServer; public class TimeSyncSingleton { private static TimeSyncServer INSTANCE = null; + private TimeSyncSingleton() {} + public static boolean load() { if (INSTANCE == null) { try { diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index ebdb2ae34..06a409577 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -179,20 +179,29 @@ class PhotonCamera { using DistortionMatrix = Eigen::Matrix; /** - * @brief Get the camera calibration matrix, in standard OpenCV form + * Get the camera calibration matrix, in standard OpenCV form * * @return std::optional */ std::optional GetCameraMatrix(); /** - * @brief Get the camera calibration distortion coefficients, in OPENCV8 form. - * Higher order terms are set to zero. + * Returns the camera calibration's distortion coefficients, in OPENCV8 form. + * Higher-order terms are set to 0 * - * @return std::optional + * @return The distortion coefficients in a 8x1 matrix, if they are published + * by the camera. Empty otherwise. */ std::optional GetDistCoeffs(); + /** + * Sets whether or not coprocessor version checks will occur. Setting this to + * true will silence all console warnings about coproccessor connection, so be + * careful when enabling this and ensure all your coprocessors are + * communicating to the robot properly and everything has matching versions. + * + * @param enabled Whether or not to enable coprocessor version checks + */ static void SetVersionCheckEnabled(bool enabled); std::shared_ptr GetCameraTable() const { return rootTable; } diff --git a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h index d88dddaaa..955bda85a 100644 --- a/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h +++ b/photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h @@ -48,42 +48,188 @@ namespace photon { class PhotonCameraSim { public: + /** + * Constructs a handle for simulating PhotonCamera values. Processing + * simulated targets through this class will change the associated + * PhotonCamera's results. + * + * WARNING: 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 + */ explicit PhotonCameraSim(PhotonCamera* camera); + + /** + * Constructs a handle for simulating 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 + * @param tagLayout The AprilTagFieldLayout used to solve for tag + * positions. + */ PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props, const frc::AprilTagFieldLayout& tagLayout = frc::AprilTagFieldLayout::LoadField( frc::AprilTagField::kDefaultField)); + + /** + * Constructs a handle for simulating 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 maxSightRange Maximum distance at which the target is + * illuminated to your camera. Note that minimum target area of the image is + * separate from this. + */ PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props, double minTargetAreaPercent, units::meter_t maxSightRange); + /** + * Returns the camera being simulated. + * + * @return The camera + */ inline PhotonCamera* GetCamera() { return cam; } + + /** + * Returns the minimum percentage (0 - 100) a detected target must take up of + * the camera's image to be processed. + * + * @return The percentage + */ inline double GetMinTargetAreaPercent() { return minTargetAreaPercent; } + + /** + * Returns the minimum number of pixels a detected target must take up in the + * camera's image to be processed. + * + * @return The number of pixels + */ inline double GetMinTargetAreaPixels() { return minTargetAreaPercent / 100.0 * prop.GetResArea(); } + + /** + * Returns the maximum distance at which the target is illuminated to your + * camera. Note that minimum target area of the image is separate from this. + * + * @return The distance + */ inline units::meter_t GetMaxSightRange() { return maxSightRange; } inline const cs::CvSource& GetVideoSimRaw() { return videoSimRaw; } inline const cv::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. + */ bool CanSeeTargetPose(const frc::Pose3d& camPose, const VisionTargetSim& target); + + /** + * Determines if all target points are inside the camera's image. + * + * @param points The target's 2d image points + * @return True if all the target points are inside the camera's image, false + * otherwise. + */ bool CanSeeCorner(const std::vector& points); + + /** + * 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 + */ std::optional ConsumeNextEntryTime(); + /** + * Sets the minimum percentage (0 - 100) a detected target must take up of the + * camera's image to be processed. + * + * @param areaPercent The percentage + */ inline void SetMinTargetAreaPercent(double areaPercent) { minTargetAreaPercent = areaPercent; } + + /** + * Sets the minimum number of pixels a detected target must take up in the + * camera's image to be processed. + * + * @param areaPx The number of pixels + */ inline void SetMinTargetAreaPixels(double areaPx) { minTargetAreaPercent = areaPx / prop.GetResArea() * 100; } + + /** + * Sets the maximum distance at which the target is illuminated to your + * camera. Note that minimum target area of the image is separate from this. + * + * @param rangeMeters The distance + */ inline void SetMaxSightRange(units::meter_t range) { maxSightRange = range; } + + /** + * Sets whether the raw video stream simulation is enabled. + * + * Note: This may increase loop times. + * + * @param enabled Whether or not to enable the raw video stream + */ inline void EnableRawStream(bool enabled) { videoSimRawEnabled = enabled; } + + /** + * Sets whether a wireframe of the field is drawn to the raw video stream. + * + * Note: This will dramatically increase loop times. + * + * @param enabled Whether or not to enable the wireframe in the raw video + * stream + */ inline void EnableDrawWireframe(bool enabled) { videoSimWireframeEnabled = enabled; } + + /** + * Sets the resolution of the drawn wireframe if enabled. Drawn line segments + * will be subdivided into smaller segments based on a threshold set by the + * resolution. + * + * @param resolution Resolution as a fraction(0 - 1) of the video frame's + * diagonal length in pixels + */ inline void SetWireframeResolution(double resolution) { videoSimWireframeResolution = resolution; } + + /** + * Sets whether the processed video stream simulation is enabled. + * + * @param enabled Whether or not to enable the processed video stream + */ inline void EnabledProcessedStream(double enabled) { videoSimProcEnabled = enabled; } diff --git a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h index 6b2630080..17c4fdbad 100644 --- a/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h @@ -40,9 +40,36 @@ #include namespace photon { + +/** + * 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. + */ class SimCameraProperties { public: + /** Default constructor which is the same as PERFECT_90DEG */ SimCameraProperties() { SetCalibration(960, 720, frc::Rotation2d{90_deg}); } + + /** + * 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 + */ SimCameraProperties(std::string path, int width, int height) {} void SetCalibration(int width, int height, frc::Rotation2d fovDiag); @@ -55,27 +82,65 @@ class SimCameraProperties { errorStdDevPx = newErrorStdDevPx; } + /** + * Sets the simulated FPS for this camera. + * + * @param fps The average frames per second the camera should process at. + * **Exposure time limits FPS if set!** + */ void SetFPS(units::hertz_t fps) { frameSpeed = units::math::max(1 / fps, exposureTime); } - void SetExposureTime(units::second_t newExposureTime) { - exposureTime = newExposureTime; - frameSpeed = units::math::max(frameSpeed, exposureTime); + /** + * Sets the simulated exposure time for this camera. + * + * @param exposureTime The amount of time the "shutter" is open for one frame. + * Affects motion blur. **Frame speed(from FPS) is limited to this!** + */ + void SetExposureTime(units::second_t exposureTime) { + this->exposureTime = exposureTime; + frameSpeed = units::math::max(frameSpeed, this->exposureTime); } - void SetAvgLatency(units::second_t newAvgLatency) { - avgLatency = newAvgLatency; + /** + * Sets the simulated latency for this camera. + * + * @param avgLatency The average latency (from image capture to data + * published) a frame should have + */ + void SetAvgLatency(units::second_t avgLatency) { + this->avgLatency = avgLatency; } - void SetLatencyStdDev(units::second_t newLatencyStdDev) { - latencyStdDev = newLatencyStdDev; + /** + * Sets the simulated latency variation for this camera. + * + * @param latencyStdDev The standard deviation of the latency + */ + void SetLatencyStdDev(units::second_t latencyStdDev) { + this->latencyStdDev = latencyStdDev; } + /** + * Gets the width of the simulated camera image. + * + * @return The width in pixels + */ int GetResWidth() const { return resWidth; } + /** + * Gets the height of the simulated camera image. + * + * @return The height in pixels + */ int GetResHeight() const { return resHeight; } + /** + * Gets the area of the simulated camera image. + * + * @return The area in pixels + */ int GetResArea() const { return resWidth * resHeight; } double GetAspectRatio() const { @@ -84,23 +149,63 @@ class SimCameraProperties { Eigen::Matrix GetIntrinsics() const { return camIntrinsics; } + /** + * Returns the camera calibration's distortion coefficients, in OPENCV8 form. + * Higher-order terms are set to 0 + * + * @return The distortion coefficients in an 8x1 matrix + */ Eigen::Matrix GetDistCoeffs() const { return distCoeffs; } + /** + * Gets the FPS of the simulated camera. + * + * @return The FPS + */ units::hertz_t GetFPS() const { return 1 / frameSpeed; } + /** + * Gets the time per frame of the simulated camera. + * + * @return The time per frame + */ units::second_t GetFrameSpeed() const { return frameSpeed; } + /** + * Gets the exposure time of the simulated camera. + * + * @return The exposure time + */ units::second_t GetExposureTime() const { return exposureTime; } + /** + * Gets the average latency of the simulated camera. + * + * @return The average latency + */ units::second_t GetAverageLatency() const { return avgLatency; } + /** + * Gets the time per frame of the simulated camera. + * + * @return The time per frame + */ units::second_t GetLatencyStdDev() const { return latencyStdDev; } + /** + * The percentage (0 - 100) of this camera's resolution the contour takes up + * in pixels of the image. + * + * @param points Points of the contour + * @return The percentage + */ double GetContourAreaPercent(const std::vector& points) { return cv::contourArea(photon::OpenCVHelp::GetConvexHull(points)) / GetResArea() * 100; } + /** The yaw from the principal point of this camera to the pixel x value. + * Positive values left. */ frc::Rotation2d GetPixelYaw(double pixelX) const { double fx = camIntrinsics(0, 0); double cx = camIntrinsics(0, 2); @@ -108,18 +213,56 @@ class SimCameraProperties { return frc::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 + * #getCorrectedPixelRot(const cv::Point2d). + */ frc::Rotation2d GetPixelPitch(double pixelY) const { double fy = camIntrinsics(1, 1); double cy = camIntrinsics(1, 2); double yOffset = cy - pixelY; return frc::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 + * #getCorrectedPixelRot(const cv::Point2d). + */ frc::Rotation3d GetPixelRot(const cv::Point2d& point) const { return frc::Rotation3d{0_rad, GetPixelPitch(point.y).Radians(), GetPixelYaw(point.x).Radians()}; } + /** + * 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). + */ frc::Rotation3d GetCorrectedPixelRot(const cv::Point2d& point) const { double fx = camIntrinsics(0, 0); double cx = camIntrinsics(0, 2); @@ -151,10 +294,40 @@ class SimCameraProperties { units::math::hypot(GetHorizFOV().Radians(), GetVertFOV().Radians())}; } + /** + * Determines where the line segment defined by the two given translations + * intersects the camera's frustum/field-of-vision, if at all. + * + *

The line is parametrized so any of its points p = t * (b - a) + + * a. This method returns these values of t, minimum first, defining + * the region of the line segment which is visible in the frustum. If both + * ends of the line segment are visible, this simply returns {0, 1}. If, for + * example, point b is visible while a is not, and half of the line segment is + * inside the camera frustum, {0.5, 1} would be returned. + * + * @param camRt The change in basis from world coordinates to camera + * coordinates. See RotTrlTransform3d#makeRelativeTo(frc::Pose3d). + * @param a The initial translation of the line + * @param b The final translation of the line + * @return A Pair of Doubles. The values may be empty: + * - {double, double} : Two parametrized values(t), minimum first, + * representing which segment of the line is visible in the camera frustum. + * - {double, std::nullopt} : One value(t) representing a single + * intersection point. For example, the line only intersects the intersection + * of two adjacent viewplanes. + * - {std::nullopt, std::nullopt} : No values. The line segment is not + * visible in the camera frustum. + */ std::pair, std::optional> GetVisibleLine( const RotTrlTransform3d& camRt, const frc::Translation3d& a, const frc::Translation3d& b) const; + /** + * Returns these points after applying this camera's estimated noise. + * + * @param points The points to add noise to + * @return The points with noise + */ std::vector EstPixelNoise( const std::vector& points) { if (avgErrorPx == 0 && errorStdDevPx == 0) { @@ -174,17 +347,46 @@ class SimCameraProperties { return noisyPts; } + /** + * Returns an estimation of a frame's processing latency with noise added. + * + * @return The latency estimate + */ units::second_t EstLatency() { return units::math::max(avgLatency + gaussian(generator) * latencyStdDev, 0_s); } + /** + * Estimates how long until the next frame should be processed. + * + * @return The estimated time until the next frame + */ units::second_t EstSecUntilNextFrame() { return frameSpeed + units::math::max(0_s, EstLatency() - frameSpeed); } + /** + * Creates a set of camera properties where the camera has a 960x720 + * resolution, 90 degree FOV, and is a "perfect" lagless camera. + * + * @return The properties for this theoretical camera + */ static SimCameraProperties PERFECT_90DEG() { return SimCameraProperties{}; } + /** + * Creates a set of camera properties matching those of Microsoft Lifecam + * running on a Raspberry Pi 4 at 320x240 resolution. + * + * Note that this set of properties represents *a camera setup*, not *your + * camera setup*. Do not use these camera properties for any non-sim vision + * calculations, especially the calibration data. Always use your camera's + * calibration data to do vision calculations in non-sim environments. These + * properties exist as a sample that may be used to get representative data in + * sim. + * + * @return The properties for this camera setup + */ static SimCameraProperties PI4_LIFECAM_320_240() { SimCameraProperties prop{}; prop.SetCalibration( @@ -202,6 +404,19 @@ class SimCameraProperties { return prop; } + /** + * Creates a set of camera properties matching those of Microsoft Lifecam + * running on a Raspberry Pi 4 at 640x480 resolution. + * + *

Note that this set of properties represents *a camera setup*, not *your + * camera setup*. Do not use these camera properties for any non-sim vision + * calculations, especially the calibration data. Always use your camera's + * calibration data to do vision calculations in non-sim environments. These + * properties exist as a sample that may be used to get representative data in + * sim. + * + * @return The properties for this camera setup + */ static SimCameraProperties PI4_LIFECAM_640_480() { SimCameraProperties prop{}; prop.SetCalibration( @@ -219,6 +434,19 @@ class SimCameraProperties { return prop; } + /** + * Creates a set of camera properties matching those of a Limelight 2 running + * at 640x480 resolution. + * + *

Note that this set of properties represents *a camera setup*, not *your + * camera setup*. Do not use these camera properties for any non-sim vision + * calculations, especially the calibration data. Always use your camera's + * calibration data to do vision calculations in non-sim environments. These + * properties exist as a sample that may be used to get representative data in + * sim. + * + * @return The properties for this camera setup + */ static SimCameraProperties LL2_640_480() { SimCameraProperties prop{}; prop.SetCalibration( @@ -236,6 +464,19 @@ class SimCameraProperties { return prop; } + /** + * Creates a set of camera properties matching those of a Limelight 2 running + * at 960x720 resolution. + * + *

Note that this set of properties represents *a camera setup*, not *your + * camera setup*. Do not use these camera properties for any non-sim vision + * calculations, especially the calibration data. Always use your camera's + * calibration data to do vision calculations in non-sim environments. These + * properties exist as a sample that may be used to get representative data in + * sim. + * + * @return The properties for this camera setup + */ static SimCameraProperties LL2_960_720() { SimCameraProperties prop{}; prop.SetCalibration( @@ -253,6 +494,19 @@ class SimCameraProperties { return prop; } + /** + * Creates a set of camera properties matching those of a Limelight 2 running + * at 1280x720 resolution. + * + *

Note that this set of properties represents *a camera setup*, not *your + * camera setup*. Do not use these camera properties for any non-sim vision + * calculations, especially the calibration data. Always use your camera's + * calibration data to do vision calculations in non-sim environments. These + * properties exist as a sample that may be used to get representative data in + * sim. + * + * @return The properties for this camera setup + */ static SimCameraProperties LL2_1280_720() { SimCameraProperties prop{}; prop.SetCalibration( diff --git a/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h index 71975c2f8..a86ae4a04 100644 --- a/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h +++ b/photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h @@ -73,6 +73,16 @@ static std::unordered_map LoadAprilTagImages() { return retVal; } +/** + * Gets the points representing the corners of this image. Because image pixels + * are accessed through a cv::Mat, the point (0,0) actually represents the + * center of the top-left pixel and not the actual top-left corner. + * + *

Order of corners returned is: [BL, BR, TR, TL] + * + * @param size Size of image + * @return The corners + */ static std::vector GetImageCorners(const cv::Size& size) { std::vector retVal{}; retVal.emplace_back(cv::Point2f{-0.5f, size.height - 0.5f}); @@ -82,6 +92,12 @@ static std::vector GetImageCorners(const cv::Size& size) { return retVal; } +/** + * Gets the points representing the marker(black square) corners. + * + * @param scale The scale of the tag image (10*scale x 10*scale image) + * @return The points + */ static std::vector Get36h11MarkerPts(int scale) { cv::Rect2f roi36h11{cv::Point2f{1, 1}, cv::Point2f{8, 8}}; roi36h11.x *= scale; @@ -96,6 +112,11 @@ static std::vector Get36h11MarkerPts(int scale) { return pts; } +/** + * Gets the points representing the marker(black square) corners. + * + * @return The points + */ static std::vector Get36h11MarkerPts() { return Get36h11MarkerPts(1); } @@ -104,12 +125,26 @@ static const std::unordered_map kTag36h11Images = LoadAprilTagImages(); static const std::vector kTag36h11MarkPts = Get36h11MarkerPts(); +/** Updates the properties of this cs::CvSource video stream with the given + * camera properties. */ [[maybe_unused]] static void UpdateVideoProp(cs::CvSource& video, const SimCameraProperties& prop) { video.SetResolution(prop.GetResWidth(), prop.GetResHeight()); video.SetFPS(prop.GetFPS().to()); } - +/** + * Warps the image of a specific 36h11 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 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. + * @param destination The destination image to place the warped tag image onto. + */ [[maybe_unused]] static void Warp165h5TagImage( int tagId, const std::vector& dstPoints, bool antialiasing, cv::Mat& destination) { @@ -232,14 +267,32 @@ static const std::vector kTag36h11MarkPts = Get36h11MarkerPts(); cv::copyTo(tempRoi, destination(boundingRect), tempMask); } +/** + * Given a line thickness in a 640x480 image, try to scale to the given + * destination image resolution. + * + * @param thickness480p A hypothetical line thickness in a 640x480 image + * @param destination The destination image to scale to + * @return Scaled thickness which cannot be less than 1 + */ static double GetScaledThickness(double thickness480p, - const cv::Mat& destinationMat) { - double scaleX = destinationMat.size().width / 640.0; - double scaleY = destinationMat.size().height / 480.0; + const cv::Mat& destination) { + double scaleX = destination.size().width / 640.0; + double scaleY = destination.size().height / 480.0; double minScale = std::min(scaleX, scaleY); return std::max(thickness480p * minScale, 1.0); } +/** + * Draw a filled ellipse in the destination image. + * + * @param dstPoints The points in the destination image representing the + * rectangle in which the ellipse is inscribed. + * @param color The color of the ellipse. This is a scalar with BGR values + * (0-255) + * @param destination The destination image to draw onto. The image should be in + * the BGR color space. + */ [[maybe_unused]] static void DrawInscribedEllipse( const std::vector& dstPoints, const cv::Scalar& color, cv::Mat& destination) { @@ -261,6 +314,16 @@ static void DrawPoly(const std::vector& dstPoints, int thickness, } } +/** + * 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. + */ [[maybe_unused]] static void DrawTagDetection( int id, const std::vector& dstPoints, cv::Mat& destination) { double thickness = GetScaledThickness(1, destination); @@ -275,6 +338,10 @@ static void DrawPoly(const std::vector& dstPoints, int thickness, static_cast(thickness), cv::LINE_AA); } +/** + * The translations used to draw the field side walls and driver station walls. + * It is a vector of vectors because the translations are not all connected. + */ static std::vector> GetFieldWallLines() { std::vector> list; @@ -328,6 +395,15 @@ static std::vector> GetFieldWallLines() { return list; } +/** + * The translations used to draw the field floor subdivisions (not the floor + * outline). It is a vector of vectors because the translations are not all + * connected. + * + * @param subdivisions How many "subdivisions" along the width/length of the + * floor. E.g. 3 subdivisions would mean 2 lines along the length and 2 lines + * along the width creating a 3x3 "grid". + */ static std::vector> GetFieldFloorLines( int subdivisions) { std::vector> list; @@ -345,6 +421,23 @@ static std::vector> GetFieldFloorLines( return list; } +/** + * Convert 3D lines represented by the given series of translations into a + * polygon(s) in the camera's image. + * + * @param camRt The change in basis from world coordinates to camera + * coordinates. See RotTrlTransform3d#makeRelativeTo(Pose3d). + * @param prop The simulated camera's properties. + * @param trls A sequential series of translations defining the polygon to be + * drawn. + * @param resolution Resolution as a fraction(0 - 1) of the video frame's + * diagonal length in pixels. Line segments will be subdivided if they exceed + * this resolution. + * @param isClosed If the final translation should also draw a line to the first + * translation. + * @param destination The destination image that is being drawn to. + * @return A list of polygons(which are an array of points) + */ static std::vector> PolyFrom3dLines( const RotTrlTransform3d& camRt, const SimCameraProperties& prop, const std::vector& trls, double resolution, @@ -403,6 +496,25 @@ static std::vector> PolyFrom3dLines( return polyPointList; } +/** + * Draw a wireframe of the field to the given image. + * + * @param camRt The change in basis from world coordinates to camera + * coordinates. See RotTrlTransform3d#makeRelativeTo(frc::Pose3d). + * @param prop The simulated camera's properties. + * @param resolution Resolution as a fraction(0 - 1) of the video frame's + * diagonal length in pixels. Line segments will be subdivided if they exceed + * this resolution. + * @param wallThickness Thickness of the lines used for drawing the field walls + * in pixels. This is scaled by #getScaledThickness(double, cv::Mat). + * @param wallColor Color of the lines used for drawing the field walls. + * @param floorSubdivisions A NxN "grid" is created from the floor where this + * parameter is N, which defines the floor lines. + * @param floorThickness Thickness of the lines used for drawing the field floor + * grid in pixels. This is scaled by #getScaledThickness(double, cv::Mat). + * @param floorColor Color of the lines used for drawing the field floor grid. + * @param destination The destination image to draw to. + */ [[maybe_unused]] static void DrawFieldWireFrame( const RotTrlTransform3d& camRt, const SimCameraProperties& prop, double resolution, double wallThickness, const cv::Scalar& wallColor, diff --git a/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h index aca9e3d91..81f1a87a6 100644 --- a/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h +++ b/photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h @@ -38,12 +38,32 @@ #include "photon/simulation/PhotonCameraSim.h" namespace photon { +/** + * A simulated vision system involving a camera(s) and coprocessor(s) mounted on + * a mobile robot running PhotonVision, detecting targets placed on the field. + * VisionTargetSims added to this class will be detected by the PhotonCameraSim + * 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. + */ class VisionSystemSim { public: + /** + * A simulated vision system involving a camera(s) and coprocessor(s) mounted + * on a mobile robot running PhotonVision, detecting targets placed on the + * field. VisionTargetSims added to this class will be detected by the + * PhotonCameraSims 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. + */ explicit VisionSystemSim(std::string visionSystemName) { std::string tableName = "VisionSystemSim-" + visionSystemName; frc::SmartDashboard::PutData(tableName + "/Sim Field", &dbgField); } + + /** Get one of the simulated cameras. */ std::optional GetCameraSim(std::string name) { auto it = camSimMap.find(name); if (it != camSimMap.end()) { @@ -52,6 +72,8 @@ class VisionSystemSim { return std::nullopt; } } + + /** Get all the simulated cameras. */ std::vector GetCameraSims() { std::vector retVal; for (auto const& cam : camSimMap) { @@ -59,6 +81,15 @@ class VisionSystemSim { } return retVal; } + + /** + * 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 PhotonCamera. + * + * @param cameraSim The camera simulation + * @param robotToCamera The transform from the robot pose to the camera pose + */ void AddCamera(PhotonCameraSim* cameraSim, const frc::Transform3d& robotToCamera) { auto found = @@ -73,10 +104,19 @@ class VisionSystemSim { frc::Pose3d{} + robotToCamera); } } + + /** Remove all simulated cameras from this vision system. */ void ClearCameras() { camSimMap.clear(); camTrfMap.clear(); } + + /** + * Remove a simulated camera from this vision system. + * + * @param cameraSim The camera to remove + * @return If the camera was present and removed + */ bool RemoveCamera(PhotonCameraSim* cameraSim) { int numOfElementsRemoved = camSimMap.erase(std::string{cameraSim->GetCamera()->GetCameraName()}); @@ -86,9 +126,28 @@ class VisionSystemSim { return false; } } + + /** + * Get a simulated camera's position relative to the robot. If the requested + * camera is invalid, an empty optional is returned. + * + * @param cameraSim The specific camera to get the robot-to-camera transform + * of + * @return The transform of this camera, or an empty optional if it is invalid + */ std::optional GetRobotToCamera(PhotonCameraSim* cameraSim) { return GetRobotToCamera(cameraSim, frc::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 The specific camera to get the robot-to-camera transform + * of + * @param time Timestamp of when the transform should be observed + * @return The transform of this camera, or an empty optional if it is invalid + */ std::optional GetRobotToCamera(PhotonCameraSim* cameraSim, units::second_t time) { if (camTrfMap.find(cameraSim) != camTrfMap.end()) { @@ -105,9 +164,26 @@ class VisionSystemSim { return std::nullopt; } } + + /** + * Get a simulated camera's position on the field. If the requested camera is + * invalid, an empty optional is returned. + * + * @param cameraSim The specific camera to get the field pose of + * @return The pose of this camera, or an empty optional if it is invalid + */ std::optional GetCameraPose(PhotonCameraSim* cameraSim) { return GetCameraPose(cameraSim, frc::Timer::GetFPGATimestamp()); } + + /** + * Get a simulated camera's position on the field. If the requested camera is + * invalid, an empty optional is returned. + * + * @param cameraSim The specific camera to get the field pose of + * @param time Timestamp of when the pose should be observed + * @return The pose of this camera, or an empty optional if it is invalid + */ std::optional GetCameraPose(PhotonCameraSim* cameraSim, units::second_t time) { auto robotToCamera = GetRobotToCamera(cameraSim, time); @@ -117,6 +193,15 @@ class VisionSystemSim { return std::make_optional(GetRobotPose(time) + robotToCamera.value()); } } + + /** + * 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 + */ bool AdjustCamera(PhotonCameraSim* cameraSim, const frc::Transform3d& robotToCamera) { if (camTrfMap.find(cameraSim) != camTrfMap.end()) { @@ -127,11 +212,21 @@ class VisionSystemSim { return false; } } + + /** Reset the previous transforms for all cameras to their current transform. + */ void ResetCameraTransforms() { for (const auto& pair : camTrfMap) { ResetCameraTransforms(pair.first); } } + + /** + * Reset the transform history for this camera to just the current transform. + * + * @param cameraSim The camera to reset + * @return If the cameraSim was valid and transforms were reset + */ bool ResetCameraTransforms(PhotonCameraSim* cameraSim) { units::second_t now = frc::Timer::GetFPGATimestamp(); if (camTrfMap.find(cameraSim) != camTrfMap.end()) { @@ -145,6 +240,12 @@ class VisionSystemSim { return false; } } + + /** + * Returns all the vision targets on the field. + * + * @return The vision targets + */ std::vector GetVisionTargets() { std::vector all{}; for (const auto& entry : targetSets) { @@ -154,12 +255,40 @@ class VisionSystemSim { } return all; } + + /** + * Returns all the vision targets of the specified type on the field. + * + * @param type The type of vision targets to return + * @return The vision targets + */ std::vector GetVisionTargets(std::string type) { return targetSets[type]; } + + /** + * Adds targets on the field which your vision system is designed to detect. + * The PhotonCameras 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 + */ void AddVisionTargets(const std::vector& targets) { AddVisionTargets("targets", targets); } + + /** + * Adds targets on the field which your vision system is designed to detect. + * The PhotonCameras 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 + */ void AddVisionTargets(std::string type, const std::vector& targets) { if (!targetSets.contains(type)) { @@ -169,6 +298,20 @@ class VisionSystemSim { targetSets[type].emplace_back(tgt); } } + + /** + * Adds targets on the field which your vision system is designed to detect. + * The PhotonCameras 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 "apriltag". The poses added preserve the tag layout's current alliance + * origin. If the tag layout's alliance origin is changed, these added tags + * will have to be cleared and re-added. + * + * @param layout The field tag layout to get Apriltag poses and IDs from + */ void AddAprilTags(const frc::AprilTagFieldLayout& layout) { std::vector targets; for (const frc::AprilTag& tag : layout.GetTags()) { @@ -177,9 +320,28 @@ class VisionSystemSim { } AddVisionTargets("apriltag", targets); } + /** Removes every VisionTargetSim from the simulated field. */ void ClearVisionTargets() { targetSets.clear(); } + /** Removes all simulated AprilTag targets from the simulated field. */ void ClearAprilTags() { RemoveVisionTargets("apriltag"); } + + /** + * Removes every VisionTargetSim of the specified type from the simulated + * field. + * + * @param type Type of target (e.g. "cargo"). Same as the type passed into + * #addVisionTargets(String, VisionTargetSim...) + * @return The removed targets, or null if no targets of the specified type + * exist + */ void RemoveVisionTargets(std::string type) { targetSets.erase(type); } + + /** + * Removes the specified VisionTargetSims from the simulated field. + * + * @param targets The targets to remove + * @return The targets that were actually removed + */ std::vector RemoveVisionTargets( const std::vector& targets) { std::vector removedList; @@ -194,21 +356,60 @@ class VisionSystemSim { } return removedList; } + + /** + * Get the latest robot pose in meters saved by the vision system. + * + * @return The latest robot pose + */ frc::Pose3d GetRobotPose() { return GetRobotPose(frc::Timer::GetFPGATimestamp()); } + + /** + * Get the robot pose in meters saved by the vision system at this timestamp. + * + * @param timestamp Timestamp of the desired robot pose + * @return The robot pose + */ frc::Pose3d GetRobotPose(units::second_t timestamp) { return robotPoseBuffer.Sample(timestamp).value_or(frc::Pose3d{}); } + + /** + * Clears all previous robot poses and sets robotPose at current time. + * + * @param robotPose The robot pose + */ void ResetRobotPose(const frc::Pose2d& robotPose) { ResetRobotPose(frc::Pose3d{robotPose}); } + + /** + * Clears all previous robot poses and sets robotPose at current time. + * + * @param robotPose The robot pose + */ void ResetRobotPose(const frc::Pose3d& robotPose) { robotPoseBuffer.Clear(); robotPoseBuffer.AddSample(frc::Timer::GetFPGATimestamp(), robotPose); } frc::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 simulated robot pose in meters + */ void Update(const frc::Pose2d& robotPose) { Update(frc::Pose3d{robotPose}); } + + /** + * Periodic update. Ensure this is called repeatedly-- camera performance is + * used to automatically determine if a new frame should be submitted. + * + * @param robotPoseMeters The simulated robot pose in meters + */ void Update(const frc::Pose3d& robotPose) { for (auto& set : targetSets) { std::vector posesToAdd{}; diff --git a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java index 62d5d2c2c..fc9e2d107 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java @@ -24,6 +24,7 @@ import java.util.Optional; import org.photonvision.targeting.serde.PhotonStructSerializable; /** A packet that holds byte-packed data to be sent over NetworkTables. */ +@SuppressWarnings("doclint") public class Packet { // Data stored in the packet. byte[] packetData; diff --git a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java index a4e095cbb..3cd0abca1 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java +++ b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java @@ -19,6 +19,7 @@ package org.photonvision.common.dataflow.structures; import edu.wpi.first.util.struct.Struct; +@SuppressWarnings("doclint") public interface PacketSerde { int getMaxByteSize(); diff --git a/photon-targeting/src/main/java/org/photonvision/common/hardware/Platform.java b/photon-targeting/src/main/java/org/photonvision/common/hardware/Platform.java index 2887521cb..65c1970ba 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/hardware/Platform.java +++ b/photon-targeting/src/main/java/org/photonvision/common/hardware/Platform.java @@ -24,7 +24,7 @@ import java.nio.file.Files; import java.nio.file.Paths; import java.util.function.Supplier; -@SuppressWarnings("unused") +@SuppressWarnings({"unused", "doclint"}) public enum Platform { // WPILib Supported (JNI) WINDOWS_64("Windows x64", Platform::getUnknownModel, false, OSType.WINDOWS, true), diff --git a/photon-targeting/src/main/java/org/photonvision/common/hardware/VisionLEDMode.java b/photon-targeting/src/main/java/org/photonvision/common/hardware/VisionLEDMode.java index d81a2d8c5..de1729b56 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/hardware/VisionLEDMode.java +++ b/photon-targeting/src/main/java/org/photonvision/common/hardware/VisionLEDMode.java @@ -17,6 +17,7 @@ package org.photonvision.common.hardware; +@SuppressWarnings("doclint") public enum VisionLEDMode { kDefault(-1), kOff(0), diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index 6c53ae72d..5b3d50d0f 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -40,6 +40,7 @@ import org.photonvision.targeting.PhotonPipelineResult; *

However, we do expect that the actual logic which fills out values in the entries will be * different for sim vs. real camera */ +@SuppressWarnings("doclint") public class NTTopicSet { public NetworkTable subTable; diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java index 06f702769..1fad03fc4 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java @@ -25,6 +25,7 @@ import java.util.Set; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; +@SuppressWarnings("doclint") public class PacketPublisher implements AutoCloseable { public final RawPublisher publisher; private final PacketSerde photonStruct; diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java index cdd767ea7..f6b7ecb8d 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java @@ -23,6 +23,7 @@ import java.util.List; import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.dataflow.structures.PacketSerde; +@SuppressWarnings("doclint") public class PacketSubscriber implements AutoCloseable { public static class PacketResult { public final U value; diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java b/photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java index b0b915c93..7b64cdf4e 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/CameraTargetRelation.java @@ -22,6 +22,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; /** Holds various helper geometries describing the relation between camera and target. */ +@SuppressWarnings("doclint") public class CameraTargetRelation { public final Pose3d camPose; public final Transform3d camToTarg; diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index 372a5e7cf..4b239f67d 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -17,7 +17,6 @@ package org.photonvision.estimation; -import edu.wpi.first.cscore.OpenCvLoader; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -50,29 +49,39 @@ import org.opencv.imgproc.Imgproc; import org.photonvision.targeting.PnpResult; import org.photonvision.targeting.TargetCorner; +/** + * A set of various utility functions for getting non-OpenCV data into an OpenCV-compatible format, + * along with other calculation functions. + */ public final class OpenCVHelp { private static final Rotation3d NWU_TO_EDN; private static final Rotation3d EDN_TO_NWU; - /** - * @deprecated Replaced by {@link OpenCvLoader#forceStaticLoad()} - */ - @Deprecated(since = "2025", forRemoval = true) - public static void forceLoadOpenCV() { - OpenCvLoader.forceStaticLoad(); - } - static { NWU_TO_EDN = new Rotation3d(MatBuilder.fill(Nat.N3(), Nat.N3(), 0, -1, 0, 0, 0, -1, 1, 0, 0)); EDN_TO_NWU = new Rotation3d(MatBuilder.fill(Nat.N3(), Nat.N3(), 0, 0, 1, -1, 0, 0, 0, -1, 0)); } + private OpenCVHelp() {} + + /** + * Converts an EJML {@link SimpleMatrix} to OpenCV's {@link Mat} by copying the data. + * + * @param matrix The matrix of data. + * @return The {@link Mat}. Data is encoded as 64-bit floats. + */ public static Mat matrixToMat(SimpleMatrix matrix) { var mat = new Mat(matrix.getNumRows(), matrix.getNumCols(), CvType.CV_64F); mat.put(0, 0, matrix.getDDRM().getData()); return mat; } + /** + * Converts an OpenCV {@link Mat} to WPILib's {@link Matrix} by copying the data. + * + * @param mat The {@link Mat} of data. Data is assumed to be encoded as 64-bit floats. + * @return The {@link Matrix}. + */ public static Matrix matToMatrix(Mat mat) { double[] data = new double[(int) mat.total() * mat.channels()]; var doubleMat = new Mat(mat.rows(), mat.cols(), CvType.CV_64F); @@ -82,10 +91,12 @@ public final class OpenCVHelp { } /** - * Creates a new {@link MatOfPoint3f} with these 3d translations. The opencv tvec is a vector with + * Creates a new {@link MatOfPoint3f} with these 3d translations. The OpenCV tvec is a vector with * three elements representing {x, y, z} in the EDN coordinate system. * - * @param translations The translations to convert into a MatOfPoint3f + * @param translations The translations in the NWU coordinate system to convert into a + * MatOfPoint3f + * @return The OpenCV tvec. */ public static MatOfPoint3f translationToTvec(Translation3d... translations) { Point3[] points = new Point3[translations.length]; @@ -97,10 +108,11 @@ public final class OpenCVHelp { } /** - * Returns a new 3d translation from this {@link Mat}. The opencv tvec is a vector with three + * Returns a new 3d translation from this {@link Mat}. The OpenCV tvec is a vector with three * elements representing {x, y, z} in the EDN coordinate system. * * @param tvecInput The tvec to create a Translation3d from + * @return The 3d translation in the NWU coordinate system. */ public static Translation3d tvecToTranslation(Mat tvecInput) { float[] data = new float[3]; @@ -112,11 +124,12 @@ public final class OpenCVHelp { } /** - * Creates a new {@link MatOfPoint3f} with this 3d rotation. The opencv rvec Mat is a vector with + * Creates a new {@link MatOfPoint3f} with this 3d rotation. The OpenCV rvec Mat is a vector with * three elements representing the axis scaled by the angle in the EDN coordinate system. (angle = * norm, and axis = rvec / norm) * * @param rotation The rotation to convert into a MatOfPoint3f + * @return The OpenCV rvec */ public static MatOfPoint3f rotationToRvec(Rotation3d rotation) { rotation = rotationNWUtoEDN(rotation); @@ -124,11 +137,12 @@ public final class OpenCVHelp { } /** - * Returns a 3d rotation from this {@link Mat}. The opencv rvec Mat is a vector with three + * Returns a 3d rotation from this {@link Mat}. The OpenCV rvec Mat is a vector with three * elements representing the axis scaled by the angle in the EDN coordinate system. (angle = norm, * and axis = rvec / norm) * * @param rvecInput The rvec to create a Rotation3d from + * @return The 3d rotation */ public static Rotation3d rvecToRotation(Mat rvecInput) { // Get the 'rodriguez' (axis-angle, where the norm is the angle about the normalized direction @@ -142,6 +156,12 @@ public final class OpenCVHelp { return rotationEDNtoNWU(new Rotation3d(VecBuilder.fill(data[0], data[1], data[2]))); } + /** + * Calculates the average point from an array of points. + * + * @param points The array of points + * @return The average of all the points + */ public static Point avgPoint(Point[] points) { if (points == null || points.length == 0) return null; var pointMat = new MatOfPoint2f(points); @@ -151,6 +171,12 @@ public final class OpenCVHelp { return avgPt; } + /** + * Converts a {@link TargetCorner} list to a {@link Point} array. + * + * @param corners The {@link TargetCorner} list + * @return The {@link Point} array + */ public static Point[] cornersToPoints(List corners) { var points = new Point[corners.size()]; for (int i = 0; i < corners.size(); i++) { @@ -160,6 +186,12 @@ public final class OpenCVHelp { return points; } + /** + * Converts a {@link TargetCorner} array to a {@link Point} array. + * + * @param corners The {@link TargetCorner} array + * @return The {@link Point} array + */ public static Point[] cornersToPoints(TargetCorner... corners) { var points = new Point[corners.length]; for (int i = 0; i < corners.length; i++) { @@ -168,6 +200,12 @@ public final class OpenCVHelp { return points; } + /** + * Converts a {@link Point} array to a {@link TargetCorner} list. + * + * @param points The {@link Point} array + * @return The {@link TargetCorner} list + */ public static List pointsToCorners(Point... points) { var corners = new ArrayList(points.length); for (Point point : points) { @@ -176,6 +214,12 @@ public final class OpenCVHelp { return corners; } + /** + * Converts an OpenCV {@link MatOfPoint2f} to a {@link TargetCorner} list. + * + * @param matInput The Mat + * @return The {@link TargetCorner} list + */ public static List pointsToCorners(MatOfPoint2f matInput) { var corners = new ArrayList(); float[] data = new float[(int) matInput.total() * matInput.channels()]; @@ -220,6 +264,8 @@ public final class OpenCVHelp { /** * Convert a rotation delta from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0} * in EDN, this would be {0, -1, 0} in NWU. + * + * @return The converted rotation in the NWU coordinate system */ private static Rotation3d rotationEDNtoNWU(Rotation3d rot) { return EDN_TO_NWU.unaryMinus().plus(rot.plus(EDN_TO_NWU)); @@ -228,6 +274,8 @@ public final class OpenCVHelp { /** * Convert a rotation delta from NWU to EDN. For example, if you have a rotation X,Y,Z {1, 0, 0} * in NWU, this would be {0, 0, 1} in EDN. + * + * @return The converted rotation in the EDN coordinate system */ private static Rotation3d rotationNWUtoEDN(Rotation3d rot) { return NWU_TO_EDN.unaryMinus().plus(rot.plus(NWU_TO_EDN)); @@ -236,6 +284,8 @@ public final class OpenCVHelp { /** * Convert a translation from EDN to NWU. For example, if you have a translation X,Y,Z {1, 0, 0} * in EDN, this would be {0, -1, 0} in NWU. + * + * @return The converted translation in the NWU coordinate system */ private static Translation3d translationEDNtoNWU(Translation3d trl) { return trl.rotateBy(EDN_TO_NWU); @@ -244,6 +294,8 @@ public final class OpenCVHelp { /** * Convert a translation from NWU to EDN. For example, if you have a translation X,Y,Z {1, 0, 0} * in NWU, this would be {0, 0, 1} in EDN. + * + * @return The converted translation in the EDN coordinate system */ private static Translation3d translationNWUtoEDN(Translation3d trl) { return trl.rotateBy(NWU_TO_EDN); @@ -256,6 +308,7 @@ public final class OpenCVHelp { * @param pointsList the undistorted points * @param cameraMatrix standard OpenCV camera mat * @param distCoeffs standard OpenCV distortion coefficients. Must OPENCV5 or OPENCV8 + * @return the list of distorted points */ public static List distortPoints( List pointsList, Mat cameraMatrix, Mat distCoeffs) { @@ -312,8 +365,8 @@ public final class OpenCVHelp { * Project object points from the 3d world into the 2d camera image. The camera * properties(intrinsics, distortion) determine the results of this projection. * - * @param cameraMatrix the camera intrinsics matrix in standard opencv form - * @param distCoeffs the camera distortion matrix in standard opencv form + * @param cameraMatrix the camera intrinsics matrix in standard OpenCV form + * @param distCoeffs the camera distortion matrix in standard OpenCV form * @param camRt The change in basis from world coordinates to camera coordinates. See {@link * RotTrlTransform3d#makeRelativeTo(Pose3d)}. * @param objectTranslations The 3d points to be projected @@ -324,9 +377,9 @@ public final class OpenCVHelp { Matrix distCoeffs, RotTrlTransform3d camRt, List objectTranslations) { - // translate to opencv classes + // translate to OpenCV classes var objectPoints = translationToTvec(objectTranslations.toArray(new Translation3d[0])); - // opencv rvec/tvec describe a change in basis from world to camera + // OpenCV rvec/tvec describe a change in basis from world to camera var rvec = rotationToRvec(camRt.getRotation()); var tvec = translationToTvec(camRt.getTranslation()); var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage()); @@ -354,8 +407,8 @@ public final class OpenCVHelp { * projectPoints()} will naturally be distorted, so this operation is important if the image * points need to be directly used (e.g. 2d yaw/pitch). * - * @param cameraMatrix The camera intrinsics matrix in standard opencv form - * @param distCoeffs The camera distortion matrix in standard opencv form + * @param cameraMatrix The camera intrinsics matrix in standard OpenCV form + * @param distCoeffs The camera distortion matrix in standard OpenCV form * @param points The distorted image points * @return The undistorted image points */ @@ -444,8 +497,8 @@ public final class OpenCVHelp { *

This method is intended for use with individual AprilTags, and will not work unless 4 points * are provided. * - * @param cameraMatrix The camera intrinsics matrix in standard opencv form - * @param distCoeffs The camera distortion matrix in standard opencv form + * @param cameraMatrix The camera intrinsics matrix in standard OpenCV form + * @param distCoeffs The camera distortion matrix in standard OpenCV form * @param modelTrls The translations of the object corners. These should have the object pose as * their origin. These must come in a specific, pose-relative order (in NWU): *

    @@ -479,7 +532,7 @@ public final class OpenCVHelp { // IPPE_SQUARE expects our corners in a specific order modelTrls = reorderCircular(modelTrls, true, -1); imagePoints = reorderCircular(Arrays.asList(imagePoints), true, -1).toArray(Point[]::new); - // translate to opencv classes + // translate to OpenCV classes translationToTvec(modelTrls.toArray(new Translation3d[0])).assignTo(objectMat); imageMat.fromArray(imagePoints); matrixToMat(cameraMatrix.getStorage()).assignTo(cameraMatrixMat); @@ -559,8 +612,8 @@ public final class OpenCVHelp { * solution-- if you are intending to use solvePNP on a single AprilTag, see {@link * #solvePNP_SQUARE} instead. * - * @param cameraMatrix The camera intrinsics matrix in standard opencv form - * @param distCoeffs The camera distortion matrix in standard opencv form + * @param cameraMatrix The camera intrinsics matrix in standard OpenCV form + * @param distCoeffs The camera distortion matrix in standard OpenCV form * @param objectTrls The translations of the object corners, relative to the field. * @param imagePoints The projection of these 3d object points into the 2d camera image. The order * should match the given object point translations. @@ -574,7 +627,7 @@ public final class OpenCVHelp { List objectTrls, Point[] imagePoints) { try { - // translate to opencv classes + // translate to OpenCV classes MatOfPoint3f objectMat = translationToTvec(objectTrls.toArray(new Translation3d[0])); MatOfPoint2f imageMat = new MatOfPoint2f(imagePoints); Mat cameraMatrixMat = matrixToMat(cameraMatrix.getStorage()); diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java b/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java index 11bd52ff9..c3e93c8eb 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java @@ -62,6 +62,7 @@ public class RotTrlTransform3d { this(trf.getRotation(), trf.getTranslation()); } + /** Constructs the identity transform -- maps an initial pose to itself. */ public RotTrlTransform3d() { this(new Rotation3d(), new Translation3d()); } @@ -76,24 +77,40 @@ public class RotTrlTransform3d { return new RotTrlTransform3d(pose.getRotation(), pose.getTranslation()).inverse(); } - /** The inverse of this transformation. Applying the inverse will "undo" this transformation. */ + /** + * The inverse of this transformation. Applying the inverse will "undo" this transformation. + * + * @return The inverse transformation + */ public RotTrlTransform3d inverse() { var inverseRot = rot.unaryMinus(); var inverseTrl = trl.rotateBy(inverseRot).unaryMinus(); return new RotTrlTransform3d(inverseRot, inverseTrl); } - /** This transformation as a Transform3d (as if of the origin) */ + /** + * This transformation as a Transform3d (as if of the origin) + * + * @return The Transform2d + */ public Transform3d getTransform() { return new Transform3d(trl, rot); } - /** The translation component of this transformation */ + /** + * The translation component of this transformation + * + * @return The Translation3d + */ public Translation3d getTranslation() { return trl; } - /** The rotation component of this transformation */ + /** + * The rotation component of this transformation + * + * @return The Rotation3d + */ public Rotation3d getRotation() { return rot; } diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java index b06d5092d..0435a62bd 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java @@ -34,11 +34,19 @@ public class TargetModel { */ public final List vertices; + /** True if the target is planar, like an AprilTag. False otherwise. */ public final boolean isPlanar; + + /** True if the target is spherical, like a ball. False otherwise. */ public final boolean isSpherical; + /** The model of AprilTags in the 16h5 family used by FIRST in FRC 2023. */ public static final TargetModel kAprilTag16h5 = new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6)); + + /** + * The model of AprilTags in the 36h11 family used by FIRST in FRC 2024 and later years. + */ public static final TargetModel kAprilTag36h11 = new TargetModel(Units.inchesToMeters(6.5), Units.inchesToMeters(6.5)); @@ -148,6 +156,9 @@ public class TargetModel { * *

    Note: If this target is spherical, use {@link #getOrientedPose(Translation3d, * Translation3d)} with this method. + * + * @param targetPose The field pose to offset the vertices by. + * @return The list of vertices offset from the target pose. */ public List getFieldVertices(Pose3d targetPose) { var basisChange = new RotTrlTransform3d(targetPose.getRotation(), targetPose.getTranslation()); diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java index fee42fa18..98229ac6a 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -45,7 +45,15 @@ import org.photonvision.targeting.PnpResult; import org.photonvision.targeting.TargetCorner; public class VisionEstimation { - /** Get the visible {@link AprilTag}s which are in the tag layout using the visible tag IDs. */ + private VisionEstimation() {} + + /** + * Get the list of visible {@link AprilTag}s which are in the tag layout using the visible tag + * IDs. + * + * @param visTags The list of targets to search for visible tags. + * @param tagLayout The tag layout to search + */ public static List getVisibleLayoutTags( List visTags, AprilTagFieldLayout tagLayout) { return visTags.stream() @@ -74,6 +82,7 @@ public class VisionEstimation { * @param distCoeffs The camera distortion matrix in standard opencv form * @param visTags The visible tags reported by PV. Non-tag targets are automatically excluded. * @param tagLayout The known tag layout on the field + * @param tagModel The model describing the tag's geometry * @return The transformation that maps the field origin to the camera pose. Ensure the {@link * PnpResult} are present before utilizing them. */ diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java index f38b13c5d..da105ba90 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java @@ -30,8 +30,11 @@ public class MultiTargetPNPResult private static final int MAX_IDS = 32; public PnpResult estimatedPose = new PnpResult(); + + /** The fiducial IDs used to calculate this multi-target result. */ public List fiducialIDsUsed = List.of(); + /** Used for serialization and tests. */ public MultiTargetPNPResult() {} public MultiTargetPNPResult(PnpResult results, List ids) { @@ -72,9 +75,10 @@ public class MultiTargetPNPResult + "]"; } + /** MultiTargetPNPResult protobuf for serialization. */ public static final MultiTargetPNPResultProto proto = new MultiTargetPNPResultProto(); - // tODO! + /** MultiTargetPNPResult PhotonStruct for serialization. */ public static final MultiTargetPNPResultSerde photonStruct = new MultiTargetPNPResultSerde(); @Override diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java index c856ba3e4..9c6846829 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineMetadata.java @@ -48,17 +48,29 @@ public class PhotonPipelineMetadata implements PhotonStructSerializable { private static boolean HAS_WARNED = false; - // Frame capture metadata + /** Frame capture metadata. */ public PhotonPipelineMetadata metadata; - // Targets to store. + /** The list of targets detected by the pipeline. */ public List targets = new ArrayList<>(); - // Multi-tag result + /** The multitag result, if using an AprilTag pipeline with Multi-Target Estimation enabled. */ public Optional multitagResult; /** Constructs an empty pipeline result. */ @@ -53,6 +53,7 @@ public class PhotonPipelineResult * coprocessor captured the image this result contains the targeting info of * @param publishTimestampMicros The time, in uS in the coprocessor's timebase, that the * coprocessor published targeting info + * @param timeSinceLastPong The time since the last Time Sync Pong in uS. * @param targets The list of targets identified by the pipeline. */ public PhotonPipelineResult( @@ -76,6 +77,7 @@ public class PhotonPipelineResult * captured the image this result contains the targeting info of * @param publishTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor * published targeting info + * @param timeSinceLastPong The time since the last Time Sync Pong in uS. * @param targets The list of targets identified by the pipeline. * @param result Result from multi-target PNP. */ @@ -155,8 +157,11 @@ public class PhotonPipelineResult } /** - * Return the latest multi-target result. Be sure to check - * getMultiTagResult().estimatedPose.isPresent before using the pose estimate! + * Return the latest multi-target result. Be sure to check {@code getMultiTagResult().isPresent()} + * before using the pose estimate! + * + * @return The multi-target result. Empty if there's no multi-target result/Multi-Target + * Estimation is disabled in the UI. */ public Optional getMultiTagResult() { return multitagResult; @@ -212,7 +217,10 @@ public class PhotonPipelineResult return true; } + /** PhotonPipelineResult PhotonStruct for serialization. */ public static final PhotonPipelineResultSerde photonStruct = new PhotonPipelineResultSerde(); + + /** PhotonPipelineResult Protobuf for serialization. */ public static final PhotonPipelineResultProto proto = new PhotonPipelineResultProto(); @Override 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 be58da17f..58b4c8d12 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -25,28 +25,63 @@ import org.photonvision.struct.PhotonTrackedTargetSerde; import org.photonvision.targeting.proto.PhotonTrackedTargetProto; import org.photonvision.targeting.serde.PhotonStructSerializable; +/** Information about a detected target. */ public class PhotonTrackedTarget implements ProtobufSerializable, PhotonStructSerializable { private static final int MAX_CORNERS = 8; + /** The yaw of the target in degrees, with left being the positive direction. */ public double yaw; + + /** The pitch of the target in degrees, with up being the positive direction. */ public double pitch; + + /** The area (how much of the camera feed the bounding box takes up) as a percentage (0-100). */ public double area; + + /** The skew of the target in degrees, with counterclockwise being the positive direction. */ public double skew; + + /** The fiducial ID, or -1 if it doesn't exist for this target. */ public int fiducialId; + + /** The object detection class ID, or -1 if it doesn't exist for this target. */ public int objDetectId; + + /** The object detection confidence, or -1 if it doesn't exist for this target. */ public float objDetectConf; + + /** The transform with the lowest reprojection error */ public Transform3d bestCameraToTarget; + + /** The transform with the highest reprojection error */ public Transform3d altCameraToTarget; + + /** The ratio (best:alt) of reprojection errors */ public double poseAmbiguity; - // Corners from the min-area rectangle bounding the target + /** Corners from the min-area rectangle bounding the target */ public List minAreaRectCorners; - // Corners from whatever corner detection method was used + /** Corners from the corner detection method used */ public List detectedCorners; - /** Construct a tracked target, given exactly 4 corners */ + /** + * Construct a tracked target, given exactly 4 corners + * + * @param yaw The yaw of the target + * @param pitch The pitch of the target + * @param area The area of the target as a percentage of the camera image + * @param skew The skew of the target + * @param fiducialId The fiduical tag ID + * @param classId The object detection class ID + * @param objDetectConf The object detection confidence + * @param pose The best camera to target transform + * @param altPose The alternate camera to target transform + * @param ambiguity The ambiguity (best:alternate ratio of reprojection errors) of the target + * @param minAreaRectCorners The corners of minimum area bounding box of the target + * @param detectedCorners The detected corners of the target + */ public PhotonTrackedTarget( double yaw, double pitch, @@ -80,9 +115,8 @@ public class PhotonTrackedTarget this.poseAmbiguity = ambiguity; } - public PhotonTrackedTarget() { - // TODO Auto-generated constructor stub - } + /** Used for serialization. */ + public PhotonTrackedTarget() {} public double getYaw() { return yaw; @@ -92,6 +126,11 @@ public class PhotonTrackedTarget return pitch; } + /** + * The area (how much of the camera feed the bounding box takes up) as a percentage (0-100). + * + * @return The area as a percentage + */ public double getArea() { return area; } @@ -100,19 +139,29 @@ public class PhotonTrackedTarget return skew; } - /** Get the fiducial ID, or -1 if not set. */ + /** + * Get the fiducial ID, or -1 if it doesn't exist for this target. + * + * @return The fiducial ID + */ public int getFiducialId() { return fiducialId; } - /** Get the object detection class ID number, or -1 if not set. */ + /** + * Get the object detection class ID number, or -1 if it doesn't exist for this target. + * + * @return The object detection class ID + */ public int getDetectedObjectClassID() { return objDetectId; } /** - * Get the object detection confidence, or -1 if not set. This will be between 0 and 1, with 1 - * indicating most confidence, and 0 least. + * Get the object detection confidence, or -1 if it doesn't exist for this target. This will be + * between 0 and 1, with 1 indicating most confidence, and 0 least. + * + * @return The object detection confidence */ public float getDetectedObjectConfidence() { return objDetectConf; @@ -122,6 +171,8 @@ public class PhotonTrackedTarget * Get the ratio of best:alternate pose reprojection errors, called ambiguity. This is between 0 * and 1 (0 being no ambiguity, and 1 meaning both have the same reprojection error). Numbers * above 0.2 are likely to be ambiguous. -1 if invalid. + * + * @return The pose ambiguity */ public double getPoseAmbiguity() { return poseAmbiguity; @@ -130,6 +181,8 @@ public class PhotonTrackedTarget /** * Return a list of the 4 corners in image space (origin top left, x right, y down), in no * particular order, of the minimum area bounding rectangle of this target + * + * @return The list of corners */ public List getMinAreaRectCorners() { return minAreaRectCorners; @@ -147,6 +200,8 @@ public class PhotonTrackedTarget * V | | * +Y 0 ----- 1 *

+ * + * @return The list of detected corners for this target */ public List getDetectedCorners() { return detectedCorners; @@ -155,6 +210,8 @@ public class PhotonTrackedTarget /** * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag * space (X forward, Y left, Z up) with the lowest reprojection error + * + * @return The transform with the lowest reprojection error (the best) */ public Transform3d getBestCameraToTarget() { return bestCameraToTarget; @@ -163,6 +220,8 @@ public class PhotonTrackedTarget /** * Get the transform that maps camera space (X = forward, Y = left, Z = up) to object/fiducial tag * space (X forward, Y left, Z up) with the highest reprojection error + * + * @return The transform with the highest reprojection error (the alternate) */ public Transform3d getAlternateCameraToTarget() { return altCameraToTarget; @@ -253,7 +312,10 @@ public class PhotonTrackedTarget + "]"; } + /** PhotonTrackedTarget protobuf for serialization. */ public static final PhotonTrackedTargetProto proto = new PhotonTrackedTargetProto(); + + /** PhotonTrackedTarget PhotonStruct for serialization. */ public static final PhotonTrackedTargetSerde photonStruct = new PhotonTrackedTargetSerde(); @Override diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PnpResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PnpResult.java index 63e00809a..6be6ff44d 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PnpResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PnpResult.java @@ -33,6 +33,7 @@ import org.photonvision.targeting.serde.PhotonStructSerializable; *

Note that the coordinate frame of these transforms depends on the implementing solvePnP * method. */ +@SuppressWarnings("doclint") public class PnpResult implements ProtobufSerializable, PhotonStructSerializable { /** * The best-fit transform. The coordinate frame of this transform depends on the method which gave @@ -133,7 +134,10 @@ public class PnpResult implements ProtobufSerializable, PhotonStructSerializable + "]"; } + /** PnpResult protobuf for serialization. */ public static final PNPResultProto proto = new PNPResultProto(); + + /** PnpResult PhotonStruct for serialization. */ public static final PnpResultSerde photonStruct = new PnpResultSerde(); @Override diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index 7b29ea8d9..0ca122255 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -28,6 +28,7 @@ import org.photonvision.targeting.serde.PhotonStructSerializable; * Represents a point in an image at the corner of the minimum-area bounding rectangle, in pixels. * Origin at the top left, plus-x to the right, plus-y down. */ +@SuppressWarnings("doclint") public class TargetCorner implements ProtobufSerializable, PhotonStructSerializable { public double x; public double y; @@ -59,7 +60,10 @@ public class TargetCorner implements ProtobufSerializable, PhotonStructSerializa return "(" + x + "," + y + ')'; } + /** TargetCorner protobuf for serialization. */ public static final TargetCornerProto proto = new TargetCornerProto(); + + /** TargetCorner PhotonStruct for serialization. */ public static final TargetCornerSerde photonStruct = new TargetCornerSerde(); @Override diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/serde/PhotonStructSerializable.java b/photon-targeting/src/main/java/org/photonvision/targeting/serde/PhotonStructSerializable.java index 4d9c842d7..2e87a22cb 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/serde/PhotonStructSerializable.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/serde/PhotonStructSerializable.java @@ -20,5 +20,10 @@ package org.photonvision.targeting.serde; import org.photonvision.common.dataflow.structures.PacketSerde; public interface PhotonStructSerializable { + /** + * Returns the PhotonStruct serializer for this type. + * + * @return The PhotonStruct serializer + */ PacketSerde getSerde(); } diff --git a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java index b90de1353..187cfc0ef 100644 --- a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java +++ b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java @@ -20,6 +20,7 @@ package org.photonvision.utils; import edu.wpi.first.math.geometry.*; import org.photonvision.common.dataflow.structures.Packet; +@SuppressWarnings("doclint") public class PacketUtils { public static final int ROTATION2D_BYTE_SIZE = Double.BYTES; public static final int QUATERNION_BYTE_SIZE = Double.BYTES * 4; diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h index 614c17947..b7cf66035 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -94,7 +94,10 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { } /** - * Returns the estimated time the frame was taken, + * Returns the estimated time the frame was taken, in the Time Sync Server's + * time base (nt::Now). This is calculated using the estimated offset between + * Time Sync Server time and local time. The robot shall run a server, so the + * offset shall be 0. * This is much more accurate than using GetLatency() * @return The timestamp in seconds or -1 if this result was not initiated * with a timestamp. @@ -105,8 +108,11 @@ class PhotonPipelineResult : public PhotonPipelineResult_PhotonStruct { /** * Return the latest mulit-target result, as calculated on your coprocessor. - * Be sure to check getMultiTagResult().estimatedPose.isPresent before using + * Be sure to check `MultiTagResult().has_value()` before using * the pose estimate! + * + * @return The multi-target result. Empty if there's no multi-target + * result/Multi-Target Estimation is disabled in the UI. */ const std::optional& MultiTagResult() const { return multitagResult; diff --git a/shared/PhotonVersion.java.in b/shared/PhotonVersion.java.in index 34797d361..4b90c5b64 100644 --- a/shared/PhotonVersion.java.in +++ b/shared/PhotonVersion.java.in @@ -32,7 +32,7 @@ package org.photonvision; import java.util.regex.Matcher; import java.util.regex.Pattern; -@SuppressWarnings("ALL") +@SuppressWarnings("doclint") public final class PhotonVersion { public static final String versionString = "${version}"; public static final String buildDate = "${date}";