Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[photon-core] [2024] Cleanup and document coordinate system conversion #894

Merged
merged 8 commits into from
Oct 15, 2023
Merged

[photon-core] [2024] Cleanup and document coordinate system conversion #894

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
157c54d
cleanup and document coordinate conversion
amquake Aug 12, 2023
6f876f5
spotless
amquake Aug 12, 2023
d1a09aa
bump wpilib version
amquake Aug 21, 2023
25e061e
coordinate conversion tests
amquake Aug 21, 2023
b8d6091
fix/document SolvePNPPipe models and corners
amquake Aug 21, 2023
0244feb
format
amquake Aug 21, 2023
4cbe18c
Merge remote-tracking branch 'upstream' into coordinate-conversion-cl…
mcm001 Oct 15, 2023
82d7b1d
run lint
mcm001 Oct 15, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
93 changes: 34 additions & 59 deletions photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,10 +17,12 @@

package org.photonvision.common.util.math;

import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.apriltag.AprilTagPoseEstimate;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.geometry.CoordinateSystem;
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.util.Units;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.Arrays;
Expand Down Expand Up @@ -137,73 +139,46 @@ public static double lerp(double startValue, double endValue, double t) {
return startValue + (endValue - startValue) * t;
}

public static Pose3d EDNtoNWU(final Pose3d pose) {
// Change of basis matrix from EDN to NWU. Each column vector is one of the
// old basis vectors mapped to its representation in the new basis.
//
// E (+X) -> N (-Y), D (+Y) -> W (-Z), N (+Z) -> U (+X)
var R = new MatBuilder<>(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0);

// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
double w = Math.sqrt(1.0 + R.get(0, 0) + R.get(1, 1) + R.get(2, 2)) / 2.0;
double x = (R.get(2, 1) - R.get(1, 2)) / (4.0 * w);
double y = (R.get(0, 2) - R.get(2, 0)) / (4.0 * w);
double z = (R.get(1, 0) - R.get(0, 1)) / (4.0 * w);
var rotationQuat = new Rotation3d(new Quaternion(w, x, y, z));

return new Pose3d(
pose.getTranslation().rotateBy(rotationQuat), pose.getRotation().rotateBy(rotationQuat));
}

/**
* All our solvepnp code returns a tag with X left, Y up, and Z out of the tag To better match
* wpilib, we want to apply another rotation so that we get Z up, X out of the tag, and Y to the
* right. We apply the following change of basis: X -> Y Y -> Z Z -> X
* OpenCV uses the EDN coordinate system, but WPILib uses NWU. Converts a camera-to-target
* transformation from EDN to NWU.
*
* <p>Note: The detected target's rvec and tvec perform a rotation-translation transformation
* which converts points in the target's coordinate system to the camera's. This means applying
* the transformation to the target point (0,0,0) for example would give the target's center
* relative to the camera. Conveniently, if we make a translation-rotation transformation out of
* these components instead, we get the transformation from the camera to the target.
*
* @param cameraToTarget3d A camera-to-target Transform3d in EDN.
* @return A camera-to-target Transform3d in NWU.
*/
private static final Rotation3d WPILIB_BASE_ROTATION =
new Rotation3d(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(0, 1, 0, 0, 0, 1, 1, 0, 0));

public static Transform3d convertOpenCVtoPhotonTransform(Transform3d cameraToTarget3d) {
// TODO: Refactor into new pipe?
// CameraToTarget _should_ be in opencv-land EDN
var nwu =
CoordinateSystem.convert(
new Pose3d().transformBy(cameraToTarget3d),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
return new Transform3d(nwu.getTranslation(), WPILIB_BASE_ROTATION.rotateBy(nwu.getRotation()));
}

public static Pose3d convertOpenCVtoPhotonPose(Transform3d cameraToTarget3d) {
// TODO: Refactor into new pipe?
// CameraToTarget _should_ be in opencv-land EDN
var nwu =
CoordinateSystem.convert(
new Pose3d().transformBy(cameraToTarget3d),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
return new Pose3d(nwu.getTranslation(), WPILIB_BASE_ROTATION.rotateBy(nwu.getRotation()));
return CoordinateSystem.convert(
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
* From the AprilTag repo:
* "The coordinate system has the origin at the camera center. The z-axis points from the camera
* center out the camera lens. The x-axis is to the right in the image taken by the camera, and
* y is down. The tag's coordinate frame is centered at the center of the tag, with x-axis to the
* right, y-axis down, and z-axis into the tag."
*
* This means our detected transformation will be in EDN. Our subsequent uses of the transformation,
* however, assume the tag's z-axis point away from the tag instead of into it. This means we
* have to correct the transformation's rotation.
*/
private static final Rotation3d APRILTAG_BASE_ROTATION =
new Rotation3d(VecBuilder.fill(1, 0, 0), Units.degreesToRadians(180));
new Rotation3d(VecBuilder.fill(0, 1, 0), Units.degreesToRadians(180));

/**
* 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.
* AprilTag returns a camera-to-tag transform in EDN, but the tag's z-axis points into the tag
* instead of away from it and towards the camera. This means we have to correct the
* transformation's rotation.
*
* @param pose The Transform3d with translation and rotation directly from the {@link
* AprilTagPoseEstimate}.
*/
public static Transform3d convertApriltagtoOpenCV(Transform3d pose) {
var ocvRotation = APRILTAG_BASE_ROTATION.rotateBy(pose.getRotation());
Expand Down
60 changes: 27 additions & 33 deletions photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,10 @@
import org.photonvision.vision.pipe.CVPipe;
import org.photonvision.vision.target.TrackedTarget;

/**
* Determines the target corners of the {@link TrackedTarget}. The {@link
* CornerDetectionPipeParameters} affect how these corners are calculated.
*/
public class CornerDetectionPipe
extends CVPipe<
List<TrackedTarget>,
Expand All @@ -49,15 +53,15 @@ protected List<TrackedTarget> process(List<TrackedTarget> targetList) {
}

/**
* @param target the target to find the corners of.
* @return the corners. left top, left bottom, right bottom, right top
* @param target The target to find the corners of.
* @return Corners: (bottom-left, bottom-right, top-right, top-left)
*/
private List<Point> findBoundingBoxCorners(TrackedTarget target) {
// extract the corners
var points = new Point[4];
target.m_mainContour.getMinAreaRect().points(points);

// find the tl/tr/bl/br corners
// find the bl/br/tr/tl corners
// first, min by left/right
var list_ = Arrays.asList(points);
list_.sort(leftRightComparator);
Expand All @@ -68,13 +72,12 @@ private List<Point> findBoundingBoxCorners(TrackedTarget target) {
var right = new ArrayList<>(List.of(list_.get(2), list_.get(3)));
right.sort(verticalComparator);

// tl tr bl br
var tl = left.get(0);
var bl = left.get(1);
var tr = right.get(0);
var br = right.get(1);

return List.of(tl, bl, br, tr);
return List.of(bl, br, tr, tl);
}

/**
Expand All @@ -83,31 +86,22 @@ private List<Point> findBoundingBoxCorners(TrackedTarget target) {
* @return The straight line distance between them.
*/
private static double distanceBetween(Point a, Point b) {
return Math.sqrt(Math.pow(a.x - b.x, 2) + Math.pow(a.y - b.y, 2));
}

/**
* @param a First point.
* @param b Second point.
* @return The straight line distance between them.
*/
private static double distanceBetween(Translation2d a, Translation2d b) {
return Math.sqrt(Math.pow(a.getX() - b.getX(), 2) + Math.pow(a.getY() - b.getY(), 2));
double xDelta = a.x - b.x;
double yDelta = a.y - b.y;
return Math.sqrt(xDelta * xDelta + yDelta * yDelta);
}

/**
* Find the 4 most extreme corners,
* Find the 4 most extreme corners of the target's contour.
*
* @param target the target to track.
* @param convexHull weather to use the convex hull of the target.
* @return the 4 extreme corners of the contour.
* @param target The target to track.
* @param convexHull Whether to use the convex hull of the contour instead.
* @return The 4 extreme corners of the contour: (bottom-left, bottom-right, top-right, top-left)
*/
private List<Point> detectExtremeCornersByApproxPolyDp(TrackedTarget target, boolean convexHull) {
var centroid = target.getMinAreaRect().center;
Comparator<Point> distanceProvider =
Comparator.comparingDouble(
(Point point) ->
Math.sqrt(Math.pow(centroid.x - point.x, 2) + Math.pow(centroid.y - point.y, 2)));
Comparator<Point> compareCenterDist =
Comparator.comparingDouble((Point point) -> distanceBetween(centroid, point));

MatOfPoint2f targetContour;
if (convexHull) {
Expand Down Expand Up @@ -141,17 +135,17 @@ private List<Point> detectExtremeCornersByApproxPolyDp(TrackedTarget target, boo
// left top, left bottom, right bottom, right top
var boundingBoxCorners = findBoundingBoxCorners(target);

var distanceToTlComparator =
Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)));

var distanceToTrComparator =
var compareDistToTl =
Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)));

// top left and top right are the poly corners closest to the bounding box tl and tr
pointList.sort(distanceToTlComparator);
var compareDistToTr =
Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(2)));

// top left and top right are the poly corners closest to the bouding box tl and tr
pointList.sort(compareDistToTl);
var tl = pointList.get(0);
pointList.remove(tl);
pointList.sort(distanceToTrComparator);
pointList.sort(compareDistToTr);
var tr = pointList.get(0);
pointList.remove(tr);

Expand All @@ -177,11 +171,11 @@ private List<Point> detectExtremeCornersByApproxPolyDp(TrackedTarget target, boo
}
}
if (leftList.isEmpty() || rightList.isEmpty()) return null;
leftList.sort(distanceProvider);
rightList.sort(distanceProvider);
leftList.sort(compareCenterDist);
rightList.sort(compareCenterDist);
var bl = leftList.get(leftList.size() - 1);
var br = rightList.get(rightList.size() - 1);
return List.of(tl, bl, br, tr);
return List.of(bl, br, tr, tl);
}

public static class CornerDetectionPipeParameters {
Expand Down
31 changes: 14 additions & 17 deletions photon-core/src/main/java/org/photonvision/vision/pipe/impl/Draw3dTargetsPipe.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -127,7 +127,7 @@ protected Void process(Pair<Mat, List<TrackedTarget>> in) {

// Draw X, Y and Z axis
MatOfPoint3f pointMat = new MatOfPoint3f();
// Those points are in opencv-land, but we are in NWU
// OpenCV expects coordinates in EDN, but we want to visualize in NWU
// NWU | EDN
// X: Z
// Y: -X
Expand All @@ -136,11 +136,15 @@ protected Void process(Pair<Mat, List<TrackedTarget>> in) {
var list =
List.of(
new Point3(0, 0, 0),
new Point3(0, 0, AXIS_LEN),
new Point3(AXIS_LEN, 0, 0),
new Point3(0, AXIS_LEN, 0));
new Point3(0, 0, AXIS_LEN), // x-axis
new Point3(-AXIS_LEN, 0, 0), // y-axis
new Point3(0, -AXIS_LEN, 0)); // z-axis
pointMat.fromList(list);

// The detected target's rvec and tvec perform a rotation-translation transformation which
// converts points in the target's coordinate system to the camera's. This means applying
// the transformation to the target point (0,0,0) for example would give the target's center
// relative to the camera.
Calib3d.projectPoints(
pointMat,
target.getCameraRelativeRvec(),
Expand All @@ -152,26 +156,30 @@ protected Void process(Pair<Mat, List<TrackedTarget>> in) {
var axisPoints = tempMat.toList();
dividePointList(axisPoints);

// Red = x, green y, blue z
// XYZ is RGB
// y-axis = green
Imgproc.line(
in.getLeft(),
axisPoints.get(0),
axisPoints.get(2),
ColorHelper.colorToScalar(Color.GREEN),
3);
// z-axis = blue
Imgproc.line(
in.getLeft(),
axisPoints.get(0),
axisPoints.get(3),
ColorHelper.colorToScalar(Color.BLUE),
3);
// x-axis = red
Imgproc.line(
in.getLeft(),
axisPoints.get(0),
axisPoints.get(1),
ColorHelper.colorToScalar(Color.RED),
3);

// box edges perpendicular to tag
for (int i = 0; i < bottomPoints.size(); i++) {
Imgproc.line(
in.getLeft(),
Expand All @@ -180,6 +188,7 @@ protected Void process(Pair<Mat, List<TrackedTarget>> in) {
ColorHelper.colorToScalar(Color.blue),
3);
}
// box edges parallel to tag
for (int i = 0; i < topPoints.size(); i++) {
Imgproc.line(
in.getLeft(),
Expand Down Expand Up @@ -265,18 +274,6 @@ private void divideMat2f(MatOfPoint2f src, MatOfPoint dst) {
dst.fromArray(pointArray);
}

private void divideMat2f(MatOfPoint2f src, MatOfPoint2f dst) {
var hull = src.toArray();
var pointArray = new Point[hull.length];
for (int i = 0; i < hull.length; i++) {
var hullAtI = hull[i];
pointArray[i] =
new Point(
hullAtI.x / (double) params.divisor.value, hullAtI.y / (double) params.divisor.value);
}
dst.fromArray(pointArray);
}

/** Scale a given point list by the current frame divisor. the point list is mutated! */
private void dividePointList(List<Point> points) {
for (var p : points) {
Expand Down
29 changes: 3 additions & 26 deletions photon-core/src/main/java/org/photonvision/vision/pipe/impl/SolvePNPPipe.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,6 @@
package org.photonvision.vision.pipe.impl;

import edu.wpi.first.math.VecBuilder;
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;
Expand All @@ -27,7 +26,6 @@
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Scalar;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.math.MathUtils;
Expand Down Expand Up @@ -99,33 +97,12 @@ private void calculateTargetPose(TrackedTarget target) {
VecBuilder.fill(rVec.get(0, 0)[0], rVec.get(1, 0)[0], rVec.get(2, 0)[0]),
Core.norm(rVec));

Pose3d targetPose = MathUtils.convertOpenCVtoPhotonPose(new Transform3d(translation, rotation));
target.setBestCameraToTarget3d(
new Transform3d(targetPose.getTranslation(), targetPose.getRotation()));
Transform3d camToTarget =
MathUtils.convertOpenCVtoPhotonTransform(new Transform3d(translation, rotation));
target.setBestCameraToTarget3d(camToTarget);
target.setAltCameraToTarget3d(new Transform3d());
}

Mat rotationMatrix = new Mat();
Mat inverseRotationMatrix = new Mat();
Mat pzeroWorld = new Mat();
Mat kMat = new Mat();
Mat scaledTvec;

/**
* Element-wise scale a matrix by a given factor
*
* @param src the source matrix
* @param factor by how much to scale each element
* @return the scaled matrix
*/
@SuppressWarnings("SameParameterValue")
private static Mat matScale(Mat src, double factor) {
Mat dst = new Mat(src.rows(), src.cols(), src.type());
Scalar s = new Scalar(factor);
Core.multiply(src, s, dst);
return dst;
}

public static class SolvePNPPipeParams {
private final CameraCalibrationCoefficients cameraCoefficients;
private final TargetModel targetModel;
Expand Down
6 changes: 4 additions & 2 deletions photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -146,8 +146,10 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
new TargetCalculationParameters(
false, null, null, null, null, frameStaticProperties));

var correctedBestPose = MathUtils.convertOpenCVtoPhotonPose(target.getBestCameraToTarget3d());
var correctedAltPose = MathUtils.convertOpenCVtoPhotonPose(target.getAltCameraToTarget3d());
var correctedBestPose =
MathUtils.convertOpenCVtoPhotonTransform(target.getBestCameraToTarget3d());
var correctedAltPose =
MathUtils.convertOpenCVtoPhotonTransform(target.getAltCameraToTarget3d());

target.setBestCameraToTarget3d(
new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation()));
Expand Down
Loading
Loading