Skip to content

Commit

Permalink
fix corner detection, add test
Browse files Browse the repository at this point in the history
  • Loading branch information
@amquake
amquake committed Oct 20, 2023
1 parent 9af4e02 commit 03cf5fc
Show file tree
Hide file tree
Showing 8 changed files with 115 additions and 81 deletions.
3 changes: 2 additions & 1 deletion photon-core/src/main/java/org/photonvision/common/configuration/ConfigManager.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,6 @@
import org.zeroturnaround.zip.ZipUtil;

public class ConfigManager {
private static final Logger logger = new Logger(ConfigManager.class, LogGroup.General);
private static ConfigManager INSTANCE;

public static final String HW_CFG_FNAME = "hardwareConfig.json";
Expand Down Expand Up @@ -79,6 +78,8 @@ public static ConfigManager getInstance() {
return INSTANCE;
}

private static final Logger logger = new Logger(ConfigManager.class, LogGroup.General);

private void translateLegacyIfPresent(Path folderPath) {
if (!(m_provider instanceof SqlConfigProvider)) {
// Cannot import into SQL if we aren't in SQL mode rn
Expand Down
26 changes: 13 additions & 13 deletions photon-core/src/main/java/org/photonvision/vision/aruco/PhotonArucoDetector.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ public class PhotonArucoDetector {
new ArucoDetector(Objdetect.getPredefinedDictionary(Objdetect.DICT_APRILTAG_16h5));

private Mat ids = new Mat();
private ArrayList<Mat> corners = new ArrayList<Mat>();
private ArrayList<Mat> cornerMats = new ArrayList<Mat>();
private Mat cornerMat;

public ArucoDetector getDetector() {
Expand Down Expand Up @@ -61,26 +61,26 @@ public void setParams(DetectorParameters params) {
*/
public ArucoDetectionResult[] detect(Mat grayscaleImg) {
// detect tags
detector.detectMarkers(grayscaleImg, corners, ids);
detector.detectMarkers(grayscaleImg, cornerMats, ids);

ArucoDetectionResult[] toReturn = new ArucoDetectionResult[corners.size()];
for (int i = 0; i < corners.size(); i++) {
// each tag has a cornerMat
cornerMat = corners.get(i);
ArucoDetectionResult[] toReturn = new ArucoDetectionResult[cornerMats.size()];
for (int i = 0; i < cornerMats.size(); i++) {
// each detection has a Mat of corners
cornerMat = cornerMats.get(i);

// Aruco detection returns corners (TL, TR, BR, BL).
// Aruco detection returns corners (BR, BL, TL, TR).
// For parity with AprilTags and photonlib, we want (BL, BR, TR, TL).
double[] xCorners = {
cornerMat.get(0, 3)[0],
cornerMat.get(0, 2)[0],
cornerMat.get(0, 1)[0],
cornerMat.get(0, 0)[0]
cornerMat.get(0, 0)[0],
cornerMat.get(0, 3)[0],
cornerMat.get(0, 2)[0]
};
double[] yCorners = {
cornerMat.get(0, 3)[1],
cornerMat.get(0, 2)[1],
cornerMat.get(0, 1)[1],
cornerMat.get(0, 0)[1]
cornerMat.get(0, 0)[1],
cornerMat.get(0, 3)[1],
cornerMat.get(0, 2)[1]
};
cornerMat.release();

Expand Down
2 changes: 1 addition & 1 deletion photon-core/src/main/java/org/photonvision/vision/pipe/impl/ArucoDetectionPipeParams.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@

/** Detector parameters. See https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html. */
public class ArucoDetectionPipeParams {
/** Tag family. Default: {@link Aruco#DICT_APRILTAG_16h5}. */
/** Tag family. Default: {@link Objdetect#DICT_APRILTAG_16h5}. */
public int tagFamily = Objdetect.DICT_APRILTAG_16h5;

public int threshMinSize = 11;
Expand Down
6 changes: 6 additions & 0 deletions photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,9 @@
import java.util.List;
import org.opencv.core.Mat;
import org.opencv.imgproc.Imgproc;
import org.opencv.objdetect.Objdetect;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.vision.apriltag.AprilTagFamily;
import org.photonvision.vision.aruco.ArucoDetectionResult;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameThresholdType;
Expand Down Expand Up @@ -72,6 +74,10 @@ protected void setPipeParamsImpl() {
var params = new ArucoDetectionPipeParams();
// sanitize and record settings

if (settings.tagFamily == AprilTagFamily.kTag36h11) {
params.tagFamily = Objdetect.DICT_APRILTAG_36h11;
}

int threshMinSize = Math.max(3, settings.threshWinSizes.getFirst());
settings.threshWinSizes.setFirst(threshMinSize);
params.threshMinSize = threshMinSize;
Expand Down
3 changes: 3 additions & 0 deletions photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipelineSettings.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,13 @@

import com.fasterxml.jackson.annotation.JsonTypeName;
import org.photonvision.common.util.numbers.IntegerCouple;
import org.photonvision.vision.apriltag.AprilTagFamily;
import org.photonvision.vision.target.TargetModel;

@JsonTypeName("ArucoPipelineSettings")
public class ArucoPipelineSettings extends AdvancedPipelineSettings {
public AprilTagFamily tagFamily = AprilTagFamily.kTag16h5;

public IntegerCouple threshWinSizes = new IntegerCouple(11, 91);
public int threshStepSize = 40;
public double threshConstant = 10;
Expand Down
46 changes: 15 additions & 31 deletions photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,22 +144,6 @@ public TrackedTarget(

// TODO implement skew? or just yeet
m_skew = 0;

var tvec = new Mat(3, 1, CvType.CV_64FC1);
tvec.put(
0,
0,
new double[] {
bestPose.getTranslation().getX(),
bestPose.getTranslation().getY(),
bestPose.getTranslation().getZ()
});
setCameraRelativeTvec(tvec);

// Opencv expects a 3d vector with norm = angle and direction = axis
var rvec = new Mat(3, 1, CvType.CV_64FC1);
MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec);
setCameraRelativeRvec(rvec);
}

public TrackedTarget(
Expand Down Expand Up @@ -215,22 +199,22 @@ public TrackedTarget(
m_altCameraToTarget3d = altPose;

m_poseAmbiguity = tagPose.getAmbiguity();
}

var tvec = new Mat(3, 1, CvType.CV_64FC1);
tvec.put(
0,
0,
new double[] {
bestPose.getTranslation().getX(),
bestPose.getTranslation().getY(),
bestPose.getTranslation().getZ()
});
setCameraRelativeTvec(tvec);

var rvec = new Mat(3, 1, CvType.CV_64FC1);
MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec);
setCameraRelativeRvec(rvec);
var tvec = new Mat(3, 1, CvType.CV_64FC1);
tvec.put(
0,
0,
new double[] {
bestPose.getTranslation().getX(),
bestPose.getTranslation().getY(),
bestPose.getTranslation().getZ()
});
setCameraRelativeTvec(tvec);

var rvec = new Mat(3, 1, CvType.CV_64FC1);
MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec);
setCameraRelativeRvec(rvec);
}
}

public void setFiducialId(int id) {
Expand Down
39 changes: 23 additions & 16 deletions photon-core/src/test/java/org/photonvision/vision/pipeline/AprilTagTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
import org.junit.jupiter.api.Assertions;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.util.TestUtils;
import org.photonvision.vision.apriltag.AprilTagFamily;
import org.photonvision.vision.camera.QuirkyCamera;
Expand All @@ -32,8 +33,9 @@

public class AprilTagTest {
@BeforeEach
public void Init() {
public void setup() {
TestUtils.loadLibraries();
ConfigManager.getInstance().load();
}

@Test
Expand All @@ -56,13 +58,8 @@ public void testApriltagFacingCamera() {
frameProvider.requestFrameThresholdType(pipeline.getThresholdType());

CVPipelineResult pipelineResult;
try {
pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
printTestResults(pipelineResult);
} catch (RuntimeException e) {
// For now, will throw because of the Rotation3d ctor
return;
}
pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
printTestResults(pipelineResult);

// Draw on input
var outputPipe = new OutputStreamPipeline();
Expand All @@ -73,11 +70,26 @@ public void testApriltagFacingCamera() {
TestUtils.showImage(ret.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);

// these numbers are not *accurate*, but they are known and expected
var pose = pipelineResult.targets.get(0).getBestCameraToTarget3d();
var target = pipelineResult.targets.get(0);

// Test corner order
var corners = target.getTargetCorners();
Assertions.assertEquals(260, corners.get(0).x, 10);
Assertions.assertEquals(245, corners.get(0).y, 10);
Assertions.assertEquals(315, corners.get(1).x, 10);
Assertions.assertEquals(245, corners.get(1).y, 10);
Assertions.assertEquals(315, corners.get(2).x, 10);
Assertions.assertEquals(190, corners.get(2).y, 10);
Assertions.assertEquals(260, corners.get(3).x, 10);
Assertions.assertEquals(190, corners.get(3).y, 10);

var pose = target.getBestCameraToTarget3d();
// Test pose estimate translation
Assertions.assertEquals(2, pose.getTranslation().getX(), 0.2);
Assertions.assertEquals(0.1, pose.getTranslation().getY(), 0.2);
Assertions.assertEquals(0.0, pose.getTranslation().getZ(), 0.2);

// Test pose estimate rotation
// We expect the object axes to be in NWU, with the x-axis coming out of the tag
// This visible tag is facing the camera almost parallel, so in world space:

Expand Down Expand Up @@ -111,13 +123,8 @@ public void testApriltagDistorted() {
frameProvider.requestFrameThresholdType(pipeline.getThresholdType());

CVPipelineResult pipelineResult;
try {
pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
printTestResults(pipelineResult);
} catch (RuntimeException e) {
// For now, will throw because of the Rotation3d ctor
return;
}
pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
printTestResults(pipelineResult);

// Draw on input
var outputPipe = new OutputStreamPipeline();
Expand Down
71 changes: 52 additions & 19 deletions photon-core/src/test/java/org/photonvision/vision/pipeline/ArucoPipelineTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,25 +17,31 @@

package org.photonvision.vision.pipeline;

import java.io.IOException;
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.configuration.ConfigManager;
import org.photonvision.common.util.TestUtils;
import org.photonvision.vision.apriltag.AprilTagFamily;
import org.photonvision.vision.camera.QuirkyCamera;
import org.photonvision.vision.frame.provider.FileFrameProvider;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.target.TargetModel;
import org.photonvision.vision.target.TrackedTarget;

import edu.wpi.first.math.geometry.Translation3d;

public class ArucoPipelineTest {
@BeforeEach
public void Init() throws IOException {
public void setup() {
TestUtils.loadLibraries();
ConfigManager.getInstance().load();
}

@Test
public void testApriltagFacingCameraAruco() {
public void testApriltagFacingCamera() {
var pipeline = new ArucoPipeline();

pipeline.getSettings().inputShouldShow = true;
Expand All @@ -44,32 +50,59 @@ public void testApriltagFacingCameraAruco() {
pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
pipeline.getSettings().cornerDetectionUseConvexHulls = true;
pipeline.getSettings().targetModel = TargetModel.k200mmAprilTag;

// pipeline.getSettings().tagFamily = AprilTagFamily.kTag36h11;
pipeline.getSettings().tagFamily = AprilTagFamily.kTag36h11;

var frameProvider =
new FileFrameProvider(
TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag1_16h5_1280, false),
106,
TestUtils.getCoeffs("laptop_1280.json", false));
TestUtils.getApriltagImagePath(TestUtils.ApriltagTestImages.kTag1_640_480, false),
TestUtils.WPI2020Image.FOV,
TestUtils.get2020LifeCamCoeffs(false));
frameProvider.requestFrameThresholdType(pipeline.getThresholdType());

CVPipelineResult pipelineResult;
try {
pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
printTestResults(pipelineResult);
} catch (RuntimeException e) {
// For now, will throw because of the Rotation3d ctor
return;
}
pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
printTestResults(pipelineResult);

// Draw on input
var outputPipe = new OutputStreamPipeline();
outputPipe.process(
pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);
var ret =
outputPipe.process(
pipelineResult.inputAndOutputFrame, pipeline.getSettings(), pipelineResult.targets);

TestUtils.showImage(ret.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);

// these numbers are not *accurate*, but they are known and expected
var target = pipelineResult.targets.get(0);

// Test corner order
var corners = target.getTargetCorners();
Assertions.assertEquals(260, corners.get(0).x, 10);
Assertions.assertEquals(245, corners.get(0).y, 10);
Assertions.assertEquals(315, corners.get(1).x, 10);
Assertions.assertEquals(245, corners.get(1).y, 10);
Assertions.assertEquals(315, corners.get(2).x, 10);
Assertions.assertEquals(190, corners.get(2).y, 10);
Assertions.assertEquals(260, corners.get(3).x, 10);
Assertions.assertEquals(190, corners.get(3).y, 10);

var pose = target.getBestCameraToTarget3d();
// Test pose estimate translation
Assertions.assertEquals(2, pose.getTranslation().getX(), 0.2);
Assertions.assertEquals(0.1, pose.getTranslation().getY(), 0.2);
Assertions.assertEquals(0.0, pose.getTranslation().getZ(), 0.2);

// Test pose estimate rotation
// We expect the object axes to be in NWU, with the x-axis coming out of the tag
// This visible tag is facing the camera almost parallel, so in world space:

TestUtils.showImage(
pipelineResult.inputAndOutputFrame.processedImage.getMat(), "Pipeline output", 999999);
// The object's X axis should be (-1, 0, 0)
Assertions.assertEquals(
-1, new Translation3d(1, 0, 0).rotateBy(pose.getRotation()).getX(), 0.1);
// The object's Y axis should be (0, -1, 0)
Assertions.assertEquals(
-1, new Translation3d(0, 1, 0).rotateBy(pose.getRotation()).getY(), 0.1);
// The object's Z axis should be (0, 0, 1)
Assertions.assertEquals(1, new Translation3d(0, 0, 1).rotateBy(pose.getRotation()).getZ(), 0.1);
}

private static void printTestResults(CVPipelineResult pipelineResult) {
Expand Down

0 comments on commit 03cf5fc

Please sign in to comment.