Skip to content

Commit

Permalink
[photon-core] [2024] Cleanup and document coordinate system conversion (
Browse files Browse the repository at this point in the history 
#894)

* cleanup and document coordinate conversion

* spotless

* bump wpilib version

* coordinate conversion tests

* fix/document SolvePNPPipe models and corners

* format

* run lint

---------

Co-authored-by: Matthew Morley <[email protected]>
  • Loading branch information
@amquake @mcm001
amquake and mcm001 authored Oct 15, 2023
1 parent 7f94962 commit ad4f462
Show file tree
Hide file tree
Showing 9 changed files with 258 additions and 217 deletions.
93 changes: 34 additions & 59 deletions photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java
View file
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
61 changes: 27 additions & 34 deletions photon-core/src/main/java/org/photonvision/vision/pipe/impl/CornerDetectionPipe.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,14 +17,17 @@

package org.photonvision.vision.pipe.impl;

import edu.wpi.first.math.geometry.Translation2d;
import java.util.*;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
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 +52,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 +71,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 +85,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 +134,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 +170,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
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
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
Loading

0 comments on commit ad4f462

Please sign in to comment.