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

[photonlib] Simulation Visualization Update (#895)

Browse Source
This commit is contained in:
amquake
2023-09-19 16:10:04 -07:00
committed by GitHub
parent 9e371de1cb
commit 7f283640c4
11 changed files with 1140 additions and 335 deletions
Download Patch File Download Diff File Expand all files Collapse all files

257
photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View File

@@ -28,7 +28,6 @@ import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.geometry.CoordinateSystem;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
@@ -56,6 +55,9 @@ import org.opencv.imgproc.Imgproc;
import org.photonvision.targeting.TargetCorner;
public final class OpenCVHelp {
private static RotTrlTransform3d NWU_TO_EDN;
private static RotTrlTransform3d EDN_TO_NWU;
static {
try {
var loader =
@@ -65,15 +67,21 @@ public final class OpenCVHelp {
} catch (Exception e) {
throw new RuntimeException("Failed to load native libraries!", e);
}
NWU_TO_EDN =
new RotTrlTransform3d(
new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, -1, 0, 0, 0, -1, 1, 0, 0)),
new Translation3d());
EDN_TO_NWU =
new RotTrlTransform3d(
new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0)),
new Translation3d());
}
public static MatOfDouble matrixToMat(SimpleMatrix matrix) {
public static Mat matrixToMat(SimpleMatrix matrix) {
var mat = new Mat(matrix.numRows(), matrix.numCols(), CvType.CV_64F);
mat.put(0, 0, matrix.getDDRM().getData());
var wrappedMat = new MatOfDouble();
mat.convertTo(wrappedMat, CvType.CV_64F);
mat.release();
return wrappedMat;
return mat;
}
public static Matrix<Num, Num> matToMatrix(Mat mat) {
@@ -93,8 +101,7 @@ public final class OpenCVHelp {
public static MatOfPoint3f translationToTvec(Translation3d... translations) {
Point3[] points = new Point3[translations.length];
for (int i = 0; i < translations.length; i++) {
var trl =
CoordinateSystem.convert(translations[i], CoordinateSystem.NWU(), CoordinateSystem.EDN());
var trl = translationNWUtoEDN(translations[i]);
points[i] = new Point3(trl.getX(), trl.getY(), trl.getZ());
}
return new MatOfPoint3f(points);
@@ -112,10 +119,7 @@ public final class OpenCVHelp {
tvecInput.convertTo(wrapped, CvType.CV_32F);
wrapped.get(0, 0, data);
wrapped.release();
return CoordinateSystem.convert(
new Translation3d(data[0], data[1], data[2]),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
return translationEDNtoNWU(new Translation3d(data[0], data[1], data[2]));
}
/**
@@ -150,42 +154,46 @@ public final class OpenCVHelp {
return rotationEDNtoNWU(new Rotation3d(axis.div(axis.norm()), axis.norm()));
}
public static TargetCorner averageCorner(List<TargetCorner> corners) {
if (corners == null || corners.size() == 0) return null;
var pointMat = targetCornersToMat(corners);
public static Point avgPoint(Point[] points) {
if (points == null || points.length == 0) return null;
var pointMat = new MatOfPoint2f(points);
Core.reduce(pointMat, pointMat, 0, Core.REDUCE_AVG);
var avgPt = matToTargetCorners(pointMat)[0];
var avgPt = pointMat.toArray()[0];
pointMat.release();
return avgPt;
}
public static MatOfPoint2f targetCornersToMat(List<TargetCorner> corners) {
return targetCornersToMat(corners.toArray(TargetCorner[]::new));
public static Point[] cornersToPoints(List<TargetCorner> corners) {
var points = new Point[corners.size()];
for (int i = 0; i < corners.size(); i++) {
var corn = corners.get(i);
points[i] = new Point(corn.x, corn.y);
}
return points;
}
public static MatOfPoint2f targetCornersToMat(TargetCorner... corners) {
public static Point[] cornersToPoints(TargetCorner... corners) {
var points = new Point[corners.length];
for (int i = 0; i < corners.length; i++) {
points[i] = new Point(corners[i].x, corners[i].y);
}
return new MatOfPoint2f(points);
return points;
}
public static TargetCorner[] pointsToTargetCorners(Point... points) {
var corners = new TargetCorner[points.length];
public static List<TargetCorner> pointsToCorners(Point... points) {
var corners = new ArrayList<TargetCorner>(points.length);
for (int i = 0; i < points.length; i++) {
corners[i] = new TargetCorner(points[i].x, points[i].y);
corners.add(new TargetCorner(points[i].x, points[i].y));
}
return corners;
}
public static TargetCorner[] matToTargetCorners(MatOfPoint2f matInput) {
var corners = new TargetCorner[(int) matInput.total()];
public static List<TargetCorner> pointsToCorners(MatOfPoint2f matInput) {
var corners = new ArrayList<TargetCorner>();
float[] data = new float[(int) matInput.total() * matInput.channels()];
matInput.get(0, 0, data);
for (int i = 0; i < corners.length; i++) {
corners[i] = new TargetCorner(data[0 + 2 * i], data[1 + 2 * i]);
for (int i = 0; i < (int) matInput.total(); i++) {
corners.add(new TargetCorner(data[0 + 2 * i], data[1 + 2 * i]));
}
return corners;
}
@@ -221,24 +229,39 @@ public final class OpenCVHelp {
return reordered;
}
// TODO: RotTrlTransform3d removal awaiting Rotation3d performance improvements
/**
* Convert a rotation from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0} in EDN,
* this would be XYZ {0, -1, 0} in NWU.
* Convert a rotation delta from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0}
* in EDN, this would be {0, -1, 0} in NWU.
*/
private static Rotation3d rotationEDNtoNWU(Rotation3d rot) {
return CoordinateSystem.convert(
new Rotation3d(), CoordinateSystem.NWU(), CoordinateSystem.EDN())
.plus(CoordinateSystem.convert(rot, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
return new RotTrlTransform3d(EDN_TO_NWU.apply(rot), new Translation3d())
.apply(EDN_TO_NWU.inverse().getRotation());
}
/**
* Convert a rotation from EDN to NWU. For example, if you have a rotation X,Y,Z {1, 0, 0} in EDN,
* this would be XYZ {0, -1, 0} in NWU.
* Convert a rotation delta from NWU to EDN. For example, if you have a rotation X,Y,Z {1, 0, 0}
* in NWU, this would be {0, 0, 1} in EDN.
*/
private static Rotation3d rotationNWUtoEDN(Rotation3d rot) {
return CoordinateSystem.convert(
new Rotation3d(), CoordinateSystem.EDN(), CoordinateSystem.NWU())
.plus(CoordinateSystem.convert(rot, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
return new RotTrlTransform3d(NWU_TO_EDN.apply(rot), new Translation3d())
.apply(NWU_TO_EDN.inverse().getRotation());
}
/**
* Convert a translation from EDN to NWU. For example, if you have a translation X,Y,Z {1, 0, 0}
* in EDN, this would be {0, -1, 0} in NWU.
*/
private static Translation3d translationEDNtoNWU(Translation3d trl) {
return EDN_TO_NWU.apply(trl);
}
/**
* Convert a translation from NWU to EDN. For example, if you have a translation X,Y,Z {1, 0, 0}
* in NWU, this would be {0, 0, 1} in EDN.
*/
private static Translation3d translationNWUtoEDN(Translation3d trl) {
return NWU_TO_EDN.apply(trl);
}
/**
@@ -247,29 +270,27 @@ public final class OpenCVHelp {
*
* @param cameraMatrix the camera intrinsics matrix in standard opencv form
* @param distCoeffs the camera distortion matrix in standard opencv form
* @param camPose The current camera pose in the 3d world
* @param camRt The change in basis from world coordinates to camera coordinates. See {@link
* RotTrlTransform3d#makeRelativeTo(Pose3d)}.
* @param objectTranslations The 3d points to be projected
* @return The 2d points in pixels which correspond to the image of the 3d points on the camera
* @return The 2d points in pixels which correspond to the camera's image of the 3d points
*/
public static List<TargetCorner> projectPoints(
public static Point[] projectPoints(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
Pose3d camPose,
RotTrlTransform3d camRt,
List<Translation3d> objectTranslations) {
// translate to opencv classes
var objectPoints = translationToTvec(objectTranslations.toArray(new Translation3d[0]));
// opencv rvec/tvec describe a change in basis from world to camera
var basisChange = RotTrlTransform3d.makeRelativeTo(camPose);
var rvec = rotationToRvec(basisChange.getRotation());
var tvec = translationToTvec(basisChange.getTranslation());
var rvec = rotationToRvec(camRt.getRotation());
var tvec = translationToTvec(camRt.getTranslation());
var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
var distCoeffsMat = new MatOfDouble(matrixToMat(distCoeffs.getStorage()));
var imagePoints = new MatOfPoint2f();
// project to 2d
Calib3d.projectPoints(objectPoints, rvec, tvec, cameraMatrixMat, distCoeffsMat, imagePoints);
// turn 2d point Mat into TargetCorners
var corners = matToTargetCorners(imagePoints);
var points = imagePoints.toArray();
// release our Mats from native memory
objectPoints.release();
@@ -279,37 +300,37 @@ public final class OpenCVHelp {
distCoeffsMat.release();
imagePoints.release();
return Arrays.asList(corners);
return points;
}
/**
* Undistort 2d image points using a given camera's intrinsics and distortion.
*
* <p>2d image points from projectPoints(CameraProperties, Pose3d, List) projectPoints will
* naturally be distorted, so this operation is important if the image points need to be directly
* used (e.g. 2d yaw/pitch).
* <p>2d image points from {@link #projectPoints(Matrix, Matrix, RotTrlTransform3d, List)
* projectPoints()} will naturally be distorted, so this operation is important if the image
* points need to be directly used (e.g. 2d yaw/pitch).
*
* @param cameraMatrix the camera intrinsics matrix in standard opencv form
* @param distCoeffs the camera distortion matrix in standard opencv form
* @param corners The distorted image points
* @param cameraMatrix The camera intrinsics matrix in standard opencv form
* @param distCoeffs The camera distortion matrix in standard opencv form
* @param points The distorted image points
* @return The undistorted image points
*/
public static List<TargetCorner> undistortPoints(
Matrix<N3, N3> cameraMatrix, Matrix<N5, N1> distCoeffs, List<TargetCorner> corners) {
var points_in = targetCornersToMat(corners);
var points_out = new MatOfPoint2f();
public static Point[] undistortPoints(
Matrix<N3, N3> cameraMatrix, Matrix<N5, N1> distCoeffs, Point[] points) {
var distMat = new MatOfPoint2f(points);
var undistMat = new MatOfPoint2f();
var cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
var distCoeffsMat = matrixToMat(distCoeffs.getStorage());
Calib3d.undistortImagePoints(points_in, points_out, cameraMatrixMat, distCoeffsMat);
var corners_out = matToTargetCorners(points_out);
Calib3d.undistortImagePoints(distMat, undistMat, cameraMatrixMat, distCoeffsMat);
var undistPoints = undistMat.toArray();
points_in.release();
points_out.release();
distMat.release();
undistMat.release();
cameraMatrixMat.release();
distCoeffsMat.release();
return Arrays.asList(corners_out);
return undistPoints;
}
/**
@@ -318,58 +339,52 @@ public final class OpenCVHelp {
* <p>Note that rectangle size and position are stored with ints and do not have sub-pixel
* accuracy.
*
* @param corners The corners/points to be bounded
* @return Rectangle bounding the given corners
* @param points The points to be bounded
* @return Rectangle bounding the given points
*/
public static Rect getBoundingRect(List<TargetCorner> corners) {
var corn = targetCornersToMat(corners);
var rect = Imgproc.boundingRect(corn);
corn.release();
public static Rect getBoundingRect(Point[] points) {
var pointMat = new MatOfPoint2f(points);
var rect = Imgproc.boundingRect(pointMat);
pointMat.release();
return rect;
}
/**
* Gets the rotated rectangle with minimum area which bounds this contour.
*
* <p>Note that rectangle size and position are stored with doubles and have sub-pixel accuracy.
* <p>Note that rectangle size and position are stored with floats and have sub-pixel accuracy.
*
* @param corners The corners/points to be bounded
* @return Rotated rectangle bounding the given corners
* @param points The points to be bounded
* @return Rotated rectangle bounding the given points
*/
public static RotatedRect getMinAreaRect(List<TargetCorner> corners) {
var corn = targetCornersToMat(corners);
var rect = Imgproc.minAreaRect(corn);
corn.release();
public static RotatedRect getMinAreaRect(Point[] points) {
var pointMat = new MatOfPoint2f(points);
var rect = Imgproc.minAreaRect(pointMat);
pointMat.release();
return rect;
}
/**
* Get the area in pixels of this target's contour. It's important to note that this may be
* different from the area of the bounding rectangle around the contour.
* Gets the convex hull contour (the outline) of a list of points.
*
* @param corners The corners defining this contour
* @return Area in pixels (units of corner x/y)
* @param points The input contour
* @return The subset of points defining the convex hull. Note that these use ints and not floats.
*/
public static double getContourAreaPx(List<TargetCorner> corners) {
var temp = targetCornersToMat(corners);
var corn = new MatOfPoint(temp.toArray());
temp.release();
public static Point[] getConvexHull(Point[] points) {
var pointMat = new MatOfPoint(points);
// outputHull gives us indices (of corn) that make a convex hull contour
var outputHull = new MatOfInt();
Imgproc.convexHull(corn, outputHull);
Imgproc.convexHull(pointMat, outputHull);
int[] indices = outputHull.toArray();
outputHull.release();
var tempPoints = corn.toArray();
var points = tempPoints.clone();
pointMat.release();
var convexPoints = new Point[indices.length];
for (int i = 0; i < indices.length; i++) {
points[i] = tempPoints[indices[i]];
convexPoints[i] = points[indices[i]];
}
corn.fromArray(points);
// calculate area of the (convex hull) contour
double area = Imgproc.contourArea(corn);
corn.release();
return area;
return convexPoints;
}
/**
@@ -396,8 +411,8 @@ public final class OpenCVHelp {
* <li>Point 3: [0, -squareLength / 2, -squareLength / 2]
* </ul>
*
* @param imageCorners The projection of these 3d object points into the 2d camera image. The
* order should match the given object point translations.
* @param imagePoints The projection of these 3d object points into the 2d camera image. The order
* should match the given object point translations.
* @return The resulting transformation that maps the camera pose to the target pose and the
* ambiguity if an alternate solution is available.
*/
@@ -405,10 +420,10 @@ public final class OpenCVHelp {
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<Translation3d> modelTrls,
List<TargetCorner> imageCorners) {
Point[] imagePoints) {
// solvepnp inputs
MatOfPoint3f objectPoints = new MatOfPoint3f();
MatOfPoint2f imagePoints = new MatOfPoint2f();
MatOfPoint3f objectMat = new MatOfPoint3f();
MatOfPoint2f imageMat = new MatOfPoint2f();
MatOfDouble cameraMatrixMat = new MatOfDouble();
MatOfDouble distCoeffsMat = new MatOfDouble();
var rvecs = new ArrayList<Mat>();
@@ -419,10 +434,10 @@ public final class OpenCVHelp {
try {
// IPPE_SQUARE expects our corners in a specific order
modelTrls = reorderCircular(modelTrls, true, -1);
imageCorners = reorderCircular(imageCorners, true, -1);
imagePoints = reorderCircular(Arrays.asList(imagePoints), true, -1).toArray(Point[]::new);
// translate to opencv classes
translationToTvec(modelTrls.toArray(new Translation3d[0])).assignTo(objectPoints);
targetCornersToMat(imageCorners).assignTo(imagePoints);
translationToTvec(modelTrls.toArray(new Translation3d[0])).assignTo(objectMat);
imageMat.fromArray(imagePoints);
matrixToMat(cameraMatrix.getStorage()).assignTo(cameraMatrixMat);
matrixToMat(distCoeffs.getStorage()).assignTo(distCoeffsMat);
@@ -433,8 +448,8 @@ public final class OpenCVHelp {
for (int tries = 0; tries < 2; tries++) {
// calc rvecs/tvecs and associated reprojection error from image points
Calib3d.solvePnPGeneric(
objectPoints,
imagePoints,
objectMat,
imageMat,
cameraMatrixMat,
distCoeffsMat,
rvecs,
@@ -456,10 +471,10 @@ public final class OpenCVHelp {
// check if we got a NaN result
if (!Double.isNaN(errors[0])) break;
else { // add noise and retry
double[] br = imagePoints.get(0, 0);
double[] br = imageMat.get(0, 0);
br[0] -= 0.001;
br[1] -= 0.001;
imagePoints.put(0, 0, br);
imageMat.put(0, 0, br);
}
}
@@ -477,8 +492,8 @@ public final class OpenCVHelp {
return new PNPResults();
} finally {
// release our Mats from native memory
objectPoints.release();
imagePoints.release();
objectMat.release();
imageMat.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();
@@ -503,8 +518,8 @@ public final class OpenCVHelp {
* @param cameraMatrix The camera intrinsics matrix in standard opencv form
* @param distCoeffs The camera distortion matrix in standard opencv form
* @param objectTrls The translations of the object corners, relative to the field.
* @param imageCorners The projection of these 3d object points into the 2d camera image. The
* order should match the given object point translations.
* @param imagePoints The projection of these 3d object points into the 2d camera image. The order
* should match the given object point translations.
* @return The resulting transformation that maps the camera pose to the target pose. If the 3d
* model points are supplied relative to the origin, this transformation brings the camera to
* the origin.
@@ -513,13 +528,13 @@ public final class OpenCVHelp {
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<Translation3d> objectTrls,
List<TargetCorner> imageCorners) {
Point[] imagePoints) {
try {
// translate to opencv classes
MatOfPoint3f objectPoints = translationToTvec(objectTrls.toArray(new Translation3d[0]));
MatOfPoint2f imagePoints = targetCornersToMat(imageCorners);
MatOfDouble cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
MatOfDouble distCoeffsMat = matrixToMat(distCoeffs.getStorage());
MatOfPoint3f objectMat = translationToTvec(objectTrls.toArray(new Translation3d[0]));
MatOfPoint2f imageMat = new MatOfPoint2f(imagePoints);
Mat cameraMatrixMat = matrixToMat(cameraMatrix.getStorage());
Mat distCoeffsMat = matrixToMat(distCoeffs.getStorage());
var rvecs = new ArrayList<Mat>();
var tvecs = new ArrayList<Mat>();
Mat rvec = Mat.zeros(3, 1, CvType.CV_32F);
@@ -527,8 +542,8 @@ public final class OpenCVHelp {
Mat reprojectionError = new Mat();
// calc rvec/tvec from image points
Calib3d.solvePnPGeneric(
objectPoints,
imagePoints,
objectMat,
imageMat,
cameraMatrixMat,
distCoeffsMat,
rvecs,
@@ -545,8 +560,8 @@ public final class OpenCVHelp {
var best = new Transform3d(tvecToTranslation(tvecs.get(0)), rvecToRotation(rvecs.get(0)));
// release our Mats from native memory
objectPoints.release();
imagePoints.release();
objectMat.release();
imageMat.release();
cameraMatrixMat.release();
distCoeffsMat.release();
for (var v : rvecs) v.release();

158
photon-lib/src/main/java/org/photonvision/estimation/RotTrlTransform3d.java
View File

@@ -25,6 +25,7 @@
package org.photonvision.estimation;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
@@ -37,16 +38,49 @@ import java.util.stream.Collectors;
public class RotTrlTransform3d {
private final Translation3d trl;
private final Rotation3d rot;
// TODO: removal awaiting wpilib Rotation3d performance improvements
private double m_w;
private double m_x;
private double m_y;
private double m_z;
public RotTrlTransform3d() {
this(new Rotation3d(), new Translation3d());
/**
* A rotation-translation transformation.
*
* <p>Applying this RotTrlTransform3d to poses will preserve their current origin-to-pose
* transform as if the origin was transformed by these components instead.
*
* @param rot The rotation component
* @param trl The translation component
*/
public RotTrlTransform3d(Rotation3d rot, Translation3d trl) {
this.rot = rot;
var quat = rot.getQuaternion();
m_w = quat.getW();
m_x = quat.getX();
m_y = quat.getY();
m_z = quat.getZ();
this.trl = trl;
}
public RotTrlTransform3d(Pose3d initial, Pose3d last) {
// this.rot = last.getRotation().minus(initial.getRotation());
// this.trl = last.getTranslation().minus(initial.getTranslation().rotateBy(rot));
var quat = initial.getRotation().getQuaternion();
m_w = quat.getW();
m_x = quat.getX();
m_y = quat.getY();
m_z = quat.getZ();
this.rot = invrotate(last.getRotation());
this.trl = last.getTranslation().minus(rotate(initial.getTranslation()));
}
/**
* Creates a rotation-translation transformation from a Transform3d.
*
* <p>Applying this transformation to poses will preserve their current origin-to-pose transform
* as if the origin was transformed by these components.
* <p>Applying this RotTrlTransform3d to poses will preserve their current origin-to-pose
* transform as if the origin was transformed by trf instead.
*
* @param trf The origin transformation
*/
@@ -54,18 +88,40 @@ public class RotTrlTransform3d {
this(trf.getRotation(), trf.getTranslation());
}
/**
* A rotation-translation transformation.
*
* <p>Applying this transformation to poses will preserve their current origin-to-pose transform
* as if the origin was transformed by these components.
*
* @param rot The rotation component
* @param trl The translation component
*/
public RotTrlTransform3d(Rotation3d rot, Translation3d trl) {
this.rot = rot;
this.trl = trl;
public RotTrlTransform3d() {
this(new Rotation3d(), new Translation3d());
}
private Translation3d rotate(Translation3d otrl) {
final var p = new Quaternion(0.0, otrl.getX(), otrl.getY(), otrl.getZ());
final var qprime = times(times(p), new Quaternion(m_w, -m_x, -m_y, -m_z));
return new Translation3d(qprime.getX(), qprime.getY(), qprime.getZ());
}
private Translation3d invrotate(Translation3d otrl) {
m_x = -m_x;
m_y = -m_y;
m_z = -m_z;
var result = rotate(otrl);
m_x = -m_x;
m_y = -m_y;
m_z = -m_z;
return result;
}
private Rotation3d rotate(Rotation3d orot) {
return new Rotation3d(times(orot.getQuaternion()));
}
private Rotation3d invrotate(Rotation3d orot) {
m_x = -m_x;
m_y = -m_y;
m_z = -m_z;
var result = rotate(orot);
m_x = -m_x;
m_y = -m_y;
m_z = -m_z;
return result;
}
/**
@@ -80,9 +136,12 @@ public class RotTrlTransform3d {
/** The inverse of this transformation. Applying the inverse will "undo" this transformation. */
public RotTrlTransform3d inverse() {
var inverseRot = rot.unaryMinus();
var inverseTrl = trl.rotateBy(inverseRot).unaryMinus();
return new RotTrlTransform3d(inverseRot, inverseTrl);
// var inverseRot = rot.unaryMinus();
// var inverseTrl = trl.rotateBy(inverseRot).unaryMinus();
// return new RotTrlTransform3d(inverseRot, inverseTrl);
var inverseTrl = invrotate(trl).unaryMinus();
return new RotTrlTransform3d(new Rotation3d(new Quaternion(m_w, -m_x, -m_y, -m_z)), inverseTrl);
}
/** This transformation as a Transform3d (as if of the origin) */
@@ -101,19 +160,74 @@ public class RotTrlTransform3d {
}
public Translation3d apply(Translation3d trl) {
return apply(new Pose3d(trl, new Rotation3d())).getTranslation();
// return trl.rotateBy(rot).plus(this.trl);
return rotate(trl).plus(this.trl);
}
;
public List<Translation3d> applyTrls(List<Translation3d> trls) {
return trls.stream().map(this::apply).collect(Collectors.toList());
}
public Rotation3d apply(Rotation3d rot) {
return rotate(rot);
}
public List<Rotation3d> applyRots(List<Rotation3d> rots) {
return rots.stream().map(this::apply).collect(Collectors.toList());
}
public Pose3d apply(Pose3d pose) {
return new Pose3d(pose.getTranslation().rotateBy(rot).plus(trl), pose.getRotation().plus(rot));
// return new Pose3d(pose.getTranslation().rotateBy(rot).plus(trl),
// pose.getRotation().plus(rot));
return new Pose3d(apply(pose.getTranslation()), apply(pose.getRotation()));
}
public List<Pose3d> applyPoses(List<Pose3d> poses) {
return poses.stream().map(this::apply).collect(Collectors.toList());
}
// TODO: removal awaiting wpilib Rotation3d performance improvements
private Quaternion times(Quaternion other) {
final double o_w = other.getW();
final double o_x = other.getX();
final double o_y = other.getY();
final double o_z = other.getZ();
return times(m_w, m_x, m_y, m_z, o_w, o_x, o_y, o_z);
}
private static Quaternion times(Quaternion a, Quaternion b) {
final double m_w = a.getW();
final double m_x = a.getX();
final double m_y = a.getY();
final double m_z = a.getZ();
final double o_w = b.getW();
final double o_x = b.getX();
final double o_y = b.getY();
final double o_z = b.getZ();
return times(m_w, m_x, m_y, m_z, o_w, o_x, o_y, o_z);
}
private static Quaternion times(
double m_w,
double m_x,
double m_y,
double m_z,
double o_w,
double o_x,
double o_y,
double o_z) {
// https://en.wikipedia.org/wiki/Quaternion#Scalar_and_vector_parts
// v₁ x v₂
final double cross_x = m_y * o_z - o_y * m_z;
final double cross_y = o_x * m_z - m_x * o_z;
final double cross_z = m_x * o_y - o_x * m_y;
// v = w₁v₂ + w₂v₁ + v₁ x v₂
final double new_x = o_x * m_w + (m_x * o_w) + cross_x;
final double new_y = o_y * m_w + (m_y * o_w) + cross_y;
final double new_z = o_z * m_w + (m_z * o_w) + cross_z;
return new Quaternion(m_w * o_w - (m_x * o_x + m_y * o_y + m_z * o_z), new_x, new_y, new_z);
}
}

100
photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java
View File

@@ -25,8 +25,8 @@
package org.photonvision.estimation;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import java.util.ArrayList;
@@ -36,8 +36,8 @@ import java.util.stream.Collectors;
/** Describes the 3d model of a target. */
public class TargetModel {
/**
* Translations of this target's vertices relative to its pose. If this target is spherical, this
* list has one translation with x == radius.
* Translations of this target's vertices relative to its pose. Rectangular and spherical targets
* will have four vertices. See their respective constructors for more info.
*/
public final List<Translation3d> vertices;
@@ -47,7 +47,17 @@ public class TargetModel {
public static final TargetModel kTag16h5 =
new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6));
/** Creates a rectangular, planar target model given the width and height. */
/**
* Creates a rectangular, planar target model given the width and height. The model has four
* vertices:
*
* <ul>
* <li>Point 0: [0, -width/2, -height/2]
* <li>Point 1: [0, width/2, -height/2]
* <li>Point 2: [0, width/2, height/2]
* <li>Point 3: [0, -width/2, height/2]
* </ul>
*/
public TargetModel(double widthMeters, double heightMeters) {
this.vertices =
List.of(
@@ -61,18 +71,64 @@ public class TargetModel {
}
/**
* Creates a spherical target model which has similar dimensions when viewed from any angle. This
* model will only have one vertex which has x == radius.
* Creates a cuboid target model given the length, width, height. The model has eight vertices:
*
* <ul>
* <li>Point 0: [length/2, -width/2, -height/2]
* <li>Point 1: [length/2, width/2, -height/2]
* <li>Point 2: [length/2, width/2, height/2]
* <li>Point 3: [length/2, -width/2, height/2]
* <li>Point 4: [-length/2, -width/2, height/2]
* <li>Point 5: [-length/2, width/2, height/2]
* <li>Point 6: [-length/2, width/2, -height/2]
* <li>Point 7: [-length/2, -width/2, -height/2]
* </ul>
*/
public TargetModel(double lengthMeters, double widthMeters, double heightMeters) {
this(
List.of(
new Translation3d(lengthMeters / 2.0, -widthMeters / 2.0, -heightMeters / 2.0),
new Translation3d(lengthMeters / 2.0, widthMeters / 2.0, -heightMeters / 2.0),
new Translation3d(lengthMeters / 2.0, widthMeters / 2.0, heightMeters / 2.0),
new Translation3d(lengthMeters / 2.0, -widthMeters / 2.0, heightMeters / 2.0),
new Translation3d(-lengthMeters / 2.0, -widthMeters / 2.0, heightMeters / 2.0),
new Translation3d(-lengthMeters / 2.0, widthMeters / 2.0, heightMeters / 2.0),
new Translation3d(-lengthMeters / 2.0, widthMeters / 2.0, -heightMeters / 2.0),
new Translation3d(-lengthMeters / 2.0, -widthMeters / 2.0, -heightMeters / 2.0)));
}
/**
* Creates a spherical target model which has similar dimensions regardless of its rotation. This
* model has four vertices:
*
* <ul>
* <li>Point 0: [0, -radius, 0]
* <li>Point 1: [0, 0, -radius]
* <li>Point 2: [0, radius, 0]
* <li>Point 3: [0, 0, radius]
* </ul>
*
* <i>Q: Why these vertices?</i> A: This target should be oriented to the camera every frame, much
* like a sprite/decal, and these vertices represent the ellipse vertices (maxima). These vertices
* are used for drawing the image of this sphere, but do not match the corners that will be
* published by photonvision.
*/
public TargetModel(double diameterMeters) {
this.vertices = List.of(new Translation3d(diameterMeters / 2.0, 0, 0));
double radius = diameterMeters / 2.0;
this.vertices =
List.of(
new Translation3d(0, -radius, 0),
new Translation3d(0, 0, -radius),
new Translation3d(0, radius, 0),
new Translation3d(0, 0, radius));
this.isPlanar = false;
this.isSpherical = true;
}
/**
* Creates a target model from arbitrary 3d vertices. Automatically determines if the given
* vertices are planar(x == 0). More than 2 vertices must be given.
* vertices are planar(x == 0). More than 2 vertices must be given. If this is a planar model, the
* vertices should define a non-intersecting contour.
*
* @param vertices Translations representing the vertices of this target model relative to its
* pose.
@@ -95,13 +151,31 @@ public class TargetModel {
/**
* This target's vertices offset from its field pose.
*
* <p>Note: If this target is spherical, only one vertex radius meters in front of the pose is
* returned.
* <p>Note: If this target is spherical, use {@link #getOrientedPose(Translation3d,
* Translation3d)} with this method.
*/
public List<Translation3d> getFieldVertices(Pose3d targetPose) {
return vertices.stream()
.map(t -> targetPose.plus(new Transform3d(t, new Rotation3d())).getTranslation())
.collect(Collectors.toList());
var basisChange = new RotTrlTransform3d(targetPose.getRotation(), targetPose.getTranslation());
return vertices.stream().map(t -> basisChange.apply(t)).collect(Collectors.toList());
}
/**
* Returns a Pose3d with the given target translation oriented (with its relative x-axis aligned)
* to the camera translation. This is used for spherical targets which should not have their
* projection change regardless of their own rotation.
*
* @param tgtTrl This target's translation
* @param cameraTrl Camera's translation
* @return This target's pose oriented to the camera
*/
public static Pose3d getOrientedPose(Translation3d tgtTrl, Translation3d cameraTrl) {
var relCam = cameraTrl.minus(tgtTrl);
var orientToCam =
new Rotation3d(
0,
new Rotation2d(Math.hypot(relCam.getX(), relCam.getY()), -relCam.getZ()).getRadians(),
new Rotation2d(relCam.getX(), relCam.getY()).getRadians());
return new Pose3d(tgtTrl, orientToCam);
}
@Override

22
photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java
View File

@@ -35,6 +35,7 @@ import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import java.util.stream.Collectors;
import org.opencv.core.Point;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
@@ -66,7 +67,7 @@ public class VisionEstimation {
*
* @param cameraMatrix The camera intrinsics matrix in standard opencv form
* @param distCoeffs The camera distortion matrix in standard opencv form
* @param visTags The visible tags reported by PV
* @param visTags The visible tags reported by PV. Non-tag targets are automatically excluded.
* @param tagLayout The known tag layout on the field
* @return The transformation that maps the field origin to the camera pose. Ensure the {@link
* PNPResults} are present before utilizing them.
@@ -84,17 +85,28 @@ public class VisionEstimation {
}
var corners = new ArrayList<TargetCorner>();
for (var tag : visTags) corners.addAll(tag.getDetectedCorners());
var knownTags = getVisibleLayoutTags(visTags, tagLayout);
var knownTags = new ArrayList<AprilTag>();
// ensure these are AprilTags in our layout
for (var tgt : visTags) {
int id = tgt.getFiducialId();
tagLayout
.getTagPose(id)
.ifPresent(
pose -> {
knownTags.add(new AprilTag(id, pose));
corners.addAll(tgt.getDetectedCorners());
});
}
if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
return new PNPResults();
}
Point[] points = OpenCVHelp.cornersToPoints(corners);
// single-tag pnp
if (visTags.size() == 1) {
var camToTag =
OpenCVHelp.solvePNP_SQUARE(
cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, corners);
cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, points);
if (!camToTag.isPresent) return new PNPResults();
var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse());
var altPose = new Pose3d();
@@ -113,7 +125,7 @@ public class VisionEstimation {
else {
var objectTrls = new ArrayList<Translation3d>();
for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.getFieldVertices(tag.pose));
var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, corners);
var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points);
if (!camToOrigin.isPresent) return new PNPResults();
return new PNPResults(
camToOrigin.best.inverse(),

221
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View File

@@ -31,6 +31,7 @@ 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.Rotation2d;
import edu.wpi.first.util.RuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.ArrayList;
@@ -40,18 +41,20 @@ import org.opencv.core.Core;
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.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.estimation.RotTrlTransform3d;
import org.photonvision.estimation.TargetModel;
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
@@ -78,6 +81,8 @@ public class PhotonCameraSim implements AutoCloseable {
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;
@@ -200,28 +205,35 @@ public class PhotonCameraSim implements AutoCloseable {
* @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());
// var rel = new CameraTargetRelation(camPose, target.getPose());
// TODO: removal awaiting wpilib Rotation3d performance improvements
var relTarget = RotTrlTransform3d.makeRelativeTo(camPose).apply(target.getPose());
var camToTargYaw = new Rotation2d(relTarget.getX(), relTarget.getY());
var camToTargPitch =
new Rotation2d(Math.hypot(relTarget.getX(), relTarget.getY()), -relTarget.getZ());
var relCam = RotTrlTransform3d.makeRelativeTo(target.getPose()).apply(camPose);
var targToCamAngle = new Rotation2d(relCam.getX(), Math.hypot(relCam.getY(), relCam.getZ()));
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)
(Math.abs(camToTargYaw.getDegrees()) < prop.getHorizFOV().getDegrees() / 2)
&& (Math.abs(camToTargPitch.getDegrees()) < prop.getVertFOV().getDegrees() / 2)
&& (!target.getModel().isPlanar
|| Math.abs(rel.targToCamAngle.getDegrees())
|| Math.abs(targToCamAngle.getDegrees())
< 90) // camera is behind planar target and it should be occluded
&& (rel.camToTarg.getTranslation().getNorm() <= maxSightRangeMeters)); // target is too far
&& (relTarget.getTranslation().getNorm() <= maxSightRangeMeters)); // target is too far
}
/**
* Determines if all target corners are inside the camera's image.
* Determines if all target points are inside the camera's image.
*
* @param corners The corners of the target as image points(x,y)
* @param points The target's 2d image points
*/
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;
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;
@@ -289,11 +301,35 @@ public class PhotonCameraSim implements AutoCloseable {
if (sortMode != null) this.sortMode = sortMode;
}
/** Sets whether the raw video stream simulation is enabled. */
/**
* Sets whether the raw video stream simulation is enabled.
*
* <p>Note: This may increase loop times.
*/
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.
*/
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. */
public void enableProcessedStream(boolean enabled) {
videoSimProcEnabled = enabled;
@@ -310,10 +346,12 @@ public class PhotonCameraSim implements AutoCloseable {
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
// 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);
@@ -326,22 +364,64 @@ public class PhotonCameraSim implements AutoCloseable {
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));
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(targetCorners);
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 = OpenCVHelp.getMinAreaRect(noisyTargetCorners).center;
var centerRot = prop.getPixelRot(new TargetCorner(centerPt.x, centerPt.y));
var centerPt = minAreaRect.center;
var centerRot = prop.getPixelRot(centerPt);
// find contour area
double areaPercent = prop.getContourAreaPercent(noisyTargetCorners);
@@ -356,20 +436,8 @@ public class PhotonCameraSim implements AutoCloseable {
prop.getDistCoeffs(),
tgt.getModel().vertices,
noisyTargetCorners);
if (!pnpSim.isPresent) continue;
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()),
@@ -380,25 +448,76 @@ public class PhotonCameraSim implements AutoCloseable {
pnpSim.best,
pnpSim.alt,
pnpSim.ambiguity,
List.of(OpenCVHelp.pointsToTargetCorners(minAreaRectPts)),
noisyTargetCorners));
OpenCVHelp.pointsToCorners(minAreaRectPts),
OpenCVHelp.pointsToCorners(noisyTargetCorners)));
}
// render visible tags to raw video frame
if (videoSimRawEnabled) {
for (var tag : visibleTags) {
VideoSimUtil.warp16h5TagImage(
tag.getFirst(), OpenCVHelp.targetCornersToMat(tag.getSecond()), videoSimFrameRaw, true);
// 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.warp16h5TagImage(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 tag detection outline on processed view
// 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) {
if (tgt.getFiducialId() >= 0) { // apriltags
VideoSimUtil.drawTagDetection(
tgt.getFiducialId(),
OpenCVHelp.targetCornersToMat(tgt.getDetectedCorners()),
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);
}
}

266
photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java
View File

@@ -25,21 +25,30 @@
package org.photonvision.simulation;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.*;
import edu.wpi.first.wpilibj.DriverStation;
import java.io.IOException;
import java.nio.file.Path;
import java.util.ArrayList;
import java.util.List;
import java.util.Random;
import java.util.stream.Collectors;
import org.ejml.data.DMatrix3;
import org.ejml.dense.fixed.CommonOps_DDF3;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.targeting.TargetCorner;
import org.photonvision.estimation.RotTrlTransform3d;
/**
* Calibration and performance values for this camera.
@@ -69,6 +78,8 @@ public class SimCameraProperties {
private double exposureTimeMs = 0;
private double avgLatencyMs = 0;
private double latencyStdDevMs = 0;
// util
private List<DMatrix3> viewplanes = new ArrayList<>();
/** Default constructor which is the same as {@link #PERFECT_90DEG} */
public SimCameraProperties() {
@@ -151,25 +162,22 @@ public class SimCameraProperties {
}
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);
if (fovDiag.getDegrees() < 1 || fovDiag.getDegrees() > 179) {
fovDiag = Rotation2d.fromDegrees(MathUtil.clamp(fovDiag.getDegrees(), 1, 179));
DriverStation.reportError(
"Requested FOV width/height too large! Scaling below 180 degrees...", false);
"Requested invalid FOV! Clamping between (1, 179) degrees...", false);
}
double resDiag = Math.sqrt(resWidth * resWidth + resHeight * resHeight);
double diagRatio = Math.tan(fovDiag.getRadians() / 2);
var fovWidth = new Rotation2d(Math.atan(diagRatio * (resWidth / resDiag)) * 2);
var fovHeight = new Rotation2d(Math.atan(diagRatio * (resHeight / resDiag)) * 2);
// 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;
double cx = resWidth / 2.0 - 0.5;
double cy = resHeight / 2.0 - 0.5;
// use given fov to determine focal point (pixels)
double fx = cx / Math.tan(fovWidth.getRadians() / 2.0);
@@ -186,14 +194,30 @@ public class SimCameraProperties {
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;
// left, right, up, and down view planes
var p =
new Translation3d[] {
new Translation3d(
1,
new Rotation3d(0, 0, getPixelYaw(0).plus(new Rotation2d(-Math.PI / 2)).getRadians())),
new Translation3d(
1,
new Rotation3d(
0, 0, getPixelYaw(resWidth).plus(new Rotation2d(Math.PI / 2)).getRadians())),
new Translation3d(
1,
new Rotation3d(
0, getPixelPitch(0).plus(new Rotation2d(Math.PI / 2)).getRadians(), 0)),
new Translation3d(
1,
new Rotation3d(
0, getPixelPitch(resHeight).plus(new Rotation2d(-Math.PI / 2)).getRadians(), 0))
};
viewplanes.clear();
for (int i = 0; i < p.length; i++) {
viewplanes.add(new DMatrix3(p[i].getX(), p[i].getY(), p[i].getZ()));
}
}
public void setCalibError(double avgErrorPx, double errorStdDevPx) {
@@ -245,6 +269,11 @@ public class SimCameraProperties {
return resWidth * resHeight;
}
/** Width:height */
public double getAspectRatio() {
return (double) resWidth / resHeight;
}
public Matrix<N3, N3> getIntrinsics() {
return camIntrinsics.copy();
}
@@ -284,18 +313,16 @@ public class SimCameraProperties {
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
* @param points Points of the contour
*/
public double getContourAreaPercent(List<TargetCorner> corners) {
return OpenCVHelp.getContourAreaPx(corners) / getResArea() * 100;
public double getContourAreaPercent(Point[] points) {
return Imgproc.contourArea(new MatOfPoint2f(OpenCVHelp.getConvexHull(points)))
/ getResArea()
* 100;
}
/** The yaw from the principal point of this camera to the pixel x value. Positive values left. */
@@ -311,7 +338,7 @@ public class SimCameraProperties {
* 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)}.
* #getCorrectedPixelRot(Point)}.
*/
public Rotation2d getPixelPitch(double pixelY) {
double fy = camIntrinsics.get(1, 1);
@@ -326,9 +353,9 @@ public class SimCameraProperties {
* down.
*
* <p>Note that pitch is naively computed and may be incorrect. See {@link
* #getCorrectedPixelRot(TargetCorner)}.
* #getCorrectedPixelRot(Point)}.
*/
public Rotation3d getPixelRot(TargetCorner point) {
public Rotation3d getPixelRot(Point point) {
return new Rotation3d(
0, getPixelPitch(point.y).getRadians(), getPixelYaw(point.x).getRadians());
}
@@ -354,7 +381,7 @@ public class SimCameraProperties {
* @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) {
public Rotation3d getCorrectedPixelRot(Point point) {
double fx = camIntrinsics.get(0, 0);
double cx = camIntrinsics.get(0, 2);
double xOffset = cx - point.x;
@@ -389,47 +416,158 @@ public class SimCameraProperties {
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).
* Determines where the line segment defined by the two given translations intersects the camera's
* frustum/field-of-vision, if at all.
*
* @param points Pixel points on this camera's image
* @return Points mapped to an image of 1x1 resolution
* <p>The line is parametrized so any of its points <code>p = t * (b - a) + a</code>. This method
* returns these values of t, minimum first, defining the region of the line segment which is
* visible in the frustum. If both ends of the line segment are visible, this simply returns {0,
* 1}. If, for example, point b is visible while a is not, and half of the line segment is inside
* the camera frustum, {0.5, 1} would be returned.
*
* @param camRt The change in basis from world coordinates to camera coordinates. See {@link
* RotTrlTransform3d#makeRelativeTo(Pose3d)}.
* @param a The initial translation of the line
* @param b The final translation of the line
* @return A Pair of Doubles. The values may be null:
* <ul>
* <li>{Double, Double} : Two parametrized values(t), minimum first, representing which
* segment of the line is visible in the camera frustum.
* <li>{Double, null} : One value(t) representing a single intersection point. For example,
* the line only intersects the intersection of two adjacent viewplanes.
* <li>{null, null} : No values. The line segment is not visible in the camera frustum.
* </ul>
*/
public List<TargetCorner> getPixelFraction(List<TargetCorner> points) {
double resLarge = getAspectRatio() > 1 ? resWidth : resHeight;
public Pair<Double, Double> getVisibleLine(
RotTrlTransform3d camRt, Translation3d a, Translation3d b) {
// translations relative to the camera
var rela = camRt.apply(a);
var relb = camRt.apply(b);
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());
// check if both ends are behind camera
if (rela.getX() <= 0 && relb.getX() <= 0) return new Pair<>(null, null);
var av = new DMatrix3(rela.getX(), rela.getY(), rela.getZ());
var bv = new DMatrix3(relb.getX(), relb.getY(), relb.getZ());
// a to b
var abv = new DMatrix3();
CommonOps_DDF3.subtract(bv, av, abv);
// check if the ends of the line segment are visible
boolean aVisible = true;
boolean bVisible = true;
for (int i = 0; i < viewplanes.size(); i++) {
var normal = viewplanes.get(i);
double aVisibility = CommonOps_DDF3.dot(av, normal);
if (aVisibility < 0) aVisible = false;
double bVisibility = CommonOps_DDF3.dot(bv, normal);
if (bVisibility < 0) bVisible = false;
// both ends are outside at least one of the same viewplane
if (aVisibility <= 0 && bVisibility <= 0) return new Pair<>(null, null);
}
// both ends are inside frustum
if (aVisible && bVisible) return new Pair<>(Double.valueOf(0), Double.valueOf(1));
// parametrized (t=0 at a, t=1 at b) intersections with viewplanes
double[] intersections = {Double.NaN, Double.NaN, Double.NaN, Double.NaN};
// intersection points
List<DMatrix3> ipts = new ArrayList<>();
for (double val : intersections) ipts.add(null);
// find intersections
for (int i = 0; i < viewplanes.size(); i++) {
var normal = viewplanes.get(i);
// we want to know the value of t when the line intercepts this plane
// parametrized: v = t * ab + a, where v lies on the plane
// we can find the projection of a onto the plane normal
// a_projn = normal.times(av.dot(normal) / normal.dot(normal));
var a_projn = new DMatrix3();
CommonOps_DDF3.scale(
CommonOps_DDF3.dot(av, normal) / CommonOps_DDF3.dot(normal, normal), normal, a_projn);
// this projection lets us determine the scalar multiple t of ab where
// (t * ab + a) is a vector which lies on the plane
if (Math.abs(CommonOps_DDF3.dot(abv, a_projn)) < 1e-5) continue; // line is parallel to plane
intersections[i] = CommonOps_DDF3.dot(a_projn, a_projn) / -CommonOps_DDF3.dot(abv, a_projn);
// vector a to the viewplane
var apv = new DMatrix3();
CommonOps_DDF3.scale(intersections[i], abv, apv);
// av + apv = intersection point
var intersectpt = new DMatrix3();
CommonOps_DDF3.add(av, apv, intersectpt);
ipts.set(i, intersectpt);
// discard intersections outside the camera frustum
for (int j = 1; j < viewplanes.size(); j++) {
int oi = (i + j) % viewplanes.size();
var onormal = viewplanes.get(oi);
// if the dot of the intersection point with any plane normal is negative, it is outside
if (CommonOps_DDF3.dot(intersectpt, onormal) < 0) {
intersections[i] = Double.NaN;
ipts.set(i, null);
break;
}
}
// discard duplicate intersections
if (ipts.get(i) == null) continue;
for (int j = i - 1; j >= 0; j--) {
var oipt = ipts.get(j);
if (oipt == null) continue;
var diff = new DMatrix3();
CommonOps_DDF3.subtract(oipt, intersectpt, diff);
if (CommonOps_DDF3.elementMaxAbs(diff) < 1e-4) {
intersections[i] = Double.NaN;
ipts.set(i, null);
break;
}
}
}
// determine visible segment (minimum and maximum t)
double inter1 = Double.NaN;
double inter2 = Double.NaN;
for (double inter : intersections) {
if (!Double.isNaN(inter)) {
if (Double.isNaN(inter1)) inter1 = inter;
else inter2 = inter;
}
}
// two viewplane intersections
if (!Double.isNaN(inter2)) {
double max = Math.max(inter1, inter2);
double min = Math.min(inter1, inter2);
if (aVisible) min = 0;
if (bVisible) max = 1;
return new Pair<>(min, max);
}
// one viewplane intersection
else if (!Double.isNaN(inter1)) {
if (aVisible) return new Pair<>(Double.valueOf(0), inter1);
if (bVisible) return new Pair<>(inter1, Double.valueOf(1));
return new Pair<>(inter1, null);
}
// no intersections
else return new Pair<>(null, null);
}
/** Returns these points after applying this camera's estimated noise. */
public List<TargetCorner> estPixelNoise(List<TargetCorner> points) {
public Point[] estPixelNoise(Point[] 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());
Point[] noisyPts = new Point[points.length];
for (int i = 0; i < points.length; i++) {
var p = points[i];
// error pixels in random direction
double error = avgErrorPx + rand.nextGaussian() * errorStdDevPx;
double errorAngle = rand.nextDouble() * 2 * Math.PI - Math.PI;
noisyPts[i] =
new Point(p.x + error * Math.cos(errorAngle), p.y + error * Math.sin(errorAngle));
}
return noisyPts;
}
/**

326
photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java
View File

@@ -25,9 +25,13 @@
package org.photonvision.simulation;
import edu.wpi.first.cscore.CvSource;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.RuntimeLoader;
import java.awt.image.BufferedImage;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashMap;
import java.util.List;
@@ -45,6 +49,7 @@ import org.opencv.core.Size;
import org.opencv.imgcodecs.Imgcodecs;
import org.opencv.imgproc.Imgproc;
import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.estimation.RotTrlTransform3d;
public class VideoSimUtil {
public static final String kLocalTagImagesPath = "./src/main/resources/images/apriltags/";
@@ -57,6 +62,10 @@ public class VideoSimUtil {
// Points corresponding to marker(black square) corners of 8x8 16h5 tag images
public static final Point[] kTag16h5MarkerPts;
// field dimensions for wireframe
private static double fieldLength = 16.54175;
private static double fieldWidth = 8.0137;
static {
try {
var loader =
@@ -166,23 +175,23 @@ public class VideoSimUtil {
* @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...)
* @param destination The destination image to place the warped tag image onto.
*/
public static void warp16h5TagImage(
int tagId, MatOfPoint2f dstPoints, Mat destination, boolean antialiasing) {
int tagId, Point[] dstPoints, boolean antialiasing, Mat destination) {
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()));
var dstPointMat = new MatOfPoint2f(dstPoints);
// the rectangle describing the rectangle-of-interest(ROI)
var boundingRect = Imgproc.boundingRect(dstPoints);
var boundingRect = Imgproc.boundingRect(dstPointMat);
// find the perspective transform from the tag image to the warped destination points
Mat perspecTrf = Imgproc.getPerspectiveTransform(tagPoints, dstPoints);
Mat perspecTrf = Imgproc.getPerspectiveTransform(tagPoints, dstPointMat);
// check extreme image corners after transform to check if we need to expand bounding rect
var extremeCorners = new MatOfPoint2f();
Core.perspectiveTransform(tagImageCorners, extremeCorners, perspecTrf);
@@ -238,12 +247,12 @@ public class VideoSimUtil {
// upscale if supersampling
Mat scaledDstPts = new Mat();
if (supersampling > 1) {
Core.multiply(dstPoints, new Scalar(supersampling, supersampling), scaledDstPts);
Core.multiply(dstPointMat, new Scalar(supersampling, supersampling), scaledDstPts);
boundingRect.x *= supersampling;
boundingRect.y *= supersampling;
boundingRect.width *= supersampling;
boundingRect.height *= supersampling;
} else dstPoints.assignTo(scaledDstPts);
} else dstPointMat.assignTo(scaledDstPts);
// update transform relative to expanded, scaled bounding rect
Core.subtract(scaledDstPts, new Scalar(boundingRect.tl().x, boundingRect.tl().y), scaledDstPts);
@@ -291,6 +300,58 @@ public class VideoSimUtil {
tempROI.copyTo(destination.submat(boundingRect), tempMask);
}
/**
* Given a line thickness in a 640x480 image, try to scale to the given destination image
* resolution.
*
* @param thickness480p A hypothetical line thickness in a 640x480 image
* @param destinationImg The destination image to scale to
* @return Scaled thickness which cannot be less than 1
*/
public static double getScaledThickness(double thickness480p, Mat destinationImg) {
double scaleX = destinationImg.width() / 640.0;
double scaleY = destinationImg.height() / 480.0;
double minScale = Math.min(scaleX, scaleY);
return Math.max(thickness480p * minScale, 1.0);
}
/**
* Draw a filled ellipse in the destination image.
*
* @param dstPoints The points in the destination image representing the rectangle in which the
* ellipse is inscribed.
* @param color The color of the ellipse. This is a scalar with BGR values (0-255)
* @param destination The destination image to draw onto. The image should be in the BGR color
* space.
*/
public static void drawInscribedEllipse(Point[] dstPoints, Scalar color, Mat destination) {
// create RotatedRect from points
var rect = OpenCVHelp.getMinAreaRect(dstPoints);
// inscribe ellipse inside rectangle
Imgproc.ellipse(destination, rect, color, -1, Imgproc.LINE_AA);
}
/**
* Draw a polygon outline or filled polygon to the destination image with the given points.
*
* @param dstPoints The points in the destination image representing the polygon.
* @param thickness The thickness of the outline in pixels. If this is not positive, a filled
* polygon is drawn instead.
* @param color The color drawn. This should match the color space of the destination image.
* @param isClosed If the last point should connect to the first point in the polygon outline.
* @param destination The destination image to draw onto.
*/
public static void drawPoly(
Point[] dstPoints, int thickness, Scalar color, boolean isClosed, Mat destination) {
var dstPointsd = new MatOfPoint(dstPoints);
if (thickness > 0) {
Imgproc.polylines(
destination, List.of(dstPointsd), isClosed, color, thickness, Imgproc.LINE_AA);
} else {
Imgproc.fillPoly(destination, List.of(dstPointsd), color, Imgproc.LINE_AA);
}
}
/**
* Draws a contour around the given points and text of the id onto the destination image.
*
@@ -300,31 +361,242 @@ public class VideoSimUtil {
* @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;
public static void drawTagDetection(int id, Point[] dstPoints, Mat destination) {
double thickness = getScaledThickness(1, destination);
drawPoly(dstPoints, (int) thickness, new Scalar(0, 0, 255), true, destination);
var rect = Imgproc.boundingRect(new MatOfPoint(dstPoints));
var textPt = new Point(rect.x + rect.width, rect.y);
textPt.x += thickness;
textPt.y += thickness;
Imgproc.putText(
destination,
String.valueOf(id),
textPt,
Imgproc.FONT_HERSHEY_PLAIN,
1.5 * minScale,
new Scalar(0, 0, 255),
thickness,
1.5 * thickness,
new Scalar(0, 200, 0),
(int) thickness,
Imgproc.LINE_AA);
}
/**
* Set the field dimensions that are used for drawing the field wireframe.
*
* @param fieldLengthMeters
* @param fieldWidthMeters
*/
public static void setFieldDimensionsMeters(double fieldLengthMeters, double fieldWidthMeters) {
fieldLength = fieldLengthMeters;
fieldWidth = fieldWidthMeters;
}
/**
* The translations used to draw the field side walls and driver station walls. It is a List of
* Lists because the translations are not all connected.
*/
private static List<List<Translation3d>> getFieldWallLines() {
var list = new ArrayList<List<Translation3d>>();
final double sideHt = Units.inchesToMeters(19.5);
final double driveHt = Units.inchesToMeters(35);
final double topHt = Units.inchesToMeters(78);
// field floor
list.add(
List.of(
new Translation3d(0, 0, 0),
new Translation3d(fieldLength, 0, 0),
new Translation3d(fieldLength, fieldWidth, 0),
new Translation3d(0, fieldWidth, 0),
new Translation3d(0, 0, 0)));
// right side wall
list.add(
List.of(
new Translation3d(0, 0, 0),
new Translation3d(0, 0, sideHt),
new Translation3d(fieldLength, 0, sideHt),
new Translation3d(fieldLength, 0, 0)));
// red driverstation
list.add(
List.of(
new Translation3d(fieldLength, 0, sideHt),
new Translation3d(fieldLength, 0, topHt),
new Translation3d(fieldLength, fieldWidth, topHt),
new Translation3d(fieldLength, fieldWidth, sideHt)));
list.add(
List.of(
new Translation3d(fieldLength, 0, driveHt),
new Translation3d(fieldLength, fieldWidth, driveHt)));
// left side wall
list.add(
List.of(
new Translation3d(0, fieldWidth, 0),
new Translation3d(0, fieldWidth, sideHt),
new Translation3d(fieldLength, fieldWidth, sideHt),
new Translation3d(fieldLength, fieldWidth, 0)));
// blue driverstation
list.add(
List.of(
new Translation3d(0, 0, sideHt),
new Translation3d(0, 0, topHt),
new Translation3d(0, fieldWidth, topHt),
new Translation3d(0, fieldWidth, sideHt)));
list.add(List.of(new Translation3d(0, 0, driveHt), new Translation3d(0, fieldWidth, driveHt)));
return list;
}
/**
* The translations used to draw the field floor subdivisions (not the floor outline). It is a
* List of Lists because the translations are not all connected.
*
* @param subdivisions How many "subdivisions" along the width/length of the floor. E.g. 3
* subdivisions would mean 2 lines along the length and 2 lines along the width creating a 3x3
* "grid".
*/
private static List<List<Translation3d>> getFieldFloorLines(int subdivisions) {
var list = new ArrayList<List<Translation3d>>();
final double subLength = fieldLength / subdivisions;
final double subWidth = fieldWidth / subdivisions;
// field floor subdivisions
for (int i = 0; i < subdivisions; i++) {
list.add(
List.of(
new Translation3d(0, subWidth * (i + 1), 0),
new Translation3d(fieldLength, subWidth * (i + 1), 0)));
list.add(
List.of(
new Translation3d(subLength * (i + 1), 0, 0),
new Translation3d(subLength * (i + 1), fieldWidth, 0)));
}
return list;
}
/**
* Convert 3D lines represented by the given series of translations into a polygon(s) in the
* camera's image.
*
* @param camRt The change in basis from world coordinates to camera coordinates. See {@link
* RotTrlTransform3d#makeRelativeTo(Pose3d)}.
* @param prop The simulated camera's properties.
* @param trls A sequential series of translations defining the polygon to be drawn.
* @param resolution Resolution as a fraction(0 - 1) of the video frame's diagonal length in
* pixels. Line segments will be subdivided if they exceed this resolution.
* @param isClosed If the final translation should also draw a line to the first translation.
* @param destination The destination image that is being drawn to.
* @return A list of polygons(which are an array of points)
*/
public static List<Point[]> polyFrom3dLines(
RotTrlTransform3d camRt,
SimCameraProperties prop,
List<Translation3d> trls,
double resolution,
boolean isClosed,
Mat destination) {
resolution = Math.hypot(destination.size().height, destination.size().width) * resolution;
List<Translation3d> pts = new ArrayList<>(trls);
if (isClosed) pts.add(pts.get(0));
List<Point[]> polyPointList = new ArrayList<>();
for (int i = 0; i < pts.size() - 1; i++) {
var pta = pts.get(i);
var ptb = pts.get(i + 1);
// check if line is inside camera fulcrum
var inter = prop.getVisibleLine(camRt, pta, ptb);
if (inter.getSecond() == null) continue;
// cull line to the inside of the camera fulcrum
double inter1 = inter.getFirst().doubleValue();
double inter2 = inter.getSecond().doubleValue();
var baseDelta = ptb.minus(pta);
var old_pta = pta;
if (inter1 > 0) pta = old_pta.plus(baseDelta.times(inter1));
if (inter2 < 1) ptb = old_pta.plus(baseDelta.times(inter2));
baseDelta = ptb.minus(pta);
// project points into 2d
var poly = new ArrayList<Point>();
poly.addAll(
Arrays.asList(
OpenCVHelp.projectPoints(
prop.getIntrinsics(), prop.getDistCoeffs(), camRt, List.of(pta, ptb))));
var pxa = poly.get(0);
var pxb = poly.get(1);
// subdivide projected line based on desired resolution
double pxDist = Math.hypot(pxb.x - pxa.x, pxb.y - pxa.y);
int subdivisions = (int) (pxDist / resolution);
var subDelta = baseDelta.div(subdivisions + 1);
var subPts = new ArrayList<Translation3d>();
for (int j = 0; j < subdivisions; j++) {
subPts.add(pta.plus(subDelta.times(j + 1)));
}
if (subPts.size() > 0) {
poly.addAll(
1,
Arrays.asList(
OpenCVHelp.projectPoints(
prop.getIntrinsics(), prop.getDistCoeffs(), camRt, subPts)));
}
polyPointList.add(poly.toArray(Point[]::new));
}
return polyPointList;
}
/**
* Draw a wireframe of the field to the given image.
*
* @param camRt The change in basis from world coordinates to camera coordinates. See {@link
* RotTrlTransform3d#makeRelativeTo(Pose3d)}.
* @param prop The simulated camera's properties.
* @param resolution Resolution as a fraction(0 - 1) of the video frame's diagonal length in
* pixels. Line segments will be subdivided if they exceed this resolution.
* @param wallThickness Thickness of the lines used for drawing the field walls in pixels. This is
* scaled by {@link #getScaledThickness(double, Mat)}.
* @param wallColor Color of the lines used for drawing the field walls.
* @param floorSubdivisions A NxN "grid" is created from the floor where this parameter is N,
* which defines the floor lines.
* @param floorThickness Thickness of the lines used for drawing the field floor grid in pixels.
* This is scaled by {@link #getScaledThickness(double, Mat)}.
* @param floorColor Color of the lines used for drawing the field floor grid.
* @param destination The destination image to draw to.
*/
public static void drawFieldWireframe(
RotTrlTransform3d camRt,
SimCameraProperties prop,
double resolution,
double wallThickness,
Scalar wallColor,
int floorSubdivisions,
double floorThickness,
Scalar floorColor,
Mat destination) {
for (var trls : getFieldFloorLines(floorSubdivisions)) {
var polys = VideoSimUtil.polyFrom3dLines(camRt, prop, trls, resolution, false, destination);
for (var poly : polys) {
drawPoly(
poly,
(int) Math.round(getScaledThickness(floorThickness, destination)),
floorColor,
false,
destination);
}
}
for (var trls : getFieldWallLines()) {
var polys = VideoSimUtil.polyFrom3dLines(camRt, prop, trls, resolution, false, destination);
for (var poly : polys) {
drawPoly(
poly,
(int) Math.round(getScaledThickness(wallThickness, destination)),
wallColor,
false,
destination);
}
}
}
}

45
photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java
View File

@@ -130,7 +130,8 @@ public class VisionSystemSim {
* 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
* @param cameraSim The specific camera to get the robot-to-camera transform of
* @return The transform of this camera, or an empty optional if it is invalid
*/
public Optional<Transform3d> getRobotToCamera(PhotonCameraSim cameraSim) {
return getRobotToCamera(cameraSim, Timer.getFPGATimestamp());
@@ -140,9 +141,9 @@ public class VisionSystemSim {
* 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 cameraSim The 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
* @return The transform of this camera, or an empty optional if it is invalid
*/
public Optional<Transform3d> getRobotToCamera(PhotonCameraSim cameraSim, double timeSeconds) {
var trfBuffer = camTrfMap.get(cameraSim);
@@ -152,6 +153,31 @@ public class VisionSystemSim {
return Optional.of(new Transform3d(new Pose3d(), sample.orElse(new Pose3d())));
}
/**
* Get a simulated camera's position on the field. If the requested camera is invalid, an empty
* optional is returned.
*
* @param cameraSim The specific camera to get the field pose of
* @return The pose of this camera, or an empty optional if it is invalid
*/
public Optional<Pose3d> getCameraPose(PhotonCameraSim cameraSim) {
return getCameraPose(cameraSim, Timer.getFPGATimestamp());
}
/**
* Get a simulated camera's position on the field. If the requested camera is invalid, an empty
* optional is returned.
*
* @param cameraSim The specific camera to get the field pose of
* @param timeSeconds Timestamp in seconds of when the pose should be observed
* @return The pose of this camera, or an empty optional if it is invalid
*/
public Optional<Pose3d> getCameraPose(PhotonCameraSim cameraSim, double timeSeconds) {
var robotToCamera = getRobotToCamera(cameraSim, timeSeconds);
if (robotToCamera.isEmpty()) return Optional.empty();
return Optional.of(getRobotPose(timeSeconds).plus(robotToCamera.get()));
}
/**
* 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.
@@ -217,15 +243,16 @@ public class VisionSystemSim {
* 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.
* <p>The AprilTags from this layout will be added as vision targets under the type "apriltag".
* The poses added preserve the tag layout's current alliance origin. If the tag layout's alliance
* origin is changed, these added tags will have to be cleared and re-added.
*
* @param tagLayout The field tag layout to get Apriltag poses and IDs from
*/
public void addVisionTargets(AprilTagFieldLayout tagLayout) {
public void addAprilTags(AprilTagFieldLayout tagLayout) {
for (AprilTag tag : tagLayout.getTags()) {
addVisionTargets(
"apriltags",
"apriltag",
new VisionTargetSim(
tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation
TargetModel.kTag16h5,
@@ -252,6 +279,10 @@ public class VisionSystemSim {
targetSets.clear();
}
public void clearAprilTags() {
removeVisionTargets("apriltag");
}
public Set<VisionTargetSim> removeVisionTargets(String type) {
return targetSets.remove(type);
}