From c2683ce049de26bd6ddb83f012e62be0337c9929 Mon Sep 17 00:00:00 2001 From: amquake Date: Fri, 6 Jan 2023 18:42:31 -0800 Subject: [PATCH 1/3] remove distortion logs --- .../pipe/impl/AprilTagPoseEstimatorPipe.java | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java index 2ff9a3fddb..758e66fa08 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java @@ -51,14 +51,11 @@ protected AprilTagPoseEstimate process(AprilTagDetection in) { for (int i = 0; i < 4; i++) { corners[i] = new Point(in.getCornerX(i), in.getCornerY(i)); } - System.out.println("Before: " + Arrays.toString(corners)); // And shove into our matofpoints temp.fromArray(corners); - System.out.println("Size " + temp.size().toString()); - // Probably overwrites what was in temp before. I hope - Calib3d.undistortPoints( + Calib3d.undistortImagePoints( temp, temp, params.calibration.getCameraIntrinsicsMat(), @@ -70,14 +67,9 @@ protected AprilTagPoseEstimate process(AprilTagDetection in) { // Apriltagdetection expects an array in form [x1 y1 x2 y2 ...] var fixedCorners = new double[8]; for (int i = 0; i < 4; i++) { - // https://stackoverflow.com/questions/8499984/how-to-undistort-points-in-camera-shot-coordinates-and-obtain-corresponding-undi - // perform transformation. - // In fact this is equivalent to multiplication to camera matrix - - fixedCorners[i * 2] = corners[i].x * params.config.fx + params.config.cx; - fixedCorners[i * 2 + 1] = corners[i].y * params.config.fy + params.config.cy; + fixedCorners[i * 2] = corners[i].x; + fixedCorners[i * 2 + 1] = corners[i].y; } - System.out.println("After: " + Arrays.toString(fixedCorners)); // Create a new Detection with the fixed corners var corrected = @@ -91,7 +83,7 @@ protected AprilTagPoseEstimate process(AprilTagDetection in) { in.getCenterY(), fixedCorners); - return m_poseEstimator.estimateOrthogonalIteration(in, params.nIters); + return m_poseEstimator.estimateOrthogonalIteration(corrected, params.nIters); } @Override From 175c95929656fafef280b66c75d2820114d3e6ba Mon Sep 17 00:00:00 2001 From: amquake Date: Fri, 6 Jan 2023 18:50:01 -0800 Subject: [PATCH 2/3] spotless --- .../pipe/impl/AprilTagPoseEstimatorPipe.java | 271 +++++++++--------- 1 file changed, 135 insertions(+), 136 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java index 758e66fa08..1a029bbcda 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java @@ -1,136 +1,135 @@ -/* - * Copyright (C) Photon Vision. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -package org.photonvision.vision.pipe.impl; - -import edu.wpi.first.apriltag.AprilTagDetection; -import edu.wpi.first.apriltag.AprilTagPoseEstimate; -import edu.wpi.first.apriltag.AprilTagPoseEstimator; -import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config; -import java.util.Arrays; -import org.opencv.calib3d.Calib3d; -import org.opencv.core.MatOfPoint2f; -import org.opencv.core.Point; -import org.photonvision.vision.calibration.CameraCalibrationCoefficients; -import org.photonvision.vision.pipe.CVPipe; - -public class AprilTagPoseEstimatorPipe - extends CVPipe< - AprilTagDetection, - AprilTagPoseEstimate, - AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams> { - private final AprilTagPoseEstimator m_poseEstimator = - new AprilTagPoseEstimator(new AprilTagPoseEstimator.Config(0, 0, 0, 0, 0)); - - boolean useNativePoseEst; - - public AprilTagPoseEstimatorPipe() { - super(); - } - - MatOfPoint2f temp = new MatOfPoint2f(); - - @Override - protected AprilTagPoseEstimate process(AprilTagDetection in) { - // Save the corner points of our detection to an array - Point corners[] = new Point[4]; - for (int i = 0; i < 4; i++) { - corners[i] = new Point(in.getCornerX(i), in.getCornerY(i)); - } - // And shove into our matofpoints - temp.fromArray(corners); - - // Probably overwrites what was in temp before. I hope - Calib3d.undistortImagePoints( - temp, - temp, - params.calibration.getCameraIntrinsicsMat(), - params.calibration.getDistCoeffsMat()); - - // Save out undistorted corners - corners = temp.toArray(); - - // Apriltagdetection expects an array in form [x1 y1 x2 y2 ...] - var fixedCorners = new double[8]; - for (int i = 0; i < 4; i++) { - fixedCorners[i * 2] = corners[i].x; - fixedCorners[i * 2 + 1] = corners[i].y; - } - - // Create a new Detection with the fixed corners - var corrected = - new AprilTagDetection( - in.getFamily(), - in.getId(), - in.getHamming(), - in.getDecisionMargin(), - in.getHomography(), - in.getCenterX(), - in.getCenterY(), - fixedCorners); - - return m_poseEstimator.estimateOrthogonalIteration(corrected, params.nIters); - } - - @Override - public void setParams(AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams newParams) { - if (this.params == null || !this.params.equals(newParams)) { - m_poseEstimator.setConfig(newParams.config); - } - - super.setParams(newParams); - } - - public void setNativePoseEstimationEnabled(boolean enabled) { - this.useNativePoseEst = enabled; - } - - public static class AprilTagPoseEstimatorPipeParams { - final AprilTagPoseEstimator.Config config; - final CameraCalibrationCoefficients calibration; - final int nIters; - - public AprilTagPoseEstimatorPipeParams( - Config config, CameraCalibrationCoefficients cal, int nIters) { - this.config = config; - this.nIters = nIters; - this.calibration = cal; - } - - @Override - public int hashCode() { - final int prime = 31; - int result = 1; - result = prime * result + ((config == null) ? 0 : config.hashCode()); - result = prime * result + nIters; - return result; - } - - @Override - public boolean equals(Object obj) { - if (this == obj) return true; - if (obj == null) return false; - if (getClass() != obj.getClass()) return false; - AprilTagPoseEstimatorPipeParams other = (AprilTagPoseEstimatorPipeParams) obj; - if (config == null) { - if (other.config != null) return false; - } else if (!config.equals(other.config)) return false; - if (nIters != other.nIters) return false; - return true; - } - } -} +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision.vision.pipe.impl; + +import edu.wpi.first.apriltag.AprilTagDetection; +import edu.wpi.first.apriltag.AprilTagPoseEstimate; +import edu.wpi.first.apriltag.AprilTagPoseEstimator; +import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Point; +import org.photonvision.vision.calibration.CameraCalibrationCoefficients; +import org.photonvision.vision.pipe.CVPipe; + +public class AprilTagPoseEstimatorPipe + extends CVPipe< + AprilTagDetection, + AprilTagPoseEstimate, + AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams> { + private final AprilTagPoseEstimator m_poseEstimator = + new AprilTagPoseEstimator(new AprilTagPoseEstimator.Config(0, 0, 0, 0, 0)); + + boolean useNativePoseEst; + + public AprilTagPoseEstimatorPipe() { + super(); + } + + MatOfPoint2f temp = new MatOfPoint2f(); + + @Override + protected AprilTagPoseEstimate process(AprilTagDetection in) { + // Save the corner points of our detection to an array + Point corners[] = new Point[4]; + for (int i = 0; i < 4; i++) { + corners[i] = new Point(in.getCornerX(i), in.getCornerY(i)); + } + // And shove into our matofpoints + temp.fromArray(corners); + + // Probably overwrites what was in temp before. I hope + Calib3d.undistortImagePoints( + temp, + temp, + params.calibration.getCameraIntrinsicsMat(), + params.calibration.getDistCoeffsMat()); + + // Save out undistorted corners + corners = temp.toArray(); + + // Apriltagdetection expects an array in form [x1 y1 x2 y2 ...] + var fixedCorners = new double[8]; + for (int i = 0; i < 4; i++) { + fixedCorners[i * 2] = corners[i].x; + fixedCorners[i * 2 + 1] = corners[i].y; + } + + // Create a new Detection with the fixed corners + var corrected = + new AprilTagDetection( + in.getFamily(), + in.getId(), + in.getHamming(), + in.getDecisionMargin(), + in.getHomography(), + in.getCenterX(), + in.getCenterY(), + fixedCorners); + + return m_poseEstimator.estimateOrthogonalIteration(corrected, params.nIters); + } + + @Override + public void setParams(AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams newParams) { + if (this.params == null || !this.params.equals(newParams)) { + m_poseEstimator.setConfig(newParams.config); + } + + super.setParams(newParams); + } + + public void setNativePoseEstimationEnabled(boolean enabled) { + this.useNativePoseEst = enabled; + } + + public static class AprilTagPoseEstimatorPipeParams { + final AprilTagPoseEstimator.Config config; + final CameraCalibrationCoefficients calibration; + final int nIters; + + public AprilTagPoseEstimatorPipeParams( + Config config, CameraCalibrationCoefficients cal, int nIters) { + this.config = config; + this.nIters = nIters; + this.calibration = cal; + } + + @Override + public int hashCode() { + final int prime = 31; + int result = 1; + result = prime * result + ((config == null) ? 0 : config.hashCode()); + result = prime * result + nIters; + return result; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null) return false; + if (getClass() != obj.getClass()) return false; + AprilTagPoseEstimatorPipeParams other = (AprilTagPoseEstimatorPipeParams) obj; + if (config == null) { + if (other.config != null) return false; + } else if (!config.equals(other.config)) return false; + if (nIters != other.nIters) return false; + return true; + } + } +} From e4c7ac4bb5b29639694e73f10e9e7fa9336827b7 Mon Sep 17 00:00:00 2001 From: Matt Date: Fri, 6 Jan 2023 19:21:07 -0800 Subject: [PATCH 3/3] Run spotless :skull: --- .../pipe/impl/AprilTagPoseEstimatorPipe.java | 270 +++++++++--------- 1 file changed, 135 insertions(+), 135 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java index 1a029bbcda..1dc118a3eb 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AprilTagPoseEstimatorPipe.java @@ -1,135 +1,135 @@ -/* - * Copyright (C) Photon Vision. - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -package org.photonvision.vision.pipe.impl; - -import edu.wpi.first.apriltag.AprilTagDetection; -import edu.wpi.first.apriltag.AprilTagPoseEstimate; -import edu.wpi.first.apriltag.AprilTagPoseEstimator; -import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config; -import org.opencv.calib3d.Calib3d; -import org.opencv.core.MatOfPoint2f; -import org.opencv.core.Point; -import org.photonvision.vision.calibration.CameraCalibrationCoefficients; -import org.photonvision.vision.pipe.CVPipe; - -public class AprilTagPoseEstimatorPipe - extends CVPipe< - AprilTagDetection, - AprilTagPoseEstimate, - AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams> { - private final AprilTagPoseEstimator m_poseEstimator = - new AprilTagPoseEstimator(new AprilTagPoseEstimator.Config(0, 0, 0, 0, 0)); - - boolean useNativePoseEst; - - public AprilTagPoseEstimatorPipe() { - super(); - } - - MatOfPoint2f temp = new MatOfPoint2f(); - - @Override - protected AprilTagPoseEstimate process(AprilTagDetection in) { - // Save the corner points of our detection to an array - Point corners[] = new Point[4]; - for (int i = 0; i < 4; i++) { - corners[i] = new Point(in.getCornerX(i), in.getCornerY(i)); - } - // And shove into our matofpoints - temp.fromArray(corners); - - // Probably overwrites what was in temp before. I hope - Calib3d.undistortImagePoints( - temp, - temp, - params.calibration.getCameraIntrinsicsMat(), - params.calibration.getDistCoeffsMat()); - - // Save out undistorted corners - corners = temp.toArray(); - - // Apriltagdetection expects an array in form [x1 y1 x2 y2 ...] - var fixedCorners = new double[8]; - for (int i = 0; i < 4; i++) { - fixedCorners[i * 2] = corners[i].x; - fixedCorners[i * 2 + 1] = corners[i].y; - } - - // Create a new Detection with the fixed corners - var corrected = - new AprilTagDetection( - in.getFamily(), - in.getId(), - in.getHamming(), - in.getDecisionMargin(), - in.getHomography(), - in.getCenterX(), - in.getCenterY(), - fixedCorners); - - return m_poseEstimator.estimateOrthogonalIteration(corrected, params.nIters); - } - - @Override - public void setParams(AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams newParams) { - if (this.params == null || !this.params.equals(newParams)) { - m_poseEstimator.setConfig(newParams.config); - } - - super.setParams(newParams); - } - - public void setNativePoseEstimationEnabled(boolean enabled) { - this.useNativePoseEst = enabled; - } - - public static class AprilTagPoseEstimatorPipeParams { - final AprilTagPoseEstimator.Config config; - final CameraCalibrationCoefficients calibration; - final int nIters; - - public AprilTagPoseEstimatorPipeParams( - Config config, CameraCalibrationCoefficients cal, int nIters) { - this.config = config; - this.nIters = nIters; - this.calibration = cal; - } - - @Override - public int hashCode() { - final int prime = 31; - int result = 1; - result = prime * result + ((config == null) ? 0 : config.hashCode()); - result = prime * result + nIters; - return result; - } - - @Override - public boolean equals(Object obj) { - if (this == obj) return true; - if (obj == null) return false; - if (getClass() != obj.getClass()) return false; - AprilTagPoseEstimatorPipeParams other = (AprilTagPoseEstimatorPipeParams) obj; - if (config == null) { - if (other.config != null) return false; - } else if (!config.equals(other.config)) return false; - if (nIters != other.nIters) return false; - return true; - } - } -} +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision.vision.pipe.impl; + +import edu.wpi.first.apriltag.AprilTagDetection; +import edu.wpi.first.apriltag.AprilTagPoseEstimate; +import edu.wpi.first.apriltag.AprilTagPoseEstimator; +import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Point; +import org.photonvision.vision.calibration.CameraCalibrationCoefficients; +import org.photonvision.vision.pipe.CVPipe; + +public class AprilTagPoseEstimatorPipe + extends CVPipe< + AprilTagDetection, + AprilTagPoseEstimate, + AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams> { + private final AprilTagPoseEstimator m_poseEstimator = + new AprilTagPoseEstimator(new AprilTagPoseEstimator.Config(0, 0, 0, 0, 0)); + + boolean useNativePoseEst; + + public AprilTagPoseEstimatorPipe() { + super(); + } + + MatOfPoint2f temp = new MatOfPoint2f(); + + @Override + protected AprilTagPoseEstimate process(AprilTagDetection in) { + // Save the corner points of our detection to an array + Point corners[] = new Point[4]; + for (int i = 0; i < 4; i++) { + corners[i] = new Point(in.getCornerX(i), in.getCornerY(i)); + } + // And shove into our matofpoints + temp.fromArray(corners); + + // Probably overwrites what was in temp before. I hope + Calib3d.undistortImagePoints( + temp, + temp, + params.calibration.getCameraIntrinsicsMat(), + params.calibration.getDistCoeffsMat()); + + // Save out undistorted corners + corners = temp.toArray(); + + // Apriltagdetection expects an array in form [x1 y1 x2 y2 ...] + var fixedCorners = new double[8]; + for (int i = 0; i < 4; i++) { + fixedCorners[i * 2] = corners[i].x; + fixedCorners[i * 2 + 1] = corners[i].y; + } + + // Create a new Detection with the fixed corners + var corrected = + new AprilTagDetection( + in.getFamily(), + in.getId(), + in.getHamming(), + in.getDecisionMargin(), + in.getHomography(), + in.getCenterX(), + in.getCenterY(), + fixedCorners); + + return m_poseEstimator.estimateOrthogonalIteration(corrected, params.nIters); + } + + @Override + public void setParams(AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams newParams) { + if (this.params == null || !this.params.equals(newParams)) { + m_poseEstimator.setConfig(newParams.config); + } + + super.setParams(newParams); + } + + public void setNativePoseEstimationEnabled(boolean enabled) { + this.useNativePoseEst = enabled; + } + + public static class AprilTagPoseEstimatorPipeParams { + final AprilTagPoseEstimator.Config config; + final CameraCalibrationCoefficients calibration; + final int nIters; + + public AprilTagPoseEstimatorPipeParams( + Config config, CameraCalibrationCoefficients cal, int nIters) { + this.config = config; + this.nIters = nIters; + this.calibration = cal; + } + + @Override + public int hashCode() { + final int prime = 31; + int result = 1; + result = prime * result + ((config == null) ? 0 : config.hashCode()); + result = prime * result + nIters; + return result; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (obj == null) return false; + if (getClass() != obj.getClass()) return false; + AprilTagPoseEstimatorPipeParams other = (AprilTagPoseEstimatorPipeParams) obj; + if (config == null) { + if (other.config != null) return false; + } else if (!config.equals(other.config)) return false; + if (nIters != other.nIters) return false; + return true; + } + } +}