Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

[WIP] Simulation Overhaul (#742)

Browse Source 
### What does this do?

- Deprecates previous sim classes
- Has a `CameraProperties` class for describing a camera's basic/calibration info, and performance values for simulation. Calibration values can be loaded from the `config.json` in the settings exported by photonvision.
- `OpenCVHelp` provides convenience functions for using opencv methods with wpilib/photonvision classes, mainly to project 3d points to a camera's 2d image and perform solvePnP with the above camera calibration info.
  - `TargetModel`s describe the 3d shape of a target, both for projecting into the camera's 2d image and use in solvePnP.
- `PhotonCameraSim` uses camera properties to simulate how 3d targets would appear in its view, and has simulated noise, latency, and FPS. For apriltags, the best/alternate camera-to-target transform is also estimated with solvePnP.
  - `VideoSimUtil` has helper functions for drawing apriltags to a simulated raw and processed MJPEG stream for each camera using the projected tag corners.
- `VisionSystemSim` stores `VisionTargetSim`s and `PhotonCameraSim`s, and is periodically updated with the robot's simulated pose. When updating, camera sims are automatically processed and published with their visible targets from their respective poses with proper latency.

### What's still not working?

- Mac Arm builds are broken
- More examples
- Update website/docs
This commit is contained in:
amquake
2023-06-18 15:54:12 -07:00
committed by GitHub
parent 4a94775639
commit f1cadc1e1e
56 changed files with 2471 additions and 199 deletions
Download Patch File Download Diff File Expand all files Collapse all files

464
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java Normal file
View File

@@ -0,0 +1,464 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.simulation;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.cscore.CvSource;
import edu.wpi.first.cscore.VideoMode.PixelFormat;
import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.util.RuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonTargetSortMode;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.estimation.CameraTargetRelation;
import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.estimation.PNPResults;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
/**
* A handle for simulating {@link PhotonCamera} values. Processing simulated targets through this
* class will change the associated PhotonCamera's results.
*/
@SuppressWarnings("unused")
public class PhotonCameraSim implements AutoCloseable {
private final PhotonCamera cam;
NTTopicSet ts = new NTTopicSet();
private long heartbeatCounter = 0;
/** This simulated camera's {@link SimCameraProperties} */
public final SimCameraProperties prop;
private long nextNTEntryTime = WPIUtilJNI.now();
private double maxSightRangeMeters = Double.MAX_VALUE;
private static final double kDefaultMinAreaPx = 90;
private double minTargetAreaPercent;
private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest;
// video stream simulation
private final CvSource videoSimRaw;
private final Mat videoSimFrameRaw = new Mat();
private boolean videoSimRawEnabled = true;
private final CvSource videoSimProcessed;
private final Mat videoSimFrameProcessed = new Mat();
private boolean videoSimProcEnabled = true;
static {
try {
var loader =
new RuntimeLoader<>(
Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
loader.loadLibrary();
} catch (Exception e) {
throw new RuntimeException("Failed to load native libraries!", e);
}
}
@Override
public void close() {
videoSimRaw.close();
videoSimFrameRaw.release();
videoSimProcessed.close();
videoSimFrameProcessed.release();
}
/**
* Constructs a handle for simulating {@link PhotonCamera} values. Processing simulated targets
* through this class will change the associated PhotonCamera's results.
*
* <p><b>This constructor's camera has a 90 deg FOV with no simulated lag!</b>
*
* <p>By default, the minimum target area is 100 pixels and there is no maximum sight range.
*
* @param camera The camera to be simulated
*/
public PhotonCameraSim(PhotonCamera camera) {
this(camera, SimCameraProperties.PERFECT_90DEG());
}
/**
* Constructs a handle for simulating {@link PhotonCamera} values. Processing simulated targets
* through this class will change the associated PhotonCamera's results.
*
* <p>By default, the minimum target area is 100 pixels and there is no maximum sight range.
*
* @param camera The camera to be simulated
* @param prop Properties of this camera such as FOV and FPS
*/
public PhotonCameraSim(PhotonCamera camera, SimCameraProperties prop) {
this.cam = camera;
this.prop = prop;
setMinTargetAreaPixels(kDefaultMinAreaPx);
videoSimRaw =
CameraServer.putVideo(camera.getName() + "-raw", prop.getResWidth(), prop.getResHeight());
videoSimRaw.setPixelFormat(PixelFormat.kGray);
videoSimProcessed =
CameraServer.putVideo(
camera.getName() + "-processed", prop.getResWidth(), prop.getResHeight());
ts.removeEntries();
ts.subTable = camera.getCameraTable();
ts.updateEntries();
}
/**
* Constructs a handle for simulating {@link PhotonCamera} values. Processing simulated targets
* through this class will change the associated PhotonCamera's results.
*
* @param camera The camera to be simulated
* @param prop Properties of this camera such as FOV and FPS
* @param minTargetAreaPercent The minimum percentage(0 - 100) a detected target must take up of
* the camera's image to be processed. Match this with your contour filtering settings in the
* PhotonVision GUI.
* @param maxSightRangeMeters Maximum distance at which the target is illuminated to your camera.
* Note that minimum target area of the image is separate from this.
*/
public PhotonCameraSim(
PhotonCamera camera,
SimCameraProperties prop,
double minTargetAreaPercent,
double maxSightRangeMeters) {
this(camera, prop);
this.minTargetAreaPercent = minTargetAreaPercent;
this.maxSightRangeMeters = maxSightRangeMeters;
}
public PhotonCamera getCamera() {
return cam;
}
public double getMinTargetAreaPercent() {
return minTargetAreaPercent;
}
public double getMinTargetAreaPixels() {
return minTargetAreaPercent / 100.0 * prop.getResArea();
}
public double getMaxSightRangeMeters() {
return maxSightRangeMeters;
}
public PhotonTargetSortMode getTargetSortMode() {
return sortMode;
}
public CvSource getVideoSimRaw() {
return videoSimRaw;
}
public Mat getVideoSimFrameRaw() {
return videoSimFrameRaw;
}
/**
* Determines if this target's pose should be visible to the camera without considering its
* projected image points. Does not account for image area.
*
* @param camPose Camera's 3d pose
* @param target Vision target containing pose and shape
* @return If this vision target can be seen before image projection.
*/
public boolean canSeeTargetPose(Pose3d camPose, VisionTargetSim target) {
var rel = new CameraTargetRelation(camPose, target.getPose());
return (
// target translation is outside of camera's FOV
(Math.abs(rel.camToTargYaw.getDegrees()) < prop.getHorizFOV().getDegrees() / 2)
&& (Math.abs(rel.camToTargPitch.getDegrees()) < prop.getVertFOV().getDegrees() / 2)
&& (!target.getModel().isPlanar
|| Math.abs(rel.targToCamAngle.getDegrees())
< 90) // camera is behind planar target and it should be occluded
&& (rel.camToTarg.getTranslation().getNorm() <= maxSightRangeMeters)); // target is too far
}
/**
* Determines if all target corners are inside the camera's image.
*
* @param corners The corners of the target as image points(x,y)
*/
public boolean canSeeCorners(List<TargetCorner> corners) {
// corner is outside of resolution
for (var corner : corners) {
if (MathUtil.clamp(corner.x, 0, prop.getResWidth()) != corner.x
|| MathUtil.clamp(corner.y, 0, prop.getResHeight()) != corner.y) {
return false;
}
}
return true;
}
/**
* Determine if this camera should process a new frame based on performance metrics and the time
* since the last update. This returns an Optional which is either empty if no update should occur
* or a Long of the timestamp in microseconds of when the frame which should be received by NT. If
* a timestamp is returned, the last frame update time becomes that timestamp.
*
* @return Optional long which is empty while blocked or the NT entry timestamp in microseconds if
* ready
*/
public Optional<Long> consumeNextEntryTime() {
// check if this camera is ready for another frame update
long now = WPIUtilJNI.now();
long timestamp = -1;
int iter = 0;
// prepare next latest update
while (now >= nextNTEntryTime) {
timestamp = nextNTEntryTime;
long frameTime = (long) (prop.estMsUntilNextFrame() * 1e3);
nextNTEntryTime += frameTime;
// if frame time is very small, avoid blocking
if (iter++ > 50) {
timestamp = now;
nextNTEntryTime = now + frameTime;
break;
}
}
// return the timestamp of the latest update
if (timestamp >= 0) return Optional.of(timestamp);
// or this camera isn't ready to process yet
else return Optional.empty();
}
/**
* The minimum percentage(0 - 100) a detected target must take up of the camera's image to be
* processed.
*/
public void setMinTargetAreaPercent(double areaPercent) {
this.minTargetAreaPercent = areaPercent;
}
/**
* The minimum number of pixels a detected target must take up in the camera's image to be
* processed.
*/
public void setMinTargetAreaPixels(double areaPx) {
this.minTargetAreaPercent = areaPx / prop.getResArea() * 100;
}
/**
* Maximum distance at which the target is illuminated to your camera. Note that minimum target
* area of the image is separate from this.
*/
public void setMaxSightRange(double rangeMeters) {
this.maxSightRangeMeters = rangeMeters;
}
/** Defines the order the targets are sorted in the pipeline result. */
public void setTargetSortMode(PhotonTargetSortMode sortMode) {
if (sortMode != null) this.sortMode = sortMode;
}
/** Sets whether the raw video stream simulation is enabled. */
public void enableRawStream(boolean enabled) {
videoSimRawEnabled = enabled;
}
/** Sets whether the processed video stream simulation is enabled. */
public void enableProcessedStream(boolean enabled) {
videoSimProcEnabled = enabled;
}
public PhotonPipelineResult process(
double latencyMillis, Pose3d cameraPose, List<VisionTargetSim> targets) {
// sort targets by distance to camera
targets = new ArrayList<>(targets);
targets.sort(
(t1, t2) -> {
double dist1 = t1.getPose().getTranslation().getDistance(cameraPose.getTranslation());
double dist2 = t2.getPose().getTranslation().getDistance(cameraPose.getTranslation());
if (dist1 == dist2) return 0;
return dist1 < dist2 ? 1 : -1;
});
// all targets visible (in FOV)
var visibleTags = new ArrayList<Pair<Integer, List<TargetCorner>>>();
// all targets actually detectable to the camera
var detectableTgts = new ArrayList<PhotonTrackedTarget>();
// reset our frame
VideoSimUtil.updateVideoProp(videoSimRaw, prop);
VideoSimUtil.updateVideoProp(videoSimProcessed, prop);
Size videoFrameSize = new Size(prop.getResWidth(), prop.getResHeight());
Mat.zeros(videoFrameSize, CvType.CV_8UC1).assignTo(videoSimFrameRaw);
for (var tgt : targets) {
// pose isn't visible, skip to next
if (!canSeeTargetPose(cameraPose, tgt)) continue;
// find target's 3d corner points
// TODO: Handle spherical targets
var fieldCorners = tgt.getFieldVertices();
// project 3d target points into 2d image points
var targetCorners =
OpenCVHelp.projectPoints(
prop.getIntrinsics(), prop.getDistCoeffs(), cameraPose, fieldCorners);
// save visible tags for stream simulation
if (tgt.fiducialID >= 0) {
visibleTags.add(new Pair<>(tgt.fiducialID, targetCorners));
}
// estimate pixel noise
var noisyTargetCorners = prop.estPixelNoise(targetCorners);
// find the (naive) 2d yaw/pitch
var centerPt = OpenCVHelp.getMinAreaRect(noisyTargetCorners).center;
var centerRot = prop.getPixelRot(new TargetCorner(centerPt.x, centerPt.y));
// find contour area
double areaPercent = prop.getContourAreaPercent(noisyTargetCorners);
// projected target can't be detected, skip to next
if (!(canSeeCorners(noisyTargetCorners) && areaPercent >= minTargetAreaPercent)) continue;
var pnpSim = new PNPResults();
if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP
pnpSim =
OpenCVHelp.solvePNP_SQUARE(
prop.getIntrinsics(),
prop.getDistCoeffs(),
tgt.getModel().vertices,
noisyTargetCorners);
centerRot =
prop.getPixelRot(
OpenCVHelp.projectPoints(
prop.getIntrinsics(),
prop.getDistCoeffs(),
new Pose3d(),
List.of(pnpSim.best.getTranslation()))
.get(0));
}
Point[] minAreaRectPts = new Point[noisyTargetCorners.size()];
OpenCVHelp.getMinAreaRect(noisyTargetCorners).points(minAreaRectPts);
detectableTgts.add(
new PhotonTrackedTarget(
Math.toDegrees(centerRot.getZ()),
-Math.toDegrees(centerRot.getY()),
areaPercent,
Math.toDegrees(centerRot.getX()),
tgt.fiducialID,
pnpSim.best,
pnpSim.alt,
pnpSim.ambiguity,
List.of(OpenCVHelp.pointsToTargetCorners(minAreaRectPts)),
noisyTargetCorners));
}
// render visible tags to raw video frame
if (videoSimRawEnabled) {
for (var tag : visibleTags) {
VideoSimUtil.warp16h5TagImage(
tag.getFirst(), OpenCVHelp.targetCornersToMat(tag.getSecond()), videoSimFrameRaw, true);
}
videoSimRaw.putFrame(videoSimFrameRaw);
} else videoSimRaw.setConnectionStrategy(ConnectionStrategy.kForceClose);
// draw/annotate tag detection outline on processed view
if (videoSimProcEnabled) {
Imgproc.cvtColor(videoSimFrameRaw, videoSimFrameProcessed, Imgproc.COLOR_GRAY2BGR);
for (var tgt : detectableTgts) {
if (tgt.getFiducialId() >= 0) {
VideoSimUtil.drawTagDetection(
tgt.getFiducialId(),
OpenCVHelp.targetCornersToMat(tgt.getDetectedCorners()),
videoSimFrameProcessed);
}
}
videoSimProcessed.putFrame(videoSimFrameProcessed);
} else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose);
// put this simulated data to NT
if (sortMode != null) {
detectableTgts.sort(sortMode.getComparator());
}
return new PhotonPipelineResult(latencyMillis, detectableTgts);
}
/**
* Simulate one processed frame of vision data, putting one result to NT at current timestamp.
* Image capture time is assumed be (current time - latency).
*
* @param result The pipeline result to submit
*/
public void submitProcessedFrame(PhotonPipelineResult result) {
submitProcessedFrame(result, WPIUtilJNI.now());
}
/**
* Simulate one processed frame of vision data, putting one result to NT. Image capture timestamp
* overrides {@link PhotonPipelineResult#getTimestampSeconds() getTimestampSeconds()} for more
* precise latency simulation.
*
* @param result The pipeline result to submit
* @param receiveTimestamp The (sim) timestamp when this result was read by NT in microseconds
*/
public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimestamp) {
ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp);
var newPacket = new Packet(result.getPacketSize());
result.populatePacket(newPacket);
ts.rawBytesEntry.set(newPacket.getData(), receiveTimestamp);
boolean hasTargets = result.hasTargets();
ts.hasTargetEntry.set(hasTargets, receiveTimestamp);
if (!hasTargets) {
ts.targetPitchEntry.set(0.0, receiveTimestamp);
ts.targetYawEntry.set(0.0, receiveTimestamp);
ts.targetAreaEntry.set(0.0, receiveTimestamp);
ts.targetPoseEntry.set(new double[] {0.0, 0.0, 0.0}, receiveTimestamp);
ts.targetSkewEntry.set(0.0, receiveTimestamp);
} else {
var bestTarget = result.getBestTarget();
ts.targetPitchEntry.set(bestTarget.getPitch(), receiveTimestamp);
ts.targetYawEntry.set(bestTarget.getYaw(), receiveTimestamp);
ts.targetAreaEntry.set(bestTarget.getArea(), receiveTimestamp);
ts.targetSkewEntry.set(bestTarget.getSkew(), receiveTimestamp);
var transform = bestTarget.getBestCameraToTarget();
double[] poseData = {
transform.getX(), transform.getY(), transform.getRotation().toRotation2d().getDegrees()
};
ts.targetPoseEntry.set(poseData, receiveTimestamp);
}
ts.heartbeatPublisher.set(heartbeatCounter++, receiveTimestamp);
}
}

601
photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java Normal file
View File

@@ -0,0 +1,601 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.simulation;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.numbers.*;
import edu.wpi.first.wpilibj.DriverStation;
import java.io.IOException;
import java.nio.file.Path;
import java.util.List;
import java.util.Random;
import java.util.stream.Collectors;
import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.targeting.TargetCorner;
/**
* Calibration and performance values for this camera.
*
* <p>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).
*
* <p>The camera intrinsics and distortion coefficients describe the results of calibration, and how
* to map between 3d field points and 2d image points.
*
* <p>The performance values (framerate/exposure time, latency) determine how often results should
* be updated and with how much latency in simulation. High exposure time causes motion blur which
* can inhibit target detection while moving. Note that latency estimation does not account for
* network latency and the latency reported will always be perfect.
*/
public class SimCameraProperties {
private final Random rand = new Random();
// calibration
private int resWidth;
private int resHeight;
private Matrix<N3, N3> camIntrinsics;
private Matrix<N5, N1> distCoeffs;
private double avgErrorPx;
private double errorStdDevPx;
// performance
private double frameSpeedMs = 0;
private double exposureTimeMs = 0;
private double avgLatencyMs = 0;
private double latencyStdDevMs = 0;
/** Default constructor which is the same as {@link #PERFECT_90DEG} */
public SimCameraProperties() {
setCalibration(960, 720, Rotation2d.fromDegrees(90));
}
/**
* Reads camera properties from a photonvision <code>config.json</code> 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 <code>config.json</code> file
* @param width The width of the desired resolution in the JSON
* @param height The height of the desired resolution in the JSON
* @throws IOException If reading the JSON fails, either from an invalid path or a missing/invalid
* calibrated resolution.
*/
public SimCameraProperties(String path, int width, int height) throws IOException {
this(Path.of(path), width, height);
}
/**
* Reads camera properties from a photonvision <code>config.json</code> 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 <code>config.json</code> file
* @param width The width of the desired resolution in the JSON
* @param height The height of the desired resolution in the JSON
* @throws IOException If reading the JSON fails, either from an invalid path or a missing/invalid
* calibrated resolution.
*/
public SimCameraProperties(Path path, int width, int height) throws IOException {
var mapper = new ObjectMapper();
var json = mapper.readTree(path.toFile());
json = json.get("calibrations");
boolean success = false;
try {
for (int i = 0; i < json.size() && !success; i++) {
// check if this calibration entry is our desired resolution
var calib = json.get(i);
int jsonWidth = calib.get("resolution").get("width").asInt();
int jsonHeight = calib.get("resolution").get("height").asInt();
if (jsonWidth != width || jsonHeight != height) continue;
// get the relevant calibration values
var jsonIntrinsicsNode = calib.get("cameraIntrinsics").get("data");
double[] jsonIntrinsics = new double[jsonIntrinsicsNode.size()];
for (int j = 0; j < jsonIntrinsicsNode.size(); j++) {
jsonIntrinsics[j] = jsonIntrinsicsNode.get(j).asDouble();
}
var jsonDistortNode = calib.get("cameraExtrinsics").get("data");
double[] jsonDistortion = new double[jsonDistortNode.size()];
for (int j = 0; j < jsonDistortNode.size(); j++) {
jsonDistortion[j] = jsonDistortNode.get(j).asDouble();
}
var jsonViewErrors = calib.get("perViewErrors");
double jsonAvgError = 0;
for (int j = 0; j < jsonViewErrors.size(); j++) {
jsonAvgError += jsonViewErrors.get(j).asDouble();
}
jsonAvgError /= jsonViewErrors.size();
double jsonErrorStdDev = calib.get("standardDeviation").asDouble();
// assign the read JSON values to this CameraProperties
setCalibration(
jsonWidth,
jsonHeight,
Matrix.mat(Nat.N3(), Nat.N3()).fill(jsonIntrinsics),
Matrix.mat(Nat.N5(), Nat.N1()).fill(jsonDistortion));
setCalibError(jsonAvgError, jsonErrorStdDev);
success = true;
}
} catch (Exception e) {
throw new IOException("Invalid calibration JSON");
}
if (!success) throw new IOException("Requested resolution not found in calibration");
}
public void setRandomSeed(long seed) {
rand.setSeed(seed);
}
public void setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) {
double s = Math.sqrt(resWidth * resWidth + resHeight * resHeight);
var fovWidth = new Rotation2d(fovDiag.getRadians() * (resWidth / s));
var fovHeight = new Rotation2d(fovDiag.getRadians() * (resHeight / s));
double maxFovDeg = Math.max(fovWidth.getDegrees(), fovHeight.getDegrees());
if (maxFovDeg > 179) {
double scale = 179.0 / maxFovDeg;
fovWidth = new Rotation2d(fovWidth.getRadians() * scale);
fovHeight = new Rotation2d(fovHeight.getRadians() * scale);
DriverStation.reportError(
"Requested FOV width/height too large! Scaling below 180 degrees...", false);
}
// assume no distortion
var distCoeff = VecBuilder.fill(0, 0, 0, 0, 0);
// assume centered principal point (pixels)
double cx = resWidth / 2.0;
double cy = resHeight / 2.0;
// use given fov to determine focal point (pixels)
double fx = cx / Math.tan(fovWidth.getRadians() / 2.0);
double fy = cy / Math.tan(fovHeight.getRadians() / 2.0);
// create camera intrinsics matrix
var camIntrinsics = Matrix.mat(Nat.N3(), Nat.N3()).fill(fx, 0, cx, 0, fy, cy, 0, 0, 1);
setCalibration(resWidth, resHeight, camIntrinsics, distCoeff);
}
public void setCalibration(
int resWidth, int resHeight, Matrix<N3, N3> camIntrinsics, Matrix<N5, N1> distCoeffs) {
this.resWidth = resWidth;
this.resHeight = resHeight;
this.camIntrinsics = camIntrinsics;
this.distCoeffs = distCoeffs;
}
public void setCameraIntrinsics(Matrix<N3, N3> camIntrinsics) {
this.camIntrinsics = camIntrinsics;
}
public void setDistortionCoeffs(Matrix<N5, N1> distCoeffs) {
this.distCoeffs = distCoeffs;
}
public void setCalibError(double avgErrorPx, double errorStdDevPx) {
this.avgErrorPx = avgErrorPx;
this.errorStdDevPx = errorStdDevPx;
}
/**
* @param fps The average frames per second the camera should process at. <b>Exposure time limits
* FPS if set!</b>
*/
public void setFPS(double fps) {
frameSpeedMs = Math.max(1000.0 / fps, exposureTimeMs);
}
/**
* @param exposureTimeMs The amount of time the "shutter" is open for one frame. Affects motion
* blur. <b>Frame speed(from FPS) is limited to this!</b>
*/
public void setExposureTimeMs(double exposureTimeMs) {
this.exposureTimeMs = exposureTimeMs;
frameSpeedMs = Math.max(frameSpeedMs, exposureTimeMs);
}
/**
* @param avgLatencyMs The average latency (from image capture to data published) in milliseconds
* a frame should have
*/
public void setAvgLatencyMs(double avgLatencyMs) {
this.avgLatencyMs = avgLatencyMs;
}
/**
* @param latencyStdDevMs The standard deviation in milliseconds of the latency
*/
public void setLatencyStdDevMs(double latencyStdDevMs) {
this.latencyStdDevMs = latencyStdDevMs;
}
public int getResWidth() {
return resWidth;
}
public int getResHeight() {
return resHeight;
}
public int getResArea() {
return resWidth * resHeight;
}
public Matrix<N3, N3> getIntrinsics() {
return camIntrinsics.copy();
}
public Vector<N5> getDistCoeffs() {
return new Vector<>(distCoeffs);
}
public double getFPS() {
return 1000.0 / frameSpeedMs;
}
public double getFrameSpeedMs() {
return frameSpeedMs;
}
public double getExposureTimeMs() {
return exposureTimeMs;
}
public double getAvgLatencyMs() {
return avgLatencyMs;
}
public double getLatencyStdDevMs() {
return latencyStdDevMs;
}
public SimCameraProperties copy() {
var newProp = new SimCameraProperties();
newProp.setCalibration(resWidth, resHeight, camIntrinsics, distCoeffs);
newProp.setCalibError(avgErrorPx, errorStdDevPx);
newProp.setFPS(getFPS());
newProp.setExposureTimeMs(exposureTimeMs);
newProp.setAvgLatencyMs(avgLatencyMs);
newProp.setLatencyStdDevMs(latencyStdDevMs);
return newProp;
}
public List<TargetCorner> undistort(List<TargetCorner> points) {
return OpenCVHelp.undistortPoints(camIntrinsics, distCoeffs, points);
}
/**
* The percentage(0 - 100) of this camera's resolution the contour takes up in pixels of the
* image.
*
* @param corners Corners of the contour
*/
public double getContourAreaPercent(List<TargetCorner> corners) {
return OpenCVHelp.getContourAreaPx(corners) / getResArea() * 100;
}
/** The yaw from the principal point of this camera to the pixel x value. Positive values left. */
public Rotation2d getPixelYaw(double pixelX) {
double fx = camIntrinsics.get(0, 0);
// account for principal point not being centered
double cx = camIntrinsics.get(0, 2);
double xOffset = cx - pixelX;
return new Rotation2d(fx, xOffset);
}
/**
* The pitch from the principal point of this camera to the pixel y value. Pitch is positive down.
*
* <p>Note that this angle is naively computed and may be incorrect. See {@link
* #getCorrectedPixelRot(TargetCorner)}.
*/
public Rotation2d getPixelPitch(double pixelY) {
double fy = camIntrinsics.get(1, 1);
// account for principal point not being centered
double cy = camIntrinsics.get(1, 2);
double yOffset = cy - pixelY;
return new Rotation2d(fy, -yOffset);
}
/**
* Finds the yaw and pitch to the given image point. Yaw is positive left, and pitch is positive
* down.
*
* <p>Note that pitch is naively computed and may be incorrect. See {@link
* #getCorrectedPixelRot(TargetCorner)}.
*/
public Rotation3d getPixelRot(TargetCorner point) {
return new Rotation3d(
0, getPixelPitch(point.y).getRadians(), getPixelYaw(point.x).getRadians());
}
/**
* Gives the yaw and pitch of the line intersecting the camera lens and the given pixel
* coordinates on the sensor. Yaw is positive left, and pitch positive down.
*
* <p>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:
*
* <pre>pitch = arctan(pixel y offset / focal length y)</pre>
*
* 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:
*
* <pre>focal length y ⟶ (focal length y / cos(arctan(pixel x offset / focal length x)))</pre>
*
* @return Rotation3d with yaw and pitch of the line projected out of the camera from the given
* pixel (roll is zero).
*/
public Rotation3d getCorrectedPixelRot(TargetCorner point) {
double fx = camIntrinsics.get(0, 0);
double cx = camIntrinsics.get(0, 2);
double xOffset = cx - point.x;
double fy = camIntrinsics.get(1, 1);
double cy = camIntrinsics.get(1, 2);
double yOffset = cy - point.y;
// calculate yaw normally
var yaw = new Rotation2d(fx, xOffset);
// correct pitch based on yaw
var pitch = new Rotation2d(fy / Math.cos(Math.atan(xOffset / fx)), -yOffset);
return new Rotation3d(0, pitch.getRadians(), yaw.getRadians());
}
public Rotation2d getHorizFOV() {
// sum of FOV left and right principal point
var left = getPixelYaw(0);
var right = getPixelYaw(resWidth);
return left.minus(right);
}
public Rotation2d getVertFOV() {
// sum of FOV above and below principal point
var above = getPixelPitch(0);
var below = getPixelPitch(resHeight);
return below.minus(above);
}
public Rotation2d getDiagFOV() {
return new Rotation2d(Math.hypot(getHorizFOV().getRadians(), getVertFOV().getRadians()));
}
/** Width:height */
public double getAspectRatio() {
return (double) resWidth / resHeight;
}
/**
* Returns these pixel points as fractions of a 1x1 square image. This means the camera's aspect
* ratio and resolution will be used, and the points' x and y may not reach all portions(e.g. a
* wide aspect ratio means some of the top and bottom of the square image is unreachable).
*
* @param points Pixel points on this camera's image
* @return Points mapped to an image of 1x1 resolution
*/
public List<TargetCorner> getPixelFraction(List<TargetCorner> points) {
double resLarge = getAspectRatio() > 1 ? resWidth : resHeight;
return points.stream()
.map(
p -> {
// offset to account for aspect ratio
return new TargetCorner(
(p.x + (resLarge - resWidth) / 2.0) / resLarge,
(p.y + (resLarge - resHeight) / 2.0) / resLarge);
})
.collect(Collectors.toList());
}
/** Returns these points after applying this camera's estimated noise. */
public List<TargetCorner> estPixelNoise(List<TargetCorner> points) {
if (avgErrorPx == 0 && errorStdDevPx == 0) return points;
return points.stream()
.map(
p -> {
// error pixels in random direction
double error = avgErrorPx + rand.nextGaussian() * errorStdDevPx;
double errorAngle = rand.nextDouble() * 2 * Math.PI - Math.PI;
return new TargetCorner(
p.x + error * Math.cos(errorAngle), p.y + error * Math.sin(errorAngle));
})
.collect(Collectors.toList());
}
/**
* @return Noisy estimation of a frame's processing latency in milliseconds
*/
public double estLatencyMs() {
return Math.max(avgLatencyMs + rand.nextGaussian() * latencyStdDevMs, 0);
}
/**
* @return Estimate how long until the next frame should be processed in milliseconds
*/
public double estMsUntilNextFrame() {
// exceptional processing latency blocks the next frame
return frameSpeedMs + Math.max(0, estLatencyMs() - frameSpeedMs);
}
// pre-calibrated example cameras
/** 960x720 resolution, 90 degree FOV, "perfect" lagless camera */
public static SimCameraProperties PERFECT_90DEG() {
return new SimCameraProperties();
}
public static SimCameraProperties PI4_LIFECAM_320_240() {
var prop = new SimCameraProperties();
prop.setCalibration(
320,
240,
Matrix.mat(Nat.N3(), Nat.N3())
.fill( // intrinsic
328.2733242048587,
0.0,
164.8190261141906,
0.0,
318.0609794305216,
123.8633838438093,
0.0,
0.0,
1.0),
VecBuilder.fill( // distort
0.09957946553445934,
-0.9166265114485799,
0.0019519890627236526,
-0.0036071725380870333,
1.5627234622420942));
prop.setCalibError(0.21, 0.0124);
prop.setFPS(30);
prop.setAvgLatencyMs(30);
prop.setLatencyStdDevMs(10);
return prop;
}
public static SimCameraProperties PI4_LIFECAM_640_480() {
var prop = new SimCameraProperties();
prop.setCalibration(
640,
480,
Matrix.mat(Nat.N3(), Nat.N3())
.fill( // intrinsic
669.1428078983059,
0.0,
322.53377249329213,
0.0,
646.9843137061716,
241.26567383784163,
0.0,
0.0,
1.0),
VecBuilder.fill( // distort
0.12788470750464645,
-1.2350335805796528,
0.0024990767286192732,
-0.0026958287600230705,
2.2951386729115537));
prop.setCalibError(0.26, 0.046);
prop.setFPS(15);
prop.setAvgLatencyMs(65);
prop.setLatencyStdDevMs(15);
return prop;
}
public static SimCameraProperties LL2_640_480() {
var prop = new SimCameraProperties();
prop.setCalibration(
640,
480,
Matrix.mat(Nat.N3(), Nat.N3())
.fill( // intrinsic
511.22843367007755,
0.0,
323.62049380211096,
0.0,
514.5452336723849,
261.8827920543568,
0.0,
0.0,
1.0),
VecBuilder.fill( // distort
0.1917469998873756,
-0.5142936883324216,
0.012461562046896614,
0.0014084973492408186,
0.35160648971214437));
prop.setCalibError(0.25, 0.05);
prop.setFPS(15);
prop.setAvgLatencyMs(35);
prop.setLatencyStdDevMs(8);
return prop;
}
public static SimCameraProperties LL2_960_720() {
var prop = new SimCameraProperties();
prop.setCalibration(
960,
720,
Matrix.mat(Nat.N3(), Nat.N3())
.fill( // intrinsic
769.6873145148892,
0.0,
486.1096609458122,
0.0,
773.8164483705323,
384.66071662358354,
0.0,
0.0,
1.0),
VecBuilder.fill( // distort
0.189462064814501,
-0.49903003669627627,
0.007468423590519429,
0.002496885298683693,
0.3443122090208624));
prop.setCalibError(0.35, 0.10);
prop.setFPS(10);
prop.setAvgLatencyMs(50);
prop.setLatencyStdDevMs(15);
return prop;
}
public static SimCameraProperties LL2_1280_720() {
var prop = new SimCameraProperties();
prop.setCalibration(
1280,
720,
Matrix.mat(Nat.N3(), Nat.N3())
.fill( // intrinsic
1011.3749416937393,
0.0,
645.4955139388737,
0.0,
1008.5391755084075,
508.32877656020196,
0.0,
0.0,
1.0),
VecBuilder.fill( // distort
0.13730101577061535,
-0.2904345656989261,
8.32475714507539E-4,
-3.694397782014239E-4,
0.09487962227027584));
prop.setCalibError(0.37, 0.06);
prop.setFPS(7);
prop.setAvgLatencyMs(60);
prop.setLatencyStdDevMs(20);
return prop;
}
}

179
photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java Normal file
View File

@@ -0,0 +1,179 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.simulation;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.util.Arrays;
import java.util.List;
import org.photonvision.PhotonCamera;
import org.photonvision.PhotonTargetSortMode;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
/**
* @deprecated Use {@link PhotonCameraSim} instead
*/
@Deprecated
@SuppressWarnings("unused")
public class SimPhotonCamera {
NTTopicSet ts = new NTTopicSet();
PhotonPipelineResult latestResult;
private long heartbeatCounter = 0;
/**
* Constructs a Simulated PhotonCamera from a root table.
*
* @param instance The NetworkTableInstance to pull data from. This can be a custom instance in
* simulation, but should *usually* be the default NTInstance from
* NetworkTableInstance::getDefault
* @param cameraName The name of the camera, as seen in the UI.
*/
public SimPhotonCamera(NetworkTableInstance instance, String cameraName) {
ts.removeEntries();
ts.subTable = instance.getTable(PhotonCamera.kTableName).getSubTable(cameraName);
ts.updateEntries();
}
/**
* Publishes the camera intrinsics matrix. The matrix should be in the form: spotless:off
* fx 0 cx
* 0 fy cy
* 0 0 1
* @param cameraMatrix The cam matrix
* spotless:on
*/
public void setCameraIntrinsicsMat(Matrix<N3, N3> cameraMatrix) {
ts.cameraIntrinsicsPublisher.set(cameraMatrix.getData());
}
/**
* Publishes the camera distortion matrix. The matrix should be in the form [k1 k2 p1 p2 k3]. See
* more: https://docs.opencv.org/3.4/d4/d94/tutorial_camera_calibration.html
*
* @param distortionMat The distortion mat
*/
public void setCameraDistortionMat(Matrix<N5, N1> distortionMat) {
ts.cameraDistortionPublisher.set(distortionMat.getData());
}
/**
* Constructs a Simulated PhotonCamera from the name of the camera.
*
* @param cameraName The nickname of the camera (found in the PhotonVision UI).
*/
public SimPhotonCamera(String cameraName) {
this(NetworkTableInstance.getDefault(), cameraName);
}
/**
* Simulate one processed frame of vision data, putting one result to NT.
*
* @param latencyMillis Latency of the provided frame
* @param targets Each target detected
*/
public void submitProcessedFrame(double latencyMillis, PhotonTrackedTarget... targets) {
submitProcessedFrame(latencyMillis, Arrays.asList(targets));
}
/**
* Simulate one processed frame of vision data, putting one result to NT.
*
* @param latencyMillis Latency of the provided frame
* @param sortMode Order in which to sort targets
* @param targets Each target detected
*/
public void submitProcessedFrame(
double latencyMillis, PhotonTargetSortMode sortMode, PhotonTrackedTarget... targets) {
submitProcessedFrame(latencyMillis, sortMode, Arrays.asList(targets));
}
/**
* Simulate one processed frame of vision data, putting one result to NT.
*
* @param latencyMillis Latency of the provided frame
* @param targetList List of targets detected
*/
public void submitProcessedFrame(double latencyMillis, List<PhotonTrackedTarget> targetList) {
submitProcessedFrame(latencyMillis, null, targetList);
}
/**
* Simulate one processed frame of vision data, putting one result to NT.
*
* @param latencyMillis Latency of the provided frame
* @param sortMode Order in which to sort targets
* @param targetList List of targets detected
*/
public void submitProcessedFrame(
double latencyMillis, PhotonTargetSortMode sortMode, List<PhotonTrackedTarget> targetList) {
ts.latencyMillisEntry.set(latencyMillis);
if (sortMode != null) {
targetList.sort(sortMode.getComparator());
}
PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, targetList);
var newPacket = new Packet(newResult.getPacketSize());
newResult.populatePacket(newPacket);
ts.rawBytesEntry.set(newPacket.getData());
boolean hasTargets = newResult.hasTargets();
ts.hasTargetEntry.set(hasTargets);
if (!hasTargets) {
ts.targetPitchEntry.set(0.0);
ts.targetYawEntry.set(0.0);
ts.targetAreaEntry.set(0.0);
ts.targetPoseEntry.set(new double[] {0.0, 0.0, 0.0});
ts.targetSkewEntry.set(0.0);
} else {
var bestTarget = newResult.getBestTarget();
ts.targetPitchEntry.set(bestTarget.getPitch());
ts.targetYawEntry.set(bestTarget.getYaw());
ts.targetAreaEntry.set(bestTarget.getArea());
ts.targetSkewEntry.set(bestTarget.getSkew());
var transform = bestTarget.getBestCameraToTarget();
double[] poseData = {
transform.getX(), transform.getY(), transform.getRotation().toRotation2d().getDegrees()
};
ts.targetPoseEntry.set(poseData);
}
ts.heartbeatPublisher.set(heartbeatCounter++);
latestResult = newResult;
}
PhotonPipelineResult getLatestResult() {
return latestResult;
}
}

273
photon-lib/src/main/java/org/photonvision/simulation/SimVisionSystem.java Normal file
View File

@@ -0,0 +1,273 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.simulation;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
/**
* @deprecated Use {@link VisionSystemSim} instead
*/
@Deprecated
public class SimVisionSystem {
SimPhotonCamera cam;
double camHorizFOVDegrees;
double camVertFOVDegrees;
double cameraHeightOffGroundMeters;
double maxLEDRangeMeters;
int cameraResWidth;
int cameraResHeight;
double minTargetArea;
Transform3d robotToCamera;
Field2d dbgField;
FieldObject2d dbgRobot;
FieldObject2d dbgCamera;
ArrayList<SimVisionTarget> tgtList;
/**
* Create a simulated vision system involving a camera and coprocessor mounted on a mobile robot
* running PhotonVision, detecting one or more targets scattered around the field. This assumes a
* fairly simple and distortion-less pinhole camera model.
*
* @param camName Name of the PhotonVision camera to create. Align it with the settings you use in
* the PhotonVision GUI.
* @param camDiagFOVDegrees Diagonal Field of View of the camera used. Align it with the
* manufacturer specifications, and/or whatever is configured in the PhotonVision Setting
* page.
* @param robotToCamera Transform to move from the center of the robot to the camera's mount
* position
* @param maxLEDRangeMeters Maximum distance at which your camera can illuminate the target and
* make it visible. Set to 9000 or more if your vision system does not rely on LED's.
* @param cameraResWidth Width of your camera's image sensor in pixels
* @param cameraResHeight Height of your camera's image sensor in pixels
* @param minTargetArea Minimum area that that the target should be before it's recognized as a
* target by the camera. Match this with your contour filtering settings in the PhotonVision
* GUI.
*/
public SimVisionSystem(
String camName,
double camDiagFOVDegrees,
Transform3d robotToCamera,
double maxLEDRangeMeters,
int cameraResWidth,
int cameraResHeight,
double minTargetArea) {
this.robotToCamera = robotToCamera;
this.maxLEDRangeMeters = maxLEDRangeMeters;
this.cameraResWidth = cameraResWidth;
this.cameraResHeight = cameraResHeight;
this.minTargetArea = minTargetArea;
// Calculate horizontal/vertical FOV by similar triangles
double hypotPixels = Math.hypot(cameraResWidth, cameraResHeight);
this.camHorizFOVDegrees = camDiagFOVDegrees * cameraResWidth / hypotPixels;
this.camVertFOVDegrees = camDiagFOVDegrees * cameraResHeight / hypotPixels;
cam = new SimPhotonCamera(camName);
tgtList = new ArrayList<>();
dbgField = new Field2d();
dbgRobot = dbgField.getRobotObject();
dbgCamera = dbgField.getObject(camName + " Camera");
SmartDashboard.putData(camName + " Sim Field", dbgField);
}
/**
* Add a target on the field which your vision system is designed to detect. The PhotonCamera from
* this system will report the location of the robot relative to the subset of these targets which
* are visible from the given robot position.
*
* @param target Target to add to the simulated field
*/
public void addSimVisionTarget(SimVisionTarget target) {
tgtList.add(target);
dbgField
.getObject("Target " + Integer.toString(target.targetID))
.setPose(target.targetPose.toPose2d());
}
/**
* Adds all apriltags from the provided {@link AprilTagFieldLayout} as sim vision targets. The
* poses added will preserve the tag layout's alliance origin at the time of calling this method.
*
* @param tagLayout The field tag layout to get Apriltag poses and IDs from
*/
public void addVisionTargets(AprilTagFieldLayout tagLayout) {
for (AprilTag tag : tagLayout.getTags()) {
addSimVisionTarget(
new SimVisionTarget(
tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation
Units.inchesToMeters(6),
Units.inchesToMeters(6),
tag.ID));
}
}
/**
* Clears all sim vision targets. This is useful for switching alliances and needing to repopulate
* the sim targets. NOTE: Old targets will still show on the Field2d unless overwritten by new
* targets with the same ID
*/
public void clearVisionTargets() {
tgtList.clear();
}
/**
* Adjust the camera position relative to the robot. Use this if your camera is on a gimbal or
* turret or some other mobile platform.
*
* @param newRobotToCamera New Transform from the robot to the camera
*/
public void moveCamera(Transform3d newRobotToCamera) {
this.robotToCamera = newRobotToCamera;
}
/**
* Periodic update. Call this once per frame of image data you wish to process and send to
* NetworkTables
*
* @param robotPoseMeters current pose of the robot on the field. Will be used to calculate which
* targets are actually in view, where they are at relative to the robot, and relevant
* PhotonVision parameters.
*/
public void processFrame(Pose2d robotPoseMeters) {
processFrame(new Pose3d(robotPoseMeters));
}
/**
* Periodic update. Call this once per frame of image data you wish to process and send to
* NetworkTables
*
* @param robotPoseMeters current pose of the robot in space. Will be used to calculate which
* targets are actually in view, where they are at relative to the robot, and relevant
* PhotonVision parameters.
*/
public void processFrame(Pose3d robotPoseMeters) {
Pose3d cameraPose = robotPoseMeters.transformBy(robotToCamera);
dbgRobot.setPose(robotPoseMeters.toPose2d());
dbgCamera.setPose(cameraPose.toPose2d());
ArrayList<PhotonTrackedTarget> visibleTgtList = new ArrayList<>(tgtList.size());
tgtList.forEach(
(tgt) -> {
var camToTargetTrans = new Transform3d(cameraPose, tgt.targetPose);
// Generate a transformation from camera to target,
// ignoring rotation.
var t = camToTargetTrans.getTranslation();
// Rough approximation of the alternate solution, which is (so far) always incorrect.
var altTrans =
new Translation3d(
t.getX(),
-1.0 * t.getY(),
t.getZ()); // mirrored across camera axis in Y direction
var altRot = camToTargetTrans.getRotation().times(-1.0); // flipped
var camToTargetTransAlt = new Transform3d(altTrans, altRot);
double distMeters = t.getNorm();
double area_px = tgt.tgtAreaMeters2 / getM2PerPx(distMeters);
var translationAlongGround =
new Translation2d(
tgt.targetPose.toPose2d().getX() - cameraPose.toPose2d().getX(),
tgt.targetPose.toPose2d().getY() - cameraPose.toPose2d().getY());
var camAngle = cameraPose.getRotation().toRotation2d();
var camToTgtRotation =
new Rotation2d(translationAlongGround.getX(), translationAlongGround.getY());
double yawDegrees = camToTgtRotation.minus(camAngle).getDegrees();
double camHeightAboveGround = cameraPose.getZ();
double tgtHeightAboveGround = tgt.targetPose.getZ();
double camPitchDegrees = Units.radiansToDegrees(cameraPose.getRotation().getY());
double distAlongGround = translationAlongGround.getNorm();
double pitchDegrees =
Units.radiansToDegrees(
Math.atan2((tgtHeightAboveGround - camHeightAboveGround), distAlongGround))
- camPitchDegrees;
if (camCanSeeTarget(distMeters, yawDegrees, pitchDegrees, area_px)) {
// TODO simulate target corners
visibleTgtList.add(
new PhotonTrackedTarget(
yawDegrees,
pitchDegrees,
area_px,
0.0,
tgt.targetID,
camToTargetTrans,
camToTargetTransAlt,
0.0, // TODO - simulate ambiguity when straight on?
List.of(
new TargetCorner(0, 0), new TargetCorner(0, 0),
new TargetCorner(0, 0), new TargetCorner(0, 0)),
List.of(
new TargetCorner(0, 0), new TargetCorner(0, 0),
new TargetCorner(0, 0), new TargetCorner(0, 0))));
}
});
cam.submitProcessedFrame(0.0, visibleTgtList);
}
double getM2PerPx(double dist) {
double widthMPerPx =
2 * dist * Math.tan(Units.degreesToRadians(this.camHorizFOVDegrees) / 2) / cameraResWidth;
double heightMPerPx =
2 * dist * Math.tan(Units.degreesToRadians(this.camVertFOVDegrees) / 2) / cameraResHeight;
return widthMPerPx * heightMPerPx;
}
boolean camCanSeeTarget(double distMeters, double yaw, double pitch, double area) {
boolean inRange = (distMeters < this.maxLEDRangeMeters);
boolean inHorizAngle = Math.abs(yaw) < (this.camHorizFOVDegrees / 2);
boolean inVertAngle = Math.abs(pitch) < (this.camVertFOVDegrees / 2);
boolean targetBigEnough = area > this.minTargetArea;
return (inRange && inHorizAngle && inVertAngle && targetBigEnough);
}
}

55
photon-lib/src/main/java/org/photonvision/simulation/SimVisionTarget.java Normal file
View File

@@ -0,0 +1,55 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.simulation;
import edu.wpi.first.math.geometry.Pose3d;
/**
* @deprecated Use {@link VisionTargetSim} instead
*/
@Deprecated
public class SimVisionTarget {
Pose3d targetPose;
double targetWidthMeters;
double targetHeightMeters;
double tgtAreaMeters2;
int targetID;
/**
* Describes a vision target located somewhere on the field that your SimVisionSystem can detect.
*
* @param targetPos Pose3d of the target in field-relative coordinates
* @param targetWidthMeters Width of the outer bounding box of the target in meters.
* @param targetHeightMeters Pair Height of the outer bounding box of the target in meters.
*/
public SimVisionTarget(
Pose3d targetPos, double targetWidthMeters, double targetHeightMeters, int targetID) {
this.targetPose = targetPos;
this.targetWidthMeters = targetWidthMeters;
this.targetHeightMeters = targetHeightMeters;
this.tgtAreaMeters2 = targetWidthMeters * targetHeightMeters;
this.targetID = targetID;
}
}

337
photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java Normal file
View File

@@ -0,0 +1,337 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.simulation;
import edu.wpi.first.cscore.CvSource;
import edu.wpi.first.util.RuntimeLoader;
import java.awt.image.BufferedImage;
import java.io.IOException;
import java.util.Arrays;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import javax.imageio.ImageIO;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.imgcodecs.Imgcodecs;
import org.opencv.imgproc.Imgproc;
import org.photonvision.estimation.OpenCVHelp;
public class VideoSimUtil {
public static final String kLocalTagImagesPath = "./src/main/resources/images/apriltags/";
public static final String kResourceTagImagesPath = "/images/apriltags/";
public static final String kTag16h5ImageName = "tag16_05_00000";
public static final int kNumTags16h5 = 30;
// All 16h5 tag images
private static final Map<Integer, Mat> kTag16h5Images = new HashMap<>();
// Points corresponding to marker(black square) corners of 8x8 16h5 tag images
public static final Point[] kTag16h5MarkerPts;
static {
try {
var loader =
new RuntimeLoader<>(
Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
loader.loadLibrary();
} catch (Exception e) {
throw new RuntimeException("Failed to load native libraries!", e);
}
// create Mats of 8x8 apriltag images
for (int i = 0; i < VideoSimUtil.kNumTags16h5; i++) {
Mat tagImage = VideoSimUtil.get16h5TagImage(i);
kTag16h5Images.put(i, tagImage);
}
kTag16h5MarkerPts = get16h5MarkerPts();
}
/** Updates the properties of this CvSource video stream with the given camera properties. */
public static void updateVideoProp(CvSource video, SimCameraProperties prop) {
video.setResolution(prop.getResWidth(), prop.getResHeight());
video.setFPS((int) prop.getFPS());
}
/**
* Gets the points representing the corners of this image. Because image pixels are accessed
* through a Mat, the point (0,0) actually represents the center of the top-left pixel and not the
* actual top-left corner.
*
* @param size Size of image
*/
public static Point[] getImageCorners(Size size) {
return new Point[] {
new Point(-0.5, -0.5),
new Point(size.width - 0.5, -0.5),
new Point(size.width - 0.5, size.height - 0.5),
new Point(-0.5, size.height - 0.5)
};
}
/**
* Gets the 8x8 (grayscale) image of a specific 16h5 AprilTag.
*
* @param id The fiducial id of the desired tag
*/
public static Mat get16h5TagImage(int id) {
String name = kTag16h5ImageName;
String idString = String.valueOf(id);
name = name.substring(0, name.length() - idString.length()) + idString;
var resource = VideoSimUtil.class.getResource(kResourceTagImagesPath + name + ".png");
// local IDE tests
String path = kLocalTagImagesPath + name + ".png";
// gradle tests
if (resource != null) {
path = resource.getPath();
// TODO why did we have this previously?
// if (path.startsWith("/")) path = path.substring(1);
}
Mat result = new Mat();
if (!path.startsWith("file")) result = Imgcodecs.imread(path, Imgcodecs.IMREAD_GRAYSCALE);
// reading jar file
if (result.empty()) {
BufferedImage buf;
try {
buf = ImageIO.read(resource);
} catch (IOException e) {
System.err.println("Couldn't read tag image!");
return result;
}
result = new Mat(buf.getHeight(), buf.getWidth(), CvType.CV_8UC1);
byte[] px = new byte[1];
for (int y = 0; y < result.height(); y++) {
for (int x = 0; x < result.width(); x++) {
px[0] = (byte) (buf.getRGB(x, y) & 0xFF);
result.put(y, x, px);
}
}
}
return result;
}
/** Gets the points representing the marker(black square) corners. */
public static Point[] get16h5MarkerPts() {
return get16h5MarkerPts(1);
}
/**
* Gets the points representing the marker(black square) corners.
*
* @param scale The scale of the tag image (8*scale x 8*scale image)
*/
public static Point[] get16h5MarkerPts(int scale) {
var roi16h5 = new Rect(new Point(1, 1), new Size(6, 6));
roi16h5.x *= scale;
roi16h5.y *= scale;
roi16h5.width *= scale;
roi16h5.height *= scale;
var pts = getImageCorners(roi16h5.size());
for (int i = 0; i < pts.length; i++) {
var pt = pts[i];
pts[i] = new Point(roi16h5.tl().x + pt.x, roi16h5.tl().y + pt.y);
}
return pts;
}
/**
* Warps the image of a specific 16h5 AprilTag onto the destination image at the given points.
*
* @param tagId The id of the specific tag to warp onto the destination image
* @param dstPoints Points(4) in destination image where the tag marker(black square) corners
* should be warped onto.
* @param destination The destination image to place the warped tag image onto.
* @param antialiasing If antialiasing should be performed by automatically
* supersampling/interpolating the warped image. This should be used if better stream quality
* is desired or target detection is being done on the stream, but can hurt performance.
* @see OpenCVHelp#targetCornersToMat(org.photonvision.targeting.TargetCorner...)
*/
public static void warp16h5TagImage(
int tagId, MatOfPoint2f dstPoints, Mat destination, boolean antialiasing) {
Mat tagImage = kTag16h5Images.get(tagId);
if (tagImage == null || tagImage.empty()) return;
var tagPoints = new MatOfPoint2f(kTag16h5MarkerPts);
// points of tag image corners
var tagImageCorners = new MatOfPoint2f(getImageCorners(tagImage.size()));
// the rectangle describing the rectangle-of-interest(ROI)
var boundingRect = Imgproc.boundingRect(dstPoints);
// find the perspective transform from the tag image to the warped destination points
Mat perspecTrf = Imgproc.getPerspectiveTransform(tagPoints, dstPoints);
// check extreme image corners after transform to check if we need to expand bounding rect
var extremeCorners = new MatOfPoint2f();
Core.perspectiveTransform(tagImageCorners, extremeCorners, perspecTrf);
// dilate ROI to fit full tag
boundingRect = Imgproc.boundingRect(extremeCorners);
// adjust interpolation strategy based on size of warped tag compared to tag image
var warpedContourArea = Imgproc.contourArea(extremeCorners);
double warpedTagUpscale = Math.sqrt(warpedContourArea) / Math.sqrt(tagImage.size().area());
int warpStrategy = Imgproc.INTER_NEAREST;
// automatically determine the best supersampling of warped image and scale of tag image
/*
(warpPerspective does not properly resample, so this is used to avoid aliasing in the
warped image. Supersampling is used when the warped tag is small, but is very slow
when the warped tag is large-- scaling the tag image up and using linear interpolation
instead can be performant while still effectively antialiasing. Some combination of these
two can be used in between those extremes.)
TODO: Simplify magic numbers to one or two variables, or use a more proper approach?
*/
int supersampling = 6;
supersampling = (int) Math.ceil(supersampling / warpedTagUpscale);
supersampling = Math.max(Math.min(supersampling, 8), 1);
Mat scaledTagImage = new Mat();
if (warpedTagUpscale > 2.0) {
warpStrategy = Imgproc.INTER_LINEAR;
int scaleFactor = (int) (warpedTagUpscale / 3.0) + 2;
scaleFactor = Math.max(Math.min(scaleFactor, 40), 1);
scaleFactor *= supersampling;
Imgproc.resize(
tagImage, scaledTagImage, new Size(), scaleFactor, scaleFactor, Imgproc.INTER_NEAREST);
tagPoints.fromArray(get16h5MarkerPts(scaleFactor));
} else tagImage.assignTo(scaledTagImage);
// constrain the bounding rect inside of the destination image
boundingRect.x -= 1;
boundingRect.y -= 1;
boundingRect.width += 2;
boundingRect.height += 2;
if (boundingRect.x < 0) {
boundingRect.width += boundingRect.x;
boundingRect.x = 0;
}
if (boundingRect.y < 0) {
boundingRect.height += boundingRect.y;
boundingRect.y = 0;
}
boundingRect.width = Math.min(destination.width() - boundingRect.x, boundingRect.width);
boundingRect.height = Math.min(destination.height() - boundingRect.y, boundingRect.height);
if (boundingRect.width <= 0 || boundingRect.height <= 0) return;
// upscale if supersampling
Mat scaledDstPts = new Mat();
if (supersampling > 1) {
Core.multiply(dstPoints, new Scalar(supersampling, supersampling), scaledDstPts);
boundingRect.x *= supersampling;
boundingRect.y *= supersampling;
boundingRect.width *= supersampling;
boundingRect.height *= supersampling;
} else dstPoints.assignTo(scaledDstPts);
// update transform relative to expanded, scaled bounding rect
Core.subtract(scaledDstPts, new Scalar(boundingRect.tl().x, boundingRect.tl().y), scaledDstPts);
perspecTrf = Imgproc.getPerspectiveTransform(tagPoints, scaledDstPts);
// warp (scaled) tag image onto (scaled) ROI image representing the portion of
// the destination image encapsulated by boundingRect
Mat tempROI = new Mat();
Imgproc.warpPerspective(scaledTagImage, tempROI, perspecTrf, boundingRect.size(), warpStrategy);
// downscale ROI with interpolation if supersampling
if (supersampling > 1) {
boundingRect.x /= supersampling;
boundingRect.y /= supersampling;
boundingRect.width /= supersampling;
boundingRect.height /= supersampling;
Imgproc.resize(tempROI, tempROI, boundingRect.size(), 0, 0, Imgproc.INTER_AREA);
}
// we want to copy ONLY the transformed tag to the result image, not the entire bounding rect
// using a mask only copies the source pixels which have an associated non-zero value in the
// mask
Mat tempMask = Mat.zeros(tempROI.size(), CvType.CV_8UC1);
Core.subtract(
extremeCorners, new Scalar(boundingRect.tl().x, boundingRect.tl().y), extremeCorners);
Point tempCenter = new Point();
tempCenter.x =
Arrays.stream(extremeCorners.toArray()).mapToDouble(p -> p.x).average().getAsDouble();
tempCenter.y =
Arrays.stream(extremeCorners.toArray()).mapToDouble(p -> p.y).average().getAsDouble();
// dilate tag corners
Arrays.stream(extremeCorners.toArray())
.forEach(
p -> {
double xdiff = p.x - tempCenter.x;
double ydiff = p.y - tempCenter.y;
xdiff += 1 * Math.signum(xdiff);
ydiff += 1 * Math.signum(ydiff);
new Point(tempCenter.x + xdiff, tempCenter.y + ydiff);
});
// (make inside of tag completely white in mask)
Imgproc.fillConvexPoly(tempMask, new MatOfPoint(extremeCorners.toArray()), new Scalar(255));
// copy transformed tag onto result image
tempROI.copyTo(destination.submat(boundingRect), tempMask);
}
/**
* Draws a contour around the given points and text of the id onto the destination image.
*
* @param id Fiducial ID number to draw
* @param dstPoints Points representing the four corners of the tag marker(black square) in the
* destination image.
* @param destination The destination image to draw onto. The image should be in the BGR color
* space.
*/
public static void drawTagDetection(int id, MatOfPoint2f dstPoints, Mat destination) {
var dstPointsd = new MatOfPoint(dstPoints.toArray());
double scaleX = destination.width() / 640.0;
double scaleY = destination.height() / 480.0;
double minScale = Math.min(scaleX, scaleY);
int thickness = (int) (1 * minScale);
// for(var pt : dstPoints.toArray()) {
// Imgproc.circle(destination, pt, 4, new Scalar(255), 1, Imgproc.LINE_AA);
// }
// Imgproc.rectangle(destination, extremeRect, new Scalar(255), 1, Imgproc.LINE_AA);
// Imgproc.rectangle(destination, Imgproc.boundingRect(dstPoints), new Scalar(255), 1,
// Imgproc.LINE_AA);
Imgproc.polylines(
destination, List.of(dstPointsd), true, new Scalar(0, 0, 255), thickness, Imgproc.LINE_AA);
var textPt = Imgproc.boundingRect(dstPoints).tl();
textPt.x -= 10.0 * scaleX;
textPt.y -= 12.0 * scaleY;
Imgproc.putText(
destination,
String.valueOf(id),
textPt,
Imgproc.FONT_HERSHEY_PLAIN,
1.5 * minScale,
new Scalar(0, 0, 255),
thickness,
Imgproc.LINE_AA);
}
}

377
photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java Normal file
View File

@@ -0,0 +1,377 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.simulation;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.Optional;
import java.util.Set;
import java.util.stream.Collectors;
import org.photonvision.PhotonCamera;
import org.photonvision.estimation.TargetModel;
/**
* A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot
* running PhotonVision, detecting targets placed on the field. {@link VisionTargetSim}s added to
* this class will be detected by the {@link PhotonCameraSim}s added to this class. This class
* should be updated periodically with the robot's current pose in order to publish the simulated
* camera target info.
*/
public class VisionSystemSim {
private final Map<String, PhotonCameraSim> camSimMap = new HashMap<>();
private static final double kBufferLengthSeconds = 1.5;
// save robot-to-camera for each camera over time (Pose3d is easily interpolatable)
private final Map<PhotonCameraSim, TimeInterpolatableBuffer<Pose3d>> camTrfMap = new HashMap<>();
// interpolate drivetrain with twists
private final TimeInterpolatableBuffer<Pose3d> robotPoseBuffer =
TimeInterpolatableBuffer.createBuffer(kBufferLengthSeconds);
private final Map<String, Set<VisionTargetSim>> targetSets = new HashMap<>();
private final Field2d dbgField;
/**
* A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot
* running PhotonVision, detecting targets placed on the field. {@link VisionTargetSim}s added to
* this class will be detected by the {@link PhotonCameraSim}s added to this class. This class
* should be updated periodically with the robot's current pose in order to publish the simulated
* camera target info.
*
* @param visionSystemName The specific identifier for this vision system in NetworkTables.
*/
public VisionSystemSim(String visionSystemName) {
dbgField = new Field2d();
String tableName = "VisionSystemSim-" + visionSystemName;
SmartDashboard.putData(tableName + "/Sim Field", dbgField);
}
/** Get one of the simulated cameras. */
public Optional<PhotonCameraSim> getCameraSim(String name) {
return Optional.ofNullable(camSimMap.get(name));
}
/** Get all the simulated cameras. */
public Collection<PhotonCameraSim> getCameraSims() {
return camSimMap.values();
}
/**
* Adds a simulated camera to this vision system with a specified robot-to-camera transformation.
* The vision targets registered with this vision system simulation will be observed by the
* simulated {@link PhotonCamera}.
*
* @param cameraSim The camera simulation
* @param robotToCamera The transform from the robot pose to the camera pose
*/
public void addCamera(PhotonCameraSim cameraSim, Transform3d robotToCamera) {
var existing = camSimMap.putIfAbsent(cameraSim.getCamera().getName(), cameraSim);
if (existing == null) {
camTrfMap.put(cameraSim, TimeInterpolatableBuffer.createBuffer(kBufferLengthSeconds));
camTrfMap
.get(cameraSim)
.addSample(Timer.getFPGATimestamp(), new Pose3d().plus(robotToCamera));
}
}
/** Remove all simulated cameras from this vision system. */
public void clearCameras() {
camSimMap.clear();
camTrfMap.clear();
}
/**
* Remove a simulated camera from this vision system.
*
* @return If the camera was present and removed
*/
public boolean removeCamera(PhotonCameraSim cameraSim) {
boolean success = camSimMap.remove(cameraSim.getCamera().getName()) != null;
camTrfMap.remove(cameraSim);
return success;
}
/**
* Get a simulated camera's position relative to the robot. If the requested camera is invalid, an
* empty optional is returned.
*
* @return The transform of this cameraSim, or an empty optional if it is invalid
*/
public Optional<Transform3d> getRobotToCamera(PhotonCameraSim cameraSim) {
return getRobotToCamera(cameraSim, Timer.getFPGATimestamp());
}
/**
* Get a simulated camera's position relative to the robot. If the requested camera is invalid, an
* empty optional is returned.
*
* @param cameraSim Specific camera to get the robot-to-camera transform of
* @param timeSeconds Timestamp in seconds of when the transform should be observed
* @return The transform of this cameraSim, or an empty optional if it is invalid
*/
public Optional<Transform3d> getRobotToCamera(PhotonCameraSim cameraSim, double timeSeconds) {
var trfBuffer = camTrfMap.get(cameraSim);
if (trfBuffer == null) return Optional.empty();
var sample = trfBuffer.getSample(timeSeconds);
if (sample.isEmpty()) return Optional.empty();
return Optional.of(new Transform3d(new Pose3d(), sample.orElse(new Pose3d())));
}
/**
* Adjust a camera's position relative to the robot. Use this if your camera is on a gimbal or
* turret or some other mobile platform.
*
* @param cameraSim The simulated camera to change the relative position of
* @param robotToCamera New transform from the robot to the camera
* @return If the cameraSim was valid and transform was adjusted
*/
public boolean adjustCamera(PhotonCameraSim cameraSim, Transform3d robotToCamera) {
var trfBuffer = camTrfMap.get(cameraSim);
if (trfBuffer == null) return false;
trfBuffer.addSample(Timer.getFPGATimestamp(), new Pose3d().plus(robotToCamera));
return true;
}
/** Reset the previous transforms for all cameras to their current transform. */
public void resetCameraTransforms() {
for (var cam : camTrfMap.keySet()) resetCameraTransforms(cam);
}
/**
* Reset the transform history for this camera to just the current transform.
*
* @return If the cameraSim was valid and transforms were reset
*/
public boolean resetCameraTransforms(PhotonCameraSim cameraSim) {
double now = Timer.getFPGATimestamp();
var trfBuffer = camTrfMap.get(cameraSim);
if (trfBuffer == null) return false;
var lastTrf = new Transform3d(new Pose3d(), trfBuffer.getSample(now).orElse(new Pose3d()));
trfBuffer.clear();
adjustCamera(cameraSim, lastTrf);
return true;
}
public Set<VisionTargetSim> getVisionTargets() {
var all = new HashSet<VisionTargetSim>();
for (var entry : targetSets.entrySet()) {
all.addAll(entry.getValue());
}
return all;
}
public Set<VisionTargetSim> getVisionTargets(String type) {
return targetSets.get(type);
}
/**
* Adds targets on the field which your vision system is designed to detect. The {@link
* PhotonCamera}s simulated from this system will report the location of the camera relative to
* the subset of these targets which are visible from the given camera position.
*
* <p>By default these are added under the type "targets".
*
* @param targets Targets to add to the simulated field
*/
public void addVisionTargets(VisionTargetSim... targets) {
addVisionTargets("targets", targets);
}
/**
* Adds targets on the field which your vision system is designed to detect. The {@link
* PhotonCamera}s simulated from this system will report the location of the camera relative to
* the subset of these targets which are visible from the given camera position.
*
* <p>The AprilTags from this layout will be added as vision targets under the type "apriltags".
* The poses added preserve the tag layout's current alliance origin.
*
* @param tagLayout The field tag layout to get Apriltag poses and IDs from
*/
public void addVisionTargets(AprilTagFieldLayout tagLayout) {
for (AprilTag tag : tagLayout.getTags()) {
addVisionTargets(
"apriltags",
new VisionTargetSim(
tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation
TargetModel.kTag16h5,
tag.ID));
}
}
/**
* Adds targets on the field which your vision system is designed to detect. The {@link
* PhotonCamera}s simulated from this system will report the location of the camera relative to
* the subset of these targets which are visible from the given camera position.
*
* @param type Type of target (e.g. "cargo").
* @param targets Targets to add to the simulated field
*/
public void addVisionTargets(String type, VisionTargetSim... targets) {
targetSets.computeIfAbsent(type, k -> new HashSet<>());
for (var tgt : targets) {
targetSets.get(type).add(tgt);
}
}
public void clearVisionTargets() {
targetSets.clear();
}
public Set<VisionTargetSim> removeVisionTargets(String type) {
return targetSets.remove(type);
}
public Set<VisionTargetSim> removeVisionTargets(VisionTargetSim... targets) {
var removeList = List.of(targets);
var removedSet = new HashSet<VisionTargetSim>();
for (var entry : targetSets.entrySet()) {
entry
.getValue()
.removeIf(
t -> {
if (removeList.contains(t)) {
removedSet.add(t);
return true;
} else return false;
});
}
return removedSet;
}
/** Get the latest robot pose in meters saved by the vision system. */
public Pose3d getRobotPose() {
return getRobotPose(Timer.getFPGATimestamp());
}
/**
* Get the robot pose in meters saved by the vision system at this timestamp.
*
* @param timestamp Timestamp of the desired robot pose
*/
public Pose3d getRobotPose(double timestamp) {
return robotPoseBuffer.getSample(timestamp).orElse(new Pose3d());
}
/** Clears all previous robot poses and sets robotPose at current time. */
public void resetRobotPose(Pose2d robotPose) {
resetRobotPose(new Pose3d(robotPose));
}
/** Clears all previous robot poses and sets robotPose at current time. */
public void resetRobotPose(Pose3d robotPose) {
robotPoseBuffer.clear();
robotPoseBuffer.addSample(Timer.getFPGATimestamp(), robotPose);
}
public Field2d getDebugField() {
return dbgField;
}
/**
* Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically
* determine if a new frame should be submitted.
*
* @param robotPoseMeters The current robot pose in meters
*/
public void update(Pose2d robotPoseMeters) {
update(new Pose3d(robotPoseMeters));
}
/**
* Periodic update. Ensure this is called repeatedly-- camera performance is used to automatically
* determine if a new frame should be submitted.
*
* @param robotPoseMeters The current robot pose in meters
*/
public void update(Pose3d robotPoseMeters) {
var targetTypes = targetSets.entrySet();
// update vision targets on field
targetTypes.forEach(
entry ->
dbgField
.getObject(entry.getKey())
.setPoses(
entry.getValue().stream()
.map(t -> t.getPose().toPose2d())
.collect(Collectors.toList())));
if (robotPoseMeters == null) return;
// save "real" robot poses over time
double now = Timer.getFPGATimestamp();
robotPoseBuffer.addSample(now, robotPoseMeters);
dbgField.setRobotPose(robotPoseMeters.toPose2d());
var allTargets = new ArrayList<VisionTargetSim>();
targetTypes.forEach((entry) -> allTargets.addAll(entry.getValue()));
var visibleTargets = new ArrayList<Pose3d>();
var cameraPose2ds = new ArrayList<Pose2d>();
// process each camera
for (var camSim : camSimMap.values()) {
// check if this camera is ready to process and get latency
var optTimestamp = camSim.consumeNextEntryTime();
if (optTimestamp.isEmpty()) continue;
// when this result "was" read by NT
long timestampNT = optTimestamp.get();
// this result's processing latency in milliseconds
double latencyMillis = camSim.prop.estLatencyMs();
// the image capture timestamp in seconds of this result
double timestampCapture = timestampNT / 1e6 - latencyMillis / 1e3;
// use camera pose from the image capture timestamp
Pose3d lateRobotPose = getRobotPose(timestampCapture);
Pose3d lateCameraPose = lateRobotPose.plus(getRobotToCamera(camSim).get());
cameraPose2ds.add(lateCameraPose.toPose2d());
// process a PhotonPipelineResult with visible targets
var camResult = camSim.process(latencyMillis, lateCameraPose, allTargets);
// publish this info to NT at estimated timestamp of receive
camSim.submitProcessedFrame(camResult, timestampNT);
// display debug results
for (var target : camResult.getTargets()) {
visibleTargets.add(lateCameraPose.transformBy(target.getBestCameraToTarget()));
}
}
if (visibleTargets.size() != 0) {
dbgField
.getObject("visibleTargetPoses")
.setPoses(visibleTargets.stream().map(Pose3d::toPose2d).collect(Collectors.toList()));
}
if (cameraPose2ds.size() != 0) dbgField.getObject("cameras").setPoses(cameraPose2ds);
}
}

94
photon-lib/src/main/java/org/photonvision/simulation/VisionTargetSim.java Normal file
View File

@@ -0,0 +1,94 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision.simulation;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.util.List;
import org.photonvision.estimation.TargetModel;
/** Describes a vision target located somewhere on the field that your vision system can detect. */
public class VisionTargetSim {
private Pose3d pose;
private TargetModel model;
public final int fiducialID;
/**
* Describes a vision target located somewhere on the field that your vision system can detect.
*
* @param pose Pose3d of the tag in field-relative coordinates
* @param model TargetModel which describes the shape of the target
*/
public VisionTargetSim(Pose3d pose, TargetModel model) {
this.pose = pose;
this.model = model;
this.fiducialID = -1;
}
/**
* Describes a fiducial tag located somewhere on the field that your vision system can detect.
*
* @param pose Pose3d of the tag in field-relative coordinates
* @param model TargetModel which describes the shape of the target(tag)
* @param id The ID of this fiducial tag
*/
public VisionTargetSim(Pose3d pose, TargetModel model, int id) {
this.pose = pose;
this.model = model;
this.fiducialID = id;
}
public void setPose(Pose3d pose) {
this.pose = pose;
}
public void setModel(TargetModel model) {
this.model = model;
}
public Pose3d getPose() {
return pose;
}
public TargetModel getModel() {
return model;
}
/** This target's vertices offset from its field pose. */
public List<Translation3d> getFieldVertices() {
return model.getFieldVertices(pose);
}
@Override
public boolean equals(Object obj) {
if (this == obj) return true;
if (obj instanceof VisionTargetSim) {
var o = (VisionTargetSim) obj;
return pose.equals(o.pose) && model.equals(o.model);
}
return false;
}
}