mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
12446a6c44
## Description compute confidence level based on target area in total image size and populate classId and confidence level in Java (while building the PhotonTrackedTarget) ## Changes - Add new VisionTargetSim constructor for object detection - If class ID specified but confidence = -1, estimate based on total area --------- Co-authored-by: Matt Morley <matthew.morley.ca@gmail.com>
717 lines
30 KiB
Java
717 lines
30 KiB
Java
/*
|
|
* 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.AprilTagFieldLayout;
|
|
import edu.wpi.first.apriltag.AprilTagFields;
|
|
import edu.wpi.first.cameraserver.CameraServer;
|
|
import edu.wpi.first.cscore.CvSource;
|
|
import edu.wpi.first.cscore.OpenCvLoader;
|
|
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.math.geometry.Transform3d;
|
|
import edu.wpi.first.util.PixelFormat;
|
|
import edu.wpi.first.util.WPIUtilJNI;
|
|
import edu.wpi.first.wpilibj.RobotController;
|
|
import java.util.ArrayList;
|
|
import java.util.List;
|
|
import java.util.Optional;
|
|
import java.util.stream.Collectors;
|
|
import org.opencv.core.CvType;
|
|
import org.opencv.core.Mat;
|
|
import org.opencv.core.Point;
|
|
import org.opencv.core.RotatedRect;
|
|
import org.opencv.core.Scalar;
|
|
import org.opencv.core.Size;
|
|
import org.opencv.imgproc.Imgproc;
|
|
import org.photonvision.PhotonCamera;
|
|
import org.photonvision.PhotonTargetSortMode;
|
|
import org.photonvision.common.networktables.NTTopicSet;
|
|
import org.photonvision.estimation.CameraTargetRelation;
|
|
import org.photonvision.estimation.OpenCVHelp;
|
|
import org.photonvision.estimation.RotTrlTransform3d;
|
|
import org.photonvision.estimation.TargetModel;
|
|
import org.photonvision.estimation.VisionEstimation;
|
|
import org.photonvision.targeting.MultiTargetPNPResult;
|
|
import org.photonvision.targeting.PhotonPipelineResult;
|
|
import org.photonvision.targeting.PhotonTrackedTarget;
|
|
import org.photonvision.targeting.PnpResult;
|
|
|
|
/**
|
|
* 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;
|
|
|
|
@SuppressWarnings("doclint")
|
|
protected NTTopicSet ts = new NTTopicSet();
|
|
|
|
private long heartbeatCounter = 1;
|
|
|
|
/** 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 = 100;
|
|
private double minTargetAreaPercent;
|
|
private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest;
|
|
|
|
private final AprilTagFieldLayout tagLayout;
|
|
|
|
// video stream simulation
|
|
private final CvSource videoSimRaw;
|
|
private final Mat videoSimFrameRaw = new Mat();
|
|
private boolean videoSimRawEnabled = true;
|
|
private boolean videoSimWireframeEnabled = false;
|
|
private double videoSimWireframeResolution = 0.1;
|
|
private final CvSource videoSimProcessed;
|
|
private final Mat videoSimFrameProcessed = new Mat();
|
|
private boolean videoSimProcEnabled = true;
|
|
|
|
static {
|
|
OpenCvLoader.forceStaticLoad();
|
|
}
|
|
|
|
@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(camera, prop, AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField));
|
|
}
|
|
|
|
/**
|
|
* 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
|
|
* @param tagLayout The {@link AprilTagFieldLayout} used to solve for tag positions.
|
|
*/
|
|
public PhotonCameraSim(
|
|
PhotonCamera camera, SimCameraProperties prop, AprilTagFieldLayout tagLayout) {
|
|
this.cam = camera;
|
|
this.prop = prop;
|
|
this.tagLayout = tagLayout;
|
|
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, AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField));
|
|
this.minTargetAreaPercent = minTargetAreaPercent;
|
|
this.maxSightRangeMeters = maxSightRangeMeters;
|
|
}
|
|
|
|
/**
|
|
* 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.
|
|
* @param tagLayout AprilTag field layout to use for multi-tag estimation
|
|
*/
|
|
public PhotonCameraSim(
|
|
PhotonCamera camera,
|
|
SimCameraProperties prop,
|
|
double minTargetAreaPercent,
|
|
double maxSightRangeMeters,
|
|
AprilTagFieldLayout tagLayout) {
|
|
this(camera, prop);
|
|
this.minTargetAreaPercent = minTargetAreaPercent;
|
|
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;
|
|
}
|
|
|
|
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 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) {
|
|
if (MathUtil.clamp(point.x, 0, prop.getResWidth()) != point.x
|
|
|| MathUtil.clamp(point.y, 0, prop.getResHeight()) != point.y) {
|
|
return false; // point is outside of resolution
|
|
}
|
|
}
|
|
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();
|
|
}
|
|
|
|
/**
|
|
* 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;
|
|
}
|
|
|
|
/**
|
|
* 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;
|
|
}
|
|
|
|
/**
|
|
* 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.
|
|
*
|
|
* @param sortMode The target sorting order
|
|
*/
|
|
public void setTargetSortMode(PhotonTargetSortMode sortMode) {
|
|
if (sortMode != null) this.sortMode = sortMode;
|
|
}
|
|
|
|
/**
|
|
* Sets whether the raw video stream simulation is enabled.
|
|
*
|
|
* <p>Note: This may increase loop times.
|
|
*
|
|
* @param enabled Whether or not to enable the raw video stream
|
|
*/
|
|
public void enableRawStream(boolean enabled) {
|
|
videoSimRawEnabled = enabled;
|
|
}
|
|
|
|
/**
|
|
* Sets whether a wireframe of the field is drawn to the raw video stream.
|
|
*
|
|
* <p>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;
|
|
}
|
|
|
|
/**
|
|
* 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
|
|
*/
|
|
public 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
|
|
*/
|
|
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 before noise
|
|
var visibleTgts = new ArrayList<Pair<VisionTargetSim, Point[]>>();
|
|
// all targets actually detected by camera (after noise)
|
|
var detectableTgts = new ArrayList<PhotonTrackedTarget>();
|
|
// basis change from world coordinates to camera coordinates
|
|
var camRt = RotTrlTransform3d.makeRelativeTo(cameraPose);
|
|
|
|
// 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) {
|
|
if (detectableTgts.size() >= 50) {
|
|
break;
|
|
}
|
|
|
|
// pose isn't visible, skip to next
|
|
if (!canSeeTargetPose(cameraPose, tgt)) continue;
|
|
|
|
// find target's 3d corner points
|
|
var fieldCorners = tgt.getFieldVertices();
|
|
if (tgt.getModel().isSpherical) { // target is spherical
|
|
var model = tgt.getModel();
|
|
// orient the model to the camera (like a sprite/decal) so it appears similar regardless of
|
|
// view
|
|
fieldCorners =
|
|
model.getFieldVertices(
|
|
TargetModel.getOrientedPose(
|
|
tgt.getPose().getTranslation(), cameraPose.getTranslation()));
|
|
}
|
|
// project 3d target points into 2d image points
|
|
var imagePoints =
|
|
OpenCVHelp.projectPoints(prop.getIntrinsics(), prop.getDistCoeffs(), camRt, fieldCorners);
|
|
// spherical targets need a rotated rectangle of their midpoints for visualization
|
|
if (tgt.getModel().isSpherical) {
|
|
var center = OpenCVHelp.avgPoint(imagePoints);
|
|
int l = 0, t, b, r = 0;
|
|
// reference point (left side midpoint)
|
|
for (int i = 1; i < 4; i++) {
|
|
if (imagePoints[i].x < imagePoints[l].x) l = i;
|
|
}
|
|
var lc = imagePoints[l];
|
|
// determine top, right, bottom midpoints
|
|
double[] angles = new double[4];
|
|
t = (l + 1) % 4;
|
|
b = (l + 1) % 4;
|
|
for (int i = 0; i < 4; i++) {
|
|
if (i == l) continue;
|
|
var ic = imagePoints[i];
|
|
angles[i] = Math.atan2(lc.y - ic.y, ic.x - lc.x);
|
|
if (angles[i] >= angles[t]) t = i;
|
|
if (angles[i] <= angles[b]) b = i;
|
|
}
|
|
for (int i = 0; i < 4; i++) {
|
|
if (i != t && i != l && i != b) r = i;
|
|
}
|
|
// create RotatedRect from midpoints
|
|
var rect =
|
|
new RotatedRect(
|
|
new Point(center.x, center.y),
|
|
new Size(imagePoints[r].x - lc.x, imagePoints[b].y - imagePoints[t].y),
|
|
Math.toDegrees(-angles[r]));
|
|
// set target corners to rect corners
|
|
Point[] points = new Point[4];
|
|
rect.points(points);
|
|
imagePoints = points;
|
|
}
|
|
// save visible targets for raw video stream simulation
|
|
visibleTgts.add(new Pair<>(tgt, imagePoints));
|
|
// estimate pixel noise
|
|
var noisyTargetCorners = prop.estPixelNoise(imagePoints);
|
|
// find the minimum area rectangle of target corners
|
|
var minAreaRect = OpenCVHelp.getMinAreaRect(noisyTargetCorners);
|
|
Point[] minAreaRectPts = new Point[4];
|
|
minAreaRect.points(minAreaRectPts);
|
|
// find the (naive) 2d yaw/pitch
|
|
var centerPt = minAreaRect.center;
|
|
var centerRot = prop.getPixelRot(centerPt);
|
|
// 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 PnpResult();
|
|
if (tgt.fiducialID >= 0 && tgt.getFieldVertices().size() == 4) { // single AprilTag solvePNP
|
|
pnpSim =
|
|
OpenCVHelp.solvePNP_SQUARE(
|
|
prop.getIntrinsics(),
|
|
prop.getDistCoeffs(),
|
|
tgt.getModel().vertices,
|
|
noisyTargetCorners)
|
|
.get();
|
|
}
|
|
|
|
// If object detection (user classId valid) but conf wasn't provided, estimate
|
|
int classId = tgt.objDetClassId;
|
|
float conf = tgt.objDetConf;
|
|
if (classId >= 0 && conf < 0) {
|
|
// Simulate confidence using sqrt-scaled area for a more realistic
|
|
// curve. Raw areaPercent/100 is tiny for most targets; sqrt scaling
|
|
// gives reasonable values even for small-but-visible objects.
|
|
conf = (float) MathUtil.clamp(Math.sqrt(areaPercent / 100.0) * 2.0, 0.0, 1.0);
|
|
}
|
|
|
|
detectableTgts.add(
|
|
new PhotonTrackedTarget(
|
|
-Math.toDegrees(centerRot.getZ()),
|
|
-Math.toDegrees(centerRot.getY()),
|
|
areaPercent,
|
|
Math.toDegrees(centerRot.getX()),
|
|
tgt.fiducialID,
|
|
classId,
|
|
conf,
|
|
pnpSim.best,
|
|
pnpSim.alt,
|
|
pnpSim.ambiguity,
|
|
OpenCVHelp.pointsToCorners(minAreaRectPts),
|
|
OpenCVHelp.pointsToCorners(noisyTargetCorners)));
|
|
}
|
|
// render visible tags to raw video frame
|
|
if (videoSimRawEnabled) {
|
|
// draw field wireframe
|
|
if (videoSimWireframeEnabled) {
|
|
VideoSimUtil.drawFieldWireframe(
|
|
camRt,
|
|
prop,
|
|
videoSimWireframeResolution,
|
|
1.5,
|
|
new Scalar(80),
|
|
6,
|
|
1,
|
|
new Scalar(30),
|
|
videoSimFrameRaw);
|
|
}
|
|
|
|
// draw targets
|
|
for (var pair : visibleTgts) {
|
|
var tgt = pair.getFirst();
|
|
var corn = pair.getSecond();
|
|
|
|
if (tgt.fiducialID >= 0) { // apriltags
|
|
VideoSimUtil.warp36h11TagImage(tgt.fiducialID, corn, true, videoSimFrameRaw);
|
|
} else if (!tgt.getModel().isSpherical) { // non-spherical targets
|
|
var contour = corn;
|
|
if (!tgt.getModel()
|
|
.isPlanar) { // visualization cant handle non-convex projections of 3d models
|
|
contour = OpenCVHelp.getConvexHull(contour);
|
|
}
|
|
VideoSimUtil.drawPoly(contour, -1, new Scalar(255), true, videoSimFrameRaw);
|
|
} else { // spherical targets
|
|
VideoSimUtil.drawInscribedEllipse(corn, new Scalar(255), videoSimFrameRaw);
|
|
}
|
|
}
|
|
videoSimRaw.putFrame(videoSimFrameRaw);
|
|
} else videoSimRaw.setConnectionStrategy(ConnectionStrategy.kForceClose);
|
|
// draw/annotate target detection outline on processed view
|
|
if (videoSimProcEnabled) {
|
|
Imgproc.cvtColor(videoSimFrameRaw, videoSimFrameProcessed, Imgproc.COLOR_GRAY2BGR);
|
|
Imgproc.drawMarker( // crosshair
|
|
videoSimFrameProcessed,
|
|
new Point(prop.getResWidth() / 2.0, prop.getResHeight() / 2.0),
|
|
new Scalar(0, 255, 0),
|
|
Imgproc.MARKER_CROSS,
|
|
(int) VideoSimUtil.getScaledThickness(15, videoSimFrameProcessed),
|
|
(int) VideoSimUtil.getScaledThickness(1, videoSimFrameProcessed),
|
|
Imgproc.LINE_AA);
|
|
for (var tgt : detectableTgts) {
|
|
if (tgt.getFiducialId() >= 0) { // apriltags
|
|
VideoSimUtil.drawTagDetection(
|
|
tgt.getFiducialId(),
|
|
OpenCVHelp.cornersToPoints(tgt.getDetectedCorners()),
|
|
videoSimFrameProcessed);
|
|
} else { // other targets
|
|
// bounding rectangle
|
|
Imgproc.rectangle(
|
|
videoSimFrameProcessed,
|
|
OpenCVHelp.getBoundingRect(OpenCVHelp.cornersToPoints(tgt.getDetectedCorners())),
|
|
new Scalar(0, 0, 255),
|
|
(int) VideoSimUtil.getScaledThickness(1, videoSimFrameProcessed),
|
|
Imgproc.LINE_AA);
|
|
|
|
VideoSimUtil.drawPoly(
|
|
OpenCVHelp.cornersToPoints(tgt.getMinAreaRectCorners()),
|
|
(int) VideoSimUtil.getScaledThickness(1, videoSimFrameProcessed),
|
|
new Scalar(255, 30, 30),
|
|
true,
|
|
videoSimFrameProcessed);
|
|
}
|
|
}
|
|
videoSimProcessed.putFrame(videoSimFrameProcessed);
|
|
} else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose);
|
|
|
|
// calculate multitag results
|
|
Optional<MultiTargetPNPResult> multitagResult = Optional.empty();
|
|
// TODO: Implement ATFL subscribing in backend
|
|
// var tagLayout = cam.getAprilTagFieldLayout();
|
|
var visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(detectableTgts, tagLayout);
|
|
if (visibleLayoutTags.size() > 1) {
|
|
List<Short> usedIDs =
|
|
visibleLayoutTags.stream().map(t -> (short) t.ID).sorted().collect(Collectors.toList());
|
|
var pnpResult =
|
|
VisionEstimation.estimateCamPosePNP(
|
|
prop.getIntrinsics(),
|
|
prop.getDistCoeffs(),
|
|
detectableTgts,
|
|
tagLayout,
|
|
TargetModel.kAprilTag36h11);
|
|
|
|
if (pnpResult.isPresent()) {
|
|
multitagResult = Optional.of(new MultiTargetPNPResult(pnpResult.get(), usedIDs));
|
|
}
|
|
}
|
|
|
|
// sort target order
|
|
if (sortMode != null) {
|
|
detectableTgts.sort(sortMode.getComparator());
|
|
}
|
|
|
|
// put this simulated data to NT
|
|
var now = RobotController.getFPGATime();
|
|
var ret =
|
|
new PhotonPipelineResult(
|
|
heartbeatCounter,
|
|
now - (long) (latencyMillis * 1000),
|
|
now,
|
|
// Pretend like we heard a pong recently
|
|
1000L + (long) ((Math.random() - 0.5) * 50),
|
|
detectableTgts,
|
|
multitagResult);
|
|
return ret;
|
|
}
|
|
|
|
/**
|
|
* 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.metadata.getLatencyMillis(), receiveTimestamp);
|
|
|
|
// Results are now dynamically sized, so let's guess 1024 bytes is big enough
|
|
ts.resultPublisher.set(result, 1024);
|
|
|
|
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 Transform3d(), 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();
|
|
ts.targetPoseEntry.set(transform, receiveTimestamp);
|
|
}
|
|
|
|
ts.cameraIntrinsicsPublisher.set(prop.getIntrinsics().getData(), receiveTimestamp);
|
|
ts.cameraDistortionPublisher.set(prop.getDistCoeffs().getData(), receiveTimestamp);
|
|
|
|
ts.heartbeatPublisher.set(heartbeatCounter, receiveTimestamp);
|
|
heartbeatCounter += 1;
|
|
}
|
|
}
|