diff --git a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java index daf3fff685..d6f1c91cce 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -193,7 +193,7 @@ public static List pointsToCorners(MatOfPoint2f matInput) { var corners = new ArrayList(); float[] data = new float[(int) matInput.total() * matInput.channels()]; matInput.get(0, 0, data); - for (int i = 0; i < (int)matInput.total(); i++) { + for (int i = 0; i < (int) matInput.total(); i++) { corners.add(new TargetCorner(data[0 + 2 * i], data[1 + 2 * i])); } return corners; @@ -307,9 +307,9 @@ public static Point[] projectPoints( /** * Undistort 2d image points using a given camera's intrinsics and distortion. * - *

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). + *

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 @@ -412,8 +412,8 @@ public static Point[] getConvexHull(Point[] points) { *

  • Point 3: [0, -squareLength / 2, -squareLength / 2] * * - * @param imagePoints 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. */ @@ -519,8 +519,8 @@ public static PNPResults solvePNP_SQUARE( * @param cameraMatrix The camera intrinsics matrix in standard opencv form * @param distCoeffs The camera distortion matrix in standard opencv form * @param objectTrls The translations of the object corners, relative to the field. - * @param imagePoints The projection of these 3d object points into the 2d camera image. The - * order should match the given object point translations. + * @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. diff --git a/photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java b/photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java index b0244a0e23..90044abb38 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/TargetModel.java @@ -71,8 +71,7 @@ public TargetModel(double widthMeters, double heightMeters) { } /** - * Creates a cuboid target model given the length, width, height. The model has eight - * vertices: + * Creates a cuboid target model given the length, width, height. The model has eight vertices: * *
      *
    • Point 0: [length/2, -width/2, -height/2] @@ -86,16 +85,16 @@ public TargetModel(double widthMeters, double heightMeters) { *
    */ 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) - )); + 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))); } /** @@ -111,8 +110,8 @@ public TargetModel(double lengthMeters, double widthMeters, double heightMeters) * * Q: Why these vertices? 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. + * 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) { double radius = diameterMeters / 2.0; diff --git a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java index 392ffd3c2f..f5fdb2953d 100644 --- a/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java +++ b/photon-lib/src/main/java/org/photonvision/estimation/VisionEstimation.java @@ -35,7 +35,6 @@ 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; @@ -68,7 +67,7 @@ public static List getVisibleLayoutTags( * * @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. Non-tag targets are automatically excluded. + * @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. @@ -90,10 +89,13 @@ public static PNPResults estimateCamPosePNP( // 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()); - }); + 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(); diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index beb349e8a9..55a1ce3060 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -40,7 +40,6 @@ import org.opencv.core.Core; import org.opencv.core.CvType; import org.opencv.core.Mat; -import org.opencv.core.MatOfPoint2f; import org.opencv.core.Point; import org.opencv.core.RotatedRect; import org.opencv.core.Scalar; @@ -56,7 +55,6 @@ 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 @@ -369,43 +367,45 @@ public PhotonPipelineResult process( 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 + // 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())); + 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) { + 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; + 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; + 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; + 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; + 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]) - ); + 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); @@ -473,8 +473,7 @@ public PhotonPipelineResult process( var corn = pair.getSecond(); if (tgt.fiducialID >= 0) { // apriltags - VideoSimUtil.warp16h5TagImage( - tgt.fiducialID, corn, true, videoSimFrameRaw); + VideoSimUtil.warp16h5TagImage(tgt.fiducialID, corn, true, videoSimFrameRaw); } else if (!tgt.getModel().isSpherical) { // non-spherical targets var contour = corn; if (!tgt.getModel() @@ -514,12 +513,11 @@ public PhotonPipelineResult process( Imgproc.LINE_AA); VideoSimUtil.drawPoly( - OpenCVHelp.cornersToPoints(tgt.getDetectedCorners()), - (int) VideoSimUtil.getScaledThickness(1, videoSimFrameProcessed), - new Scalar(255, 20, 20), - true, - videoSimFrameProcessed - ); + OpenCVHelp.cornersToPoints(tgt.getDetectedCorners()), + (int) VideoSimUtil.getScaledThickness(1, videoSimFrameProcessed), + new Scalar(255, 20, 20), + true, + videoSimFrameProcessed); } } videoSimProcessed.putFrame(videoSimFrameProcessed); diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java index 4d6f5837ee..7a3c16aacf 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java @@ -322,7 +322,9 @@ public SimCameraProperties copy() { * @param points Points of the contour */ public double getContourAreaPercent(Point[] points) { - return Imgproc.contourArea(new MatOfPoint2f(OpenCVHelp.getConvexHull(points))) / getResArea() * 100; + 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. */ @@ -563,13 +565,13 @@ public Point[] estPixelNoise(Point[] points) { if (avgErrorPx == 0 && errorStdDevPx == 0) return points; Point[] noisyPts = new Point[points.length]; - for(int i = 0; i < points.length; i++) { + 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)); + noisyPts[i] = + new Point(p.x + error * Math.cos(errorAngle), p.y + error * Math.sin(errorAngle)); } return noisyPts; } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java index b6912e0e34..f52c1ca7ad 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java @@ -317,8 +317,8 @@ public static double getScaledThickness(double thickness480p, Mat destinationImg /** * 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 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. @@ -474,8 +474,8 @@ private static List> getFieldFloorLines(int subdivisions) { } /** - * Convert 3D lines represented by the given series of translations into a polygon(s) in the camera's - * image. + * 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)}. @@ -518,8 +518,10 @@ public static List polyFrom3dLines( // project points into 2d var poly = new ArrayList(); - poly.addAll(Arrays.asList(OpenCVHelp.projectPoints( - prop.getIntrinsics(), prop.getDistCoeffs(), camRt, List.of(pta, ptb)))); + 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); @@ -533,7 +535,10 @@ public static List polyFrom3dLines( } if (subPts.size() > 0) { poly.addAll( - 1, Arrays.asList(OpenCVHelp.projectPoints(prop.getIntrinsics(), prop.getDistCoeffs(), camRt, subPts))); + 1, + Arrays.asList( + OpenCVHelp.projectPoints( + prop.getIntrinsics(), prop.getDistCoeffs(), camRt, subPts))); } polyPointList.add(poly.toArray(Point[]::new)); @@ -572,24 +577,24 @@ public static void drawFieldWireframe( Mat destination) { for (var trls : getFieldFloorLines(floorSubdivisions)) { var polys = VideoSimUtil.polyFrom3dLines(camRt, prop, trls, resolution, false, destination); - for(var poly : polys) { + for (var poly : polys) { drawPoly( - poly, - (int) Math.round(getScaledThickness(floorThickness, destination)), - floorColor, - false, - destination); + 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) { + for (var poly : polys) { drawPoly( - poly, - (int) Math.round(getScaledThickness(wallThickness, destination)), - wallColor, - false, - destination); + poly, + (int) Math.round(getScaledThickness(wallThickness, destination)), + wallColor, + false, + destination); } } } diff --git a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java index 7e6e62ef48..e483833111 100644 --- a/photon-lib/src/test/java/org/photonvision/OpenCVTest.java +++ b/photon-lib/src/test/java/org/photonvision/OpenCVTest.java @@ -159,8 +159,9 @@ public void testProjection() { assertTrue(circulation > 0, "2d fiducial points aren't counter-clockwise"); // undo projection distortion - imagePoints = OpenCVHelp.undistortPoints(prop.getIntrinsics(), prop.getDistCoeffs(), imagePoints); - + imagePoints = + OpenCVHelp.undistortPoints(prop.getIntrinsics(), prop.getDistCoeffs(), imagePoints); + // test projection results after moving camera var avgCenterRot1 = prop.getPixelRot(OpenCVHelp.avgPoint(imagePoints)); cameraPose =