Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-05 03:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix AprilTag rotation reversal bug (#482)

Browse Source 
Applies base rotation to apriltags to match solvepnp base rotation
This commit is contained in:
Matt
2022-10-08 09:27:27 -04:00
committed by GitHub
parent 2c6b0ddac3
commit 4ad9d97508
13 changed files with 226 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
photon-core/src/main/java/org/photonvision/common/util/TestUtils.java
View File

@@ -33,7 +33,6 @@ public class TestUtils {
try {
CameraServerCvJNI.forceLoad();
// PicamJNI.forceLoad();
// AprilTagJNI.forceLoad();
} catch (IOException ex) {
// ignored
}
@@ -165,7 +164,8 @@ public class TestUtils {
}
public enum ApriltagTestImages {
kRobots;
kRobots,
kTag1_640_480;
public final Path path;
@@ -233,6 +233,10 @@ public class TestUtils {
return getTestImagesPath(testMode).resolve(image.path);
}
public static Path getApriltagImagePath(ApriltagTestImages image, boolean testMode) {
return getTestImagesPath(testMode).resolve(image.path);
}
public static Path getPowercellImagePath(PowercellTestImages image, boolean testMode) {
return getPowercellPath(testMode).resolve(image.path);
}

43
photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
View File

@@ -19,14 +19,17 @@ package org.photonvision.common.util.math;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.CoordinateSystem;
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.util.Units;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.Arrays;
import java.util.List;
import org.opencv.core.Mat;
public class MathUtils {
MathUtils() {}
@@ -159,25 +162,33 @@ public class MathUtils {
// TODO: Refactor into new pipe?
public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
// CameraToTarget _should_ be in opencv-land EDN
var pose =
CoordinateSystem.convert(
new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
return pose;
return CoordinateSystem.convert(
new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
}
public static Pose3d convertApriltagtoPhotonPose(Transform3d cameraToTarget3d) {
// CameraToTarget _should_ be in opencv-land EDN
var pose =
CoordinateSystem.convert(
new Pose3d(cameraToTarget3d), CoordinateSystem.EDN(), CoordinateSystem.NWU());
/*
* The AprilTag pose rotation outputs are X left, Y down, Z away from the tag with the tag facing
* the camera upright and the camera facing the target parallel to the floor. But our OpenCV
* solvePNP code would have X left, Y up, Z towards the camera with the target facing the camera
* and both parallel to the floor. So we apply a base rotation to the rotation component of the
* apriltag pose to make it consistent with the EDN system that OpenCV uses, internally a 180
* rotation about the X axis
*/
private static final Rotation3d APRILTAG_BASE_ROTATION =
new Rotation3d(VecBuilder.fill(1, 0, 0), Units.degreesToRadians(180));
// Apply an extra rotation so that at zero pose, X ls left, Y is up, and Z is towards the camera
// to a camera facing along the +X axis of the field parallel with the ground plane
// So we need a 180 flip about X axis
var newRotation = pose.getRotation().rotateBy(new Rotation3d(0, Math.PI, 0));
/**
* Apply a 180 degree rotation about X to the rotation component of a given Apriltag pose. This
* aligns it with the OpenCV poses we use in other places.
*/
public static Transform3d convertApriltagtoOpenCV(Transform3d pose) {
var ocvRotation = APRILTAG_BASE_ROTATION.rotateBy(pose.getRotation());
return new Transform3d(pose.getTranslation(), ocvRotation);
}
return new Pose3d(pose.getTranslation(), newRotation);
public static void rotationToOpencvRvec(Rotation3d rotation, Mat rvecOutput) {
var angle = rotation.getAngle();
var axis = rotation.getAxis().times(angle);
rvecOutput.put(0, 0, axis.getData());
}
}

14
photon-core/src/main/java/org/photonvision/vision/apriltag/DetectionResult.java
View File

@@ -19,8 +19,8 @@ package org.photonvision.vision.apriltag;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.util.Arrays;
@@ -81,11 +81,11 @@ public class DetectionResult {
return error2;
}
public Pose3d getPoseResult1() {
public Transform3d getPoseResult1() {
return poseResult1;
}
public Pose3d getPoseResult2() {
public Transform3d getPoseResult2() {
return poseResult2;
}
@@ -96,9 +96,9 @@ public class DetectionResult {
double centerX, centerY;
double[] corners;
Pose3d poseResult1;
Transform3d poseResult1;
double error1;
Pose3d poseResult2;
Transform3d poseResult2;
double error2;
public DetectionResult(
@@ -125,12 +125,12 @@ public class DetectionResult {
this.error1 = err1;
this.poseResult1 =
new Pose3d(
new Transform3d(
new Translation3d(pose1TransArr[0], pose1TransArr[1], pose1TransArr[2]),
new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr)));
this.error2 = err2;
this.poseResult2 =
new Pose3d(
new Transform3d(
new Translation3d(pose2TransArr[0], pose2TransArr[1], pose2TransArr[2]),
new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr)));
}

45
photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java
View File

@@ -25,7 +25,9 @@ import org.opencv.calib3d.Calib3d;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.MatOfPoint3f;
import org.opencv.core.Point;
import org.opencv.core.Point3;
import org.opencv.imgproc.Imgproc;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
@@ -132,8 +134,51 @@ public class Draw3dTargetsPipe
ColorHelper.colorToScalar(Color.orange),
3);
}
// Draw X, Y and Z axis
MatOfPoint3f pointMat = new MatOfPoint3f();
var list =
List.of(
new Point3(0, 0, 0),
new Point3(0.2, 0, 0),
new Point3(0, 0.2, 0),
new Point3(0, 0, 0.2));
pointMat.fromList(list);
Calib3d.projectPoints(
pointMat,
target.getCameraRelativeRvec(),
target.getCameraRelativeTvec(),
params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(),
params.cameraCalibrationCoefficients.getCameraExtrinsicsMat(),
tempMat,
jac);
var axisPoints = tempMat.toList();
dividePointList(axisPoints);
// Red = x, green y, blue z
Imgproc.line(
in.getLeft(),
axisPoints.get(0),
axisPoints.get(1),
ColorHelper.colorToScalar(Color.RED),
3);
Imgproc.line(
in.getLeft(),
axisPoints.get(0),
axisPoints.get(2),
ColorHelper.colorToScalar(Color.GREEN),
3);
Imgproc.line(
in.getLeft(),
axisPoints.get(0),
axisPoints.get(3),
ColorHelper.colorToScalar(Color.BLUE),
3);
tempMat.release();
jac.release();
pointMat.release();
}
// draw corners

2
photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
View File

@@ -140,7 +140,7 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
new TargetCalculationParameters(
false, null, null, null, null, frameStaticProperties));
var correctedPose = MathUtils.convertApriltagtoPhotonPose(target.getCameraToTarget3d());
var correctedPose = MathUtils.convertOpenCVtoPhotonPose(target.getCameraToTarget3d());
target.setCameraToTarget3d(
new Transform3d(correctedPose.getTranslation(), correctedPose.getRotation()));

7
photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java
View File

@@ -181,16 +181,23 @@ public class PipelineManager {
var desiredPipelineSettings = userPipelineSettings.get(currentPipelineIndex);
switch (desiredPipelineSettings.pipelineType) {
case Reflective:
logger.debug("Creatig Reflective pipeline");
currentUserPipeline =
new ReflectivePipeline((ReflectivePipelineSettings) desiredPipelineSettings);
break;
case ColoredShape:
logger.debug("Creatig ColoredShape pipeline");
currentUserPipeline =
new ColoredShapePipeline((ColoredShapePipelineSettings) desiredPipelineSettings);
break;
case AprilTag:
logger.debug("Creatig AprilTag pipeline");
currentUserPipeline =
new AprilTagPipeline((AprilTagPipelineSettings) desiredPipelineSettings);
break;
default:
// Can be calib3d or drivermode, both of which are special cases
break;
}
}
}

2
photon-core/src/main/java/org/photonvision/vision/target/TargetModel.java
View File

@@ -108,7 +108,7 @@ public enum TargetModel implements Releasable {
new Point3(Units.inchesToMeters(3.25), Units.inchesToMeters(3.25), 0),
new Point3(Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0),
new Point3(-Units.inchesToMeters(3.25), -Units.inchesToMeters(3.25), 0)),
-Units.inchesToMeters(3.25 * 2));
Units.inchesToMeters(3.25 * 2));
@JsonIgnore private MatOfPoint3f realWorldTargetCoordinates;
@JsonIgnore private MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f();

12
photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View File

@@ -16,7 +16,6 @@
*/
package org.photonvision.vision.target;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Transform3d;
import java.util.HashMap;
import java.util.List;
@@ -26,6 +25,7 @@ import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.RotatedRect;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.vision.apriltag.DetectionResult;
import org.photonvision.vision.frame.FrameStaticProperties;
import org.photonvision.vision.opencv.*;
@@ -73,14 +73,16 @@ public class TrackedTarget implements Releasable {
m_yaw =
TargetCalculations.calculateYaw(
result.getCenterX(), params.cameraCenterPoint.x, params.horizontalFocalLength);
Pose3d bestPose = new Pose3d();
var bestPose = new Transform3d();
if (result.getError1() <= result.getError2()) {
bestPose = result.getPoseResult1();
} else {
bestPose = result.getPoseResult2();
}
m_cameraToTarget3d = new Transform3d(new Pose3d(), bestPose);
bestPose = MathUtils.convertApriltagtoOpenCV(bestPose);
m_cameraToTarget3d = bestPose;
double[] corners = result.getCorners();
Point[] cornerPoints =
@@ -114,9 +116,7 @@ public class TrackedTarget implements Releasable {
// Opencv expects a 3d vector with norm = angle and direction = axis
var rvec = new Mat(3, 1, CvType.CV_64FC1);
var angle = bestPose.getRotation().getAngle();
var axis = bestPose.getRotation().getAxis().times(angle);
rvec.put(0, 0, axis.getData());
MathUtils.rotationToOpencvRvec(bestPose.getRotation(), rvec);
setCameraRelativeRvec(rvec);
m_poseAmbiguity = result.getPoseAmbiguity();

13
photon-core/src/test/java/org/photonvision/common/configuration/ConfigTest.java
View File

@@ -30,6 +30,7 @@ import org.photonvision.common.logging.LogLevel;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.TestUtils;
import org.photonvision.common.util.file.JacksonUtils;
import org.photonvision.vision.pipeline.AprilTagPipelineSettings;
import org.photonvision.vision.pipeline.ColoredShapePipelineSettings;
import org.photonvision.vision.pipeline.ReflectivePipelineSettings;
import org.photonvision.vision.target.TargetModel;
@@ -40,6 +41,7 @@ public class ConfigTest {
new CameraConfiguration("TestCamera", "/dev/video420");
private static ReflectivePipelineSettings REFLECTIVE_PIPELINE_SETTINGS;
private static ColoredShapePipelineSettings COLORED_SHAPE_PIPELINE_SETTINGS;
private static AprilTagPipelineSettings APRIL_TAG_PIPELINE_SETTINGS;
@BeforeAll
public static void init() {
@@ -51,6 +53,7 @@ public class ConfigTest {
REFLECTIVE_PIPELINE_SETTINGS = new ReflectivePipelineSettings();
COLORED_SHAPE_PIPELINE_SETTINGS = new ColoredShapePipelineSettings();
APRIL_TAG_PIPELINE_SETTINGS = new AprilTagPipelineSettings();
REFLECTIVE_PIPELINE_SETTINGS.pipelineNickname = "2019Tape";
REFLECTIVE_PIPELINE_SETTINGS.targetModel = TargetModel.k2019DualTarget;
@@ -58,8 +61,12 @@ public class ConfigTest {
COLORED_SHAPE_PIPELINE_SETTINGS.pipelineNickname = "2019Cargo";
COLORED_SHAPE_PIPELINE_SETTINGS.pipelineIndex = 1;
APRIL_TAG_PIPELINE_SETTINGS.pipelineNickname = "apriltag";
APRIL_TAG_PIPELINE_SETTINGS.pipelineIndex = 2;
cameraConfig.addPipelineSetting(REFLECTIVE_PIPELINE_SETTINGS);
cameraConfig.addPipelineSetting(COLORED_SHAPE_PIPELINE_SETTINGS);
cameraConfig.addPipelineSetting(APRIL_TAG_PIPELINE_SETTINGS);
}
@Test
@@ -90,9 +97,12 @@ public class ConfigTest {
configMgr.getConfig().getCameraConfigurations().get("TestCamera").pipelineSettings.get(0);
var coloredShapePipelineSettings =
configMgr.getConfig().getCameraConfigurations().get("TestCamera").pipelineSettings.get(1);
var apriltagPipelineSettings =
configMgr.getConfig().getCameraConfigurations().get("TestCamera").pipelineSettings.get(2);
Assertions.assertEquals(REFLECTIVE_PIPELINE_SETTINGS, reflectivePipelineSettings);
Assertions.assertEquals(COLORED_SHAPE_PIPELINE_SETTINGS, coloredShapePipelineSettings);
Assertions.assertEquals(APRIL_TAG_PIPELINE_SETTINGS, apriltagPipelineSettings);
Assertions.assertTrue(
reflectivePipelineSettings instanceof ReflectivePipelineSettings,
@@ -100,6 +110,9 @@ public class ConfigTest {
Assertions.assertTrue(
coloredShapePipelineSettings instanceof ColoredShapePipelineSettings,
"Conig loaded pipeline settings for index 1 not of expected type ColoredShapePipelineSettings!");
Assertions.assertTrue(
apriltagPipelineSettings instanceof AprilTagPipelineSettings,
"Conig loaded pipeline settings for index 2 not of expected type AprilTagPipelineSettings!");
}
@AfterAll

8
photon-core/src/test/java/org/photonvision/vision/pipeline/SolvePNPTest.java
View File

@@ -150,6 +150,14 @@ public class SolvePNPTest {
CVPipelineResult pipelineResult = pipeline.run(frameProvider.get(), QuirkyCamera.DefaultCamera);
printTestResults(pipelineResult);
// Draw on input
var outputPipe = new OutputStreamPipeline();
outputPipe.process(
pipelineResult.inputFrame,
pipelineResult.outputFrame,
pipeline.getSettings(),
pipelineResult.targets);
// these numbers are not *accurate*, but they are known and expected
var pose = pipelineResult.targets.get(0).getCameraToTarget3d();
Assertions.assertEquals(Units.inchesToMeters(240.26), pose.getTranslation().getX(), 0.05);