OK, I’m back. We made the following pipeline (edited for brevity, but I left in a couple of the commented out lines from last year’s processor). processFrame gets called, and the viewport shows the image, but detections is always zero. I am certain we are doing something wrong, but can’t figure out what we are missing. The team qualified for worlds today, but we were at a real disadvantage without the ability to see AprilTags. Any thoughts appreciated.
package org.firstinspires.ftc.teamcode.NewCustomSwerve.SubSystems;
/*
* 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.
*/
import ...
public class AprilTagDetectionPipeline extends OpenCvPipeline
{
// STATIC CONSTANTS
// 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!
public static double fx = 578.272;
public static double fy = 578.272;
public static double cx = 402.145;
public static double cy = 221.506;
public int tagID=0;
// UNITS ARE METERS
public static double TAG_SIZE = 0.166;
// instance variables
private Mat grey = new Mat();
private ArrayList<AprilTagDetection> detections = new ArrayList<>();
private AprilTagProcessor aprilTag;
//private ArrayList<AprilTagDetection> detectionsUpdate = new ArrayList<>();
private List<AprilTagDetection> currentDetections;
private final Object detectionsUpdateSync = new Object();
Mat cameraMatrix;
double tagsizeX = TAG_SIZE;
double tagsizeY = TAG_SIZE;
private float decimation;
private boolean needToSetDecimation;
private final Object decimationSync = new Object();
private Webcam webcam;
public AprilTagDetectionPipeline(Webcam webcam) {
this.webcam = webcam;
// constructMatrix();
// Create the AprilTag processor.
aprilTag = new AprilTagProcessor.Builder()
// The following default settings are available to un-comment and edit as needed.
.setDrawAxes(true)
//.setDrawCubeProjection(false)
.setDrawTagOutline(true)
//.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11)
.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary())
.setOutputUnits(DistanceUnit.INCH, AngleUnit.RADIANS)
// == CAMERA CALIBRATION ==
// If you do not manually specify calibration parameters, the SDK will attempt
// to load a predefined calibration for your camera.
//.setLensIntrinsics(578.272, 578.272, 402.145, 221.506)
// ... these parameters are fx, fy, cx, cy.
.build();
// Adjust Image Decimation to trade-off detection-range for detection-rate.
// eg: Some typical detection data using a Logitech C920 WebCam
// Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second
// Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second
// Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default)
// Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default)
// Note: Decimation can be changed on-the-fly to adapt during a match.
//aprilTag.setDecimation(3);
}
@Override
public void init(Mat frame)
{
// Allocate a native context object. See the corresponding deletion in the finalizer
//nativeApriltagPtr = AprilTagDetectorJNI.createApriltagDetector(AprilTagDetectorJNI.TagFamily.TAG_36h11.string, 3, 3);
// aprilTag.init();
}
@Override
public void finalize()
{
// Delete the native context we created in the init() function
// AprilTagDetectorJNI.releaseApriltagDetector(nativeApriltagPtr);
}
@Override
public Mat processFrame(Mat input)
{
// Convert to greyscale
Imgproc.cvtColor(input, grey, Imgproc.COLOR_RGBA2GRAY);
currentDetections = aprilTag.getDetections();
webcam.telemetry.addData("# AprilTags Detected", currentDetections.size());
// Step through the list of detections and display info for each one.
for (AprilTagDetection detection : currentDetections) {
if (detection.metadata != null) {
webcam.tagPoseData[detection.id-1]=detection.ftcPose;
webcam.telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
webcam.telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
webcam.telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
webcam.telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation));
} else {
webcam.telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id));
webcam.telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y));
}
} // end for() loop
// Add "key" information to telemetry
webcam.telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist.");
webcam.telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)");
webcam.telemetry.addLine("RBE = Range, Bearing & Elevation");
webcam.telemetry.update();
return input;
}
public void setDecimation(float decimation)
{
synchronized (decimationSync)
{
this.decimation = decimation;
needToSetDecimation = true;
}
}
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;
}
}
}
