April Tag Detection
This commit is contained in:
parent
545bd14bb0
commit
7e3925e5b2
|
@ -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'
|
||||
// }
|
||||
}
|
||||
|
|
|
@ -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<AprilTagDetection> detections = new ArrayList<>();
|
||||
|
||||
private ArrayList<AprilTagDetection> 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<AprilTagDetection> getLatestDetections()
|
||||
{
|
||||
return detections;
|
||||
}
|
||||
|
||||
public ArrayList<AprilTagDetection> getDetectionsUpdate()
|
||||
{
|
||||
synchronized (detectionsUpdateSync)
|
||||
{
|
||||
ArrayList<AprilTagDetection> 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;
|
||||
}
|
||||
}
|
||||
}
|
|
@ -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<AprilTagDetection> 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();
|
||||
}
|
||||
}
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
|
@ -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'
|
||||
|
|
Loading…
Reference in New Issue