Skip to content

Commit

Permalink
format
Browse files Browse the repository at this point in the history
  • Loading branch information
amquake committed Aug 21, 2023
1 parent b8d6091 commit 0244feb
Show file tree
Hide file tree
Showing 4 changed files with 67 additions and 35 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,8 @@
import org.photonvision.vision.target.TrackedTarget;

/**
* Determines the target corners of the {@link TrackedTarget}. The {@link CornerDetectionPipeParameters}
* affect how these corners are calculated.
* Determines the target corners of the {@link TrackedTarget}. The {@link
* CornerDetectionPipeParameters} affect how these corners are calculated.
*/
public class CornerDetectionPipe
extends CVPipe<
Expand Down Expand Up @@ -98,7 +98,7 @@ private List<Point> findBoundingBoxCorners(TrackedTarget target) {
private static double distanceBetween(Point a, Point b) {
double xDelta = a.x - b.x;
double yDelta = a.y - b.y;
return Math.sqrt(xDelta*xDelta + yDelta*yDelta);
return Math.sqrt(xDelta * xDelta + yDelta * yDelta);
}

/**
Expand All @@ -111,8 +111,7 @@ private static double distanceBetween(Point a, Point b) {
private List<Point> detectExtremeCornersByApproxPolyDp(TrackedTarget target, boolean convexHull) {
var centroid = target.getMinAreaRect().center;
Comparator<Point> compareCenterDist =
Comparator.comparingDouble(
(Point point) -> distanceBetween(centroid, point));
Comparator.comparingDouble((Point point) -> distanceBetween(centroid, point));

MatOfPoint2f targetContour;
if (convexHull) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,25 +25,26 @@
import org.opencv.core.MatOfPoint3f;
import org.opencv.core.Point3;
import org.photonvision.vision.opencv.Releasable;
import org.photonvision.vision.pipe.impl.SolvePNPPipe;
import org.photonvision.vision.pipe.impl.CornerDetectionPipe;
import org.photonvision.vision.pipe.impl.SolvePNPPipe;

/**
* A model representing the vertices of targets with known shapes. The vertices are in the EDN
* coordinate system. When creating a TargetModel, the vertices must be supplied in a certain
* order to ensure correct correspondence with corners detected in 2D for use with SolvePNP.
* For planar targets, we expect the target's Z-axis to point towards the camera.
*
* <p>{@link SolvePNPPipe} expects 3d object points to correspond to the {@link CornerDetectionPipe} implementation.
* The 2d corner detection finds the 4 extreme corners (bottom-left, bottom-right, top-right, top-left).
* To match our expectations, this means the model vertices would look like:
* coordinate system. When creating a TargetModel, the vertices must be supplied in a certain order
* to ensure correct correspondence with corners detected in 2D for use with SolvePNP. For planar
* targets, we expect the target's Z-axis to point towards the camera.
*
* <p>{@link SolvePNPPipe} expects 3d object points to correspond to the {@link CornerDetectionPipe}
* implementation. The 2d corner detection finds the 4 extreme corners (bottom-left, bottom-right,
* top-right, top-left). To match our expectations, this means the model vertices would look like:
*
* <ul>
* <li>(+x, +y, 0)
* <li>(-x, +y, 0)
* <li>(-x, -y, 0)
* <li>(+x, -y, 0)
* </ul>
*
*
* <p>AprilTag models are currently only used for drawing on the output stream.
*/
public enum TargetModel implements Releasable {
Expand Down
Original file line number Diff line number Diff line change
@@ -1,15 +1,31 @@
/*
* 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 <https://www.gnu.org/licenses/>.
*/

package org.photonvision.common.util;

import static org.junit.jupiter.api.Assertions.assertEquals;

import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.photonvision.common.util.math.MathUtils;

import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.photonvision.common.util.math.MathUtils;

public class CoordinateConversionTest {
@BeforeAll
Expand All @@ -21,33 +37,43 @@ public static void Init() {
public void testAprilTagToOpenCV() {
// AprilTag and OpenCV both use the EDN coordinate system. AprilTag, however, assumes the tag's
// z-axis points away from the camera while we expect it to point towards the camera.
var apriltag = new Transform3d(
new Translation3d(1, 2, 3),
new Rotation3d(Math.toRadians(5), Math.toRadians(10), Math.toRadians(15)));
var apriltag =
new Transform3d(
new Translation3d(1, 2, 3),
new Rotation3d(Math.toRadians(5), Math.toRadians(10), Math.toRadians(15)));
var opencv = MathUtils.convertApriltagtoOpenCV(apriltag);
final var expectedTrl = new Translation3d(1, 2, 3);
assertEquals(expectedTrl, opencv.getTranslation(), "AprilTag to OpenCV translation conversion failed");
assertEquals(
expectedTrl, opencv.getTranslation(), "AprilTag to OpenCV translation conversion failed");
var apriltagXaxis = new Translation3d(1, 0, 0).rotateBy(apriltag.getRotation());
var apriltagYaxis = new Translation3d(0, 1, 0).rotateBy(apriltag.getRotation());
var apriltagZaxis = new Translation3d(0, 0, 1).rotateBy(apriltag.getRotation());
var opencvXaxis = new Translation3d(1, 0, 0).rotateBy(opencv.getRotation());
var opencvYaxis = new Translation3d(0, 1, 0).rotateBy(opencv.getRotation());
var opencvZaxis = new Translation3d(0, 0, 1).rotateBy(opencv.getRotation());
assertEquals(apriltagXaxis.unaryMinus(), opencvXaxis, "AprilTag to OpenCV rotation conversion failed(X-axis)");
assertEquals(apriltagYaxis, opencvYaxis, "AprilTag to OpenCV rotation conversion failed(Y-axis)");
assertEquals(apriltagZaxis.unaryMinus(), opencvZaxis, "AprilTag to OpenCV rotation conversion failed(Z-axis)");
assertEquals(
apriltagXaxis.unaryMinus(),
opencvXaxis,
"AprilTag to OpenCV rotation conversion failed(X-axis)");
assertEquals(
apriltagYaxis, opencvYaxis, "AprilTag to OpenCV rotation conversion failed(Y-axis)");
assertEquals(
apriltagZaxis.unaryMinus(),
opencvZaxis,
"AprilTag to OpenCV rotation conversion failed(Z-axis)");
}

@Test
public void testOpenCVToPhoton() {
// OpenCV uses the EDN coordinate system while wpilib is in NWU.
var opencv = new Transform3d(
new Translation3d(1, 2, 3),
new Rotation3d(VecBuilder.fill(1, 2, 3), Math.PI/8));
var opencv =
new Transform3d(
new Translation3d(1, 2, 3), new Rotation3d(VecBuilder.fill(1, 2, 3), Math.PI / 8));
var wpilib = MathUtils.convertOpenCVtoPhotonTransform(opencv);
final var expectedTrl = new Translation3d(3, -1, -2);
assertEquals(expectedTrl, wpilib.getTranslation(), "OpenCV to WPILib translation conversion failed");
var expectedRot = new Rotation3d(VecBuilder.fill(3, -1, -2), Math.PI/8);
assertEquals(
expectedTrl, wpilib.getTranslation(), "OpenCV to WPILib translation conversion failed");
var expectedRot = new Rotation3d(VecBuilder.fill(3, -1, -2), Math.PI / 8);
assertEquals(expectedRot, wpilib.getRotation(), "OpenCV to WPILib rotation conversion failed");
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,6 @@
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import java.util.stream.Collectors;
import org.junit.jupiter.api.Assertions;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
import org.photonvision.common.util.TestUtils;
Expand Down Expand Up @@ -131,7 +130,9 @@ public void test2019() {
var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
// these numbers are not *accurate*, but they are known and expected
var expectedTrl = new Translation3d(1.1, -0.05, -0.05);
assertTrue(expectedTrl.getDistance(pose.getTranslation()) < 0.05, "SolvePNP translation estimation failed");
assertTrue(
expectedTrl.getDistance(pose.getTranslation()) < 0.05,
"SolvePNP translation estimation failed");
// We expect the object axes to be in NWU, with the x-axis coming out of the tag
// This target is facing the camera almost parallel, so in world space:
// The object's X axis should be (-1, 0, 0)
Expand Down Expand Up @@ -183,14 +184,19 @@ public void test2020() {

var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
// these numbers are not *accurate*, but they are known and expected
var expectedTrl = new Translation3d(Units.inchesToMeters(236), Units.inchesToMeters(36), Units.inchesToMeters(-53));
assertTrue(expectedTrl.getDistance(pose.getTranslation()) < 0.05, "SolvePNP translation estimation failed");
var expectedTrl =
new Translation3d(
Units.inchesToMeters(236), Units.inchesToMeters(36), Units.inchesToMeters(-53));
assertTrue(
expectedTrl.getDistance(pose.getTranslation()) < 0.05,
"SolvePNP translation estimation failed");
// We expect the object axes to be in NWU, with the x-axis coming out of the tag
// Rotation around Z axis (yaw) should be mostly facing us
var xAxis = new Translation3d(1, 0, 0);
var yAxis = new Translation3d(0, 1, 0);
var zAxis = new Translation3d(0, 0, 1);
var expectedRot = new Rotation3d(Math.toRadians(-20), Math.toRadians(-20), Math.toRadians(-120));
var expectedRot =
new Rotation3d(Math.toRadians(-20), Math.toRadians(-20), Math.toRadians(-120));
assertTrue(xAxis.rotateBy(expectedRot).getDistance(xAxis.rotateBy(pose.getRotation())) < 0.1);
assertTrue(yAxis.rotateBy(expectedRot).getDistance(yAxis.rotateBy(pose.getRotation())) < 0.1);
assertTrue(zAxis.rotateBy(expectedRot).getDistance(zAxis.rotateBy(pose.getRotation())) < 0.1);
Expand Down

0 comments on commit 0244feb

Please sign in to comment.