From aa4509b1f9ff0219b4bfeef0df6e766c8a6c6798 Mon Sep 17 00:00:00 2001 From: Yash Karandikar Date: Mon, 14 Nov 2022 18:26:24 -0600 Subject: [PATCH] vendor: AprilTagDetectionPipeline --- .../vendor/AprilTagDetectionPipeline.java | 268 ++++++++++++++++++ 1 file changed, 268 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/vendor/AprilTagDetectionPipeline.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/vendor/AprilTagDetectionPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/vendor/AprilTagDetectionPipeline.java new file mode 100644 index 0000000..2c56c61 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/vendor/AprilTagDetectionPipeline.java @@ -0,0 +1,268 @@ +package org.firstinspires.ftc.teamcode.createdcode.enhancedautos.vendor; + + +import org.opencv.calib3d.Calib3d; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.MatOfDouble; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.MatOfPoint3f; +import org.opencv.core.Point; +import org.opencv.core.Point3; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.openftc.apriltag.AprilTagDetection; +import org.openftc.apriltag.AprilTagDetectorJNI; +import org.openftc.easyopencv.OpenCvPipeline; + +import java.util.ArrayList; + +public class AprilTagDetectionPipeline extends OpenCvPipeline { + private long nativeApriltagPtr; + private Mat grey = new Mat(); + private ArrayList detections = new ArrayList<>(); + + private ArrayList detectionsUpdate = new ArrayList<>(); + private final Object detectionsUpdateSync = new Object(); + + Mat cameraMatrix; + + Scalar blue = new Scalar(7, 197, 235, 255); + Scalar red = new Scalar(255, 0, 0, 255); + Scalar green = new Scalar(0, 255, 0, 255); + Scalar white = new Scalar(255, 255, 255, 255); + + double fx; + double fy; + double cx; + double cy; + + // UNITS ARE METERS + double tagsize; + double tagsizeX; + double tagsizeY; + + private float decimation; + private boolean needToSetDecimation; + private final Object decimationSync = new Object(); + + public AprilTagDetectionPipeline(double tagsize, double fx, double fy, double cx, double cy) { + this.tagsize = tagsize; + this.tagsizeX = tagsize; + this.tagsizeY = tagsize; + this.fx = fx; + this.fy = fy; + this.cx = cx; + this.cy = cy; + + constructMatrix(); + + // Allocate a native context object. See the corresponding deletion in the finalizer + nativeApriltagPtr = AprilTagDetectorJNI.createApriltagDetector(AprilTagDetectorJNI.TagFamily.TAG_36h11.string, 3, 3); + } + + @Override + public void finalize() { + // Might be null if createApriltagDetector() threw an exception + if (nativeApriltagPtr != 0) { + // Delete the native context we created in the constructor + AprilTagDetectorJNI.releaseApriltagDetector(nativeApriltagPtr); + nativeApriltagPtr = 0; + } else { + System.out.println("AprilTagDetectionPipeline.finalize(): nativeApriltagPtr was NULL"); + } + } + + @Override + public Mat processFrame(Mat input) { + // Convert to greyscale + Imgproc.cvtColor(input, grey, Imgproc.COLOR_RGBA2GRAY); + + synchronized (decimationSync) { + if (needToSetDecimation) { + AprilTagDetectorJNI.setApriltagDetectorDecimation(nativeApriltagPtr, decimation); + needToSetDecimation = false; + } + } + + // Run AprilTag + detections = AprilTagDetectorJNI.runAprilTagDetectorSimple(nativeApriltagPtr, grey, tagsize, fx, fy, cx, cy); + + synchronized (detectionsUpdateSync) { + detectionsUpdate = detections; + } + + // For fun, use OpenCV to draw 6DOF markers on the image. We actually recompute the pose using + // OpenCV because I haven't yet figured out how to re-use AprilTag's pose in OpenCV. + for (AprilTagDetection detection : detections) { + Pose pose = poseFromTrapezoid(detection.corners, cameraMatrix, tagsizeX, tagsizeY); + drawAxisMarker(input, tagsizeY / 2.0, 6, pose.rvec, pose.tvec, cameraMatrix); + draw3dCubeMarker(input, tagsizeX, tagsizeX, tagsizeY, 5, pose.rvec, pose.tvec, cameraMatrix); + } + + return input; + } + + public void setDecimation(float decimation) { + synchronized (decimationSync) { + this.decimation = decimation; + needToSetDecimation = true; + } + } + + public ArrayList getLatestDetections() { + return detections; + } + + public ArrayList getDetectionsUpdate() { + synchronized (detectionsUpdateSync) { + ArrayList ret = detectionsUpdate; + detectionsUpdate = null; + return ret; + } + } + + void constructMatrix() { + // Construct the camera matrix. + // + // -- -- + // | fx 0 cx | + // | 0 fy cy | + // | 0 0 1 | + // -- -- + // + + cameraMatrix = new Mat(3, 3, CvType.CV_32FC1); + + cameraMatrix.put(0, 0, fx); + cameraMatrix.put(0, 1, 0); + cameraMatrix.put(0, 2, cx); + + cameraMatrix.put(1, 0, 0); + cameraMatrix.put(1, 1, fy); + cameraMatrix.put(1, 2, cy); + + cameraMatrix.put(2, 0, 0); + cameraMatrix.put(2, 1, 0); + cameraMatrix.put(2, 2, 1); + } + + /** + * Draw a 3D axis marker on a detection. (Similar to what Vuforia does) + * + * @param buf the RGB buffer on which to draw the marker + * @param length the length of each of the marker 'poles' + * @param rvec the rotation vector of the detection + * @param tvec the translation vector of the detection + * @param cameraMatrix the camera matrix used when finding the detection + */ + void drawAxisMarker(Mat buf, double length, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix) { + // The points in 3D space we wish to project onto the 2D image plane. + // The origin of the coordinate space is assumed to be in the center of the detection. + MatOfPoint3f axis = new MatOfPoint3f( + new Point3(0, 0, 0), + new Point3(length, 0, 0), + new Point3(0, length, 0), + new Point3(0, 0, -length) + ); + + // Project those points + MatOfPoint2f matProjectedPoints = new MatOfPoint2f(); + Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints); + Point[] projectedPoints = matProjectedPoints.toArray(); + + // Draw the marker! + Imgproc.line(buf, projectedPoints[0], projectedPoints[1], red, thickness); + Imgproc.line(buf, projectedPoints[0], projectedPoints[2], green, thickness); + Imgproc.line(buf, projectedPoints[0], projectedPoints[3], blue, thickness); + + Imgproc.circle(buf, projectedPoints[0], thickness, white, -1); + } + + void draw3dCubeMarker(Mat buf, double length, double tagWidth, double tagHeight, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix) { + //axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0], + // [0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ]) + + // The points in 3D space we wish to project onto the 2D image plane. + // The origin of the coordinate space is assumed to be in the center of the detection. + MatOfPoint3f axis = new MatOfPoint3f( + new Point3(-tagWidth / 2, tagHeight / 2, 0), + new Point3(tagWidth / 2, tagHeight / 2, 0), + new Point3(tagWidth / 2, -tagHeight / 2, 0), + new Point3(-tagWidth / 2, -tagHeight / 2, 0), + new Point3(-tagWidth / 2, tagHeight / 2, -length), + new Point3(tagWidth / 2, tagHeight / 2, -length), + new Point3(tagWidth / 2, -tagHeight / 2, -length), + new Point3(-tagWidth / 2, -tagHeight / 2, -length)); + + // Project those points + MatOfPoint2f matProjectedPoints = new MatOfPoint2f(); + Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints); + Point[] projectedPoints = matProjectedPoints.toArray(); + + // Pillars + for (int i = 0; i < 4; i++) { + Imgproc.line(buf, projectedPoints[i], projectedPoints[i + 4], blue, thickness); + } + + // Base lines + //Imgproc.line(buf, projectedPoints[0], projectedPoints[1], blue, thickness); + //Imgproc.line(buf, projectedPoints[1], projectedPoints[2], blue, thickness); + //Imgproc.line(buf, projectedPoints[2], projectedPoints[3], blue, thickness); + //Imgproc.line(buf, projectedPoints[3], projectedPoints[0], blue, thickness); + + // Top lines + Imgproc.line(buf, projectedPoints[4], projectedPoints[5], green, thickness); + Imgproc.line(buf, projectedPoints[5], projectedPoints[6], green, thickness); + Imgproc.line(buf, projectedPoints[6], projectedPoints[7], green, thickness); + Imgproc.line(buf, projectedPoints[4], projectedPoints[7], green, thickness); + } + + /** + * Extracts 6DOF pose from a trapezoid, using a camera intrinsics matrix and the + * original size of the tag. + * + * @param points the points which form the trapezoid + * @param cameraMatrix the camera intrinsics matrix + * @param tagsizeX the original width of the tag + * @param tagsizeY the original height of the tag + * @return the 6DOF pose of the camera relative to the tag + */ + Pose poseFromTrapezoid(Point[] points, Mat cameraMatrix, double tagsizeX, double tagsizeY) { + // The actual 2d points of the tag detected in the image + MatOfPoint2f points2d = new MatOfPoint2f(points); + + // The 3d points of the tag in an 'ideal projection' + Point3[] arrayPoints3d = new Point3[4]; + arrayPoints3d[0] = new Point3(-tagsizeX / 2, tagsizeY / 2, 0); + arrayPoints3d[1] = new Point3(tagsizeX / 2, tagsizeY / 2, 0); + arrayPoints3d[2] = new Point3(tagsizeX / 2, -tagsizeY / 2, 0); + arrayPoints3d[3] = new Point3(-tagsizeX / 2, -tagsizeY / 2, 0); + MatOfPoint3f points3d = new MatOfPoint3f(arrayPoints3d); + + // Using this information, actually solve for pose + Pose pose = new Pose(); + Calib3d.solvePnP(points3d, points2d, cameraMatrix, new MatOfDouble(), pose.rvec, pose.tvec, false); + + return pose; + } + + /* + * A simple container to hold both rotation and translation + * vectors, which together form a 6DOF pose. + */ + class Pose { + Mat rvec; + Mat tvec; + + public Pose() { + rvec = new Mat(); + tvec = new Mat(); + } + + public Pose(Mat rvec, Mat tvec) { + this.rvec = rvec; + this.tvec = tvec; + } + } +}