diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 768e9fd..a21264b 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -32,7 +32,11 @@ android { dependencies { implementation project(':FtcRobotController') annotationProcessor files('lib/OpModeAnnotationProcessor.jar') - implementation 'org.openftc:easyopencv:1.5.1' + implementation 'org.openftc:easyopencv:1.5.2' implementation 'org.apache.commons:commons-math3:3.6.1' implementation 'com.acmerobotics.roadrunner:core:0.5.4' + implementation 'org.openftc:apriltag:1.1.0' +// implementation('com.acmeroborics.dashboard:dashboard:0.4.6') { +// exclude group: 'org.firstinspires.ftc' +// } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/aprilTag/AprilTagDetectionPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/aprilTag/AprilTagDetectionPipeline.java new file mode 100644 index 0000000..dd150b9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/aprilTag/AprilTagDetectionPipeline.java @@ -0,0 +1,312 @@ +/* + * Copyright (c) 2021 OpenFTC Team + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.teamcode.createdcode.aprilTag; + +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; + +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; + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/aprilTag/AprilTagDetector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/aprilTag/AprilTagDetector.java new file mode 100644 index 0000000..0c4d541 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/aprilTag/AprilTagDetector.java @@ -0,0 +1,95 @@ +package org.firstinspires.ftc.teamcode.createdcode.aprilTag; + +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import org.openftc.easyopencv.OpenCvWebcam; + +import org.openftc.apriltag.AprilTagDetection; + + +import java.util.ArrayList; + +public class AprilTagDetector { + private OpenCvWebcam webcam; + private AprilTagDetectionPipeline aprilTagDetectionPipeline; + + // private FtcDashboard dashboard = FtcDashboard.getInstance(); + // private Telemetry dashboardTelemetry = dashboard.getTelemetry(); + + private HardwareMap hardwareMap; + private int pos; + + public static final double FEET_PER_METER = 3.28084; + + // Lens intrinsics + // UNITS ARE PIXELS + // NOTE: this calibration is for the C920 webcam at 800x448. + // You will need to do your own calibration for other configurations! + private double fx = 578.272; + private double fy = 578.272; + private double cx = 402.145; + private double cy = 221.506; + + // UNITS ARE METERS + private double tagsize = 0.166; + + private int LEFT = 1; + private int MIDDLE = 2; + private int RIGHT = 3; + + private AprilTagDetection tagOfInterest = null; + + public AprilTagDetector(HardwareMap hardwareMap) { + this.hardwareMap = hardwareMap; + startCamera(); + } + + public void startCamera() { + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + webcam = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId); + aprilTagDetectionPipeline = new AprilTagDetectionPipeline(tagsize, fx, fy, cx, cy); + + webcam.setPipeline(aprilTagDetectionPipeline); + + webcam.setMillisecondsPermissionTimeout(2500); // Timeout for obtaining permission is configurable. Set before opening. + webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() + { + @Override + public void onOpened() + { + webcam.startStreaming(320,240, OpenCvCameraRotation.UPRIGHT); + } + + @Override + public void onError(int errorCode) + { + + } + }); + } + + public int getPos() { + ArrayList currentDetections = aprilTagDetectionPipeline.getLatestDetections(); + + if(currentDetections.size() != 0) { + for(AprilTagDetection tag : currentDetections) + { + if(tag.id == LEFT || tag.id == MIDDLE || tag.id == RIGHT) + { + tagOfInterest = tag; + pos = tagOfInterest.id; + } + } + } + + return pos; + } + + public void endStream() { + webcam.stopStreaming(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/aprilTag/AprilTagOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/aprilTag/AprilTagOpMode.java new file mode 100644 index 0000000..8473e55 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/aprilTag/AprilTagOpMode.java @@ -0,0 +1,21 @@ +package org.firstinspires.ftc.teamcode.createdcode.aprilTag; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +@Autonomous (group = "Test") +public class AprilTagOpMode extends LinearOpMode { + private AprilTagDetector detector; + private int pos; + + @Override + public void runOpMode() throws InterruptedException { + detector = new AprilTagDetector(hardwareMap); + while (!isStarted() && !isStopRequested()) { + pos = detector.getPos(); + telemetry.addLine(String.format("\nDetected tag ID=%d", pos)); + telemetry.update(); + sleep(50); + } + } +} diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 066870d..ac156ab 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -12,7 +12,7 @@ dependencies { def multidex_version = "2.0.1" implementation "androidx.multidex:multidex:$multidex_version" implementation 'org.openftc:apriltag:1.1.0' - implementation 'org.openftc:easyopencv:1.5.1' + implementation 'org.openftc:easyopencv:1.5.2' implementation 'org.jetbrains:annotations:15.0' implementation('com.acmerobotics.dashboard:dashboard:0.4.6') { exclude group: 'org.firstinspires.ftc'