Initial commit
This commit is contained in:
commit
fc13916f0b
|
|
@ -0,0 +1 @@
|
||||||
|
/build
|
||||||
|
|
@ -0,0 +1,80 @@
|
||||||
|
import java.text.SimpleDateFormat
|
||||||
|
|
||||||
|
buildscript {
|
||||||
|
repositories {
|
||||||
|
mavenCentral()
|
||||||
|
google()
|
||||||
|
}
|
||||||
|
dependencies {
|
||||||
|
classpath 'com.android.tools.build:gradle:8.2.1'
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
plugins {
|
||||||
|
id 'com.android.library'
|
||||||
|
id 'maven-publish'
|
||||||
|
id 'signing'
|
||||||
|
}
|
||||||
|
|
||||||
|
java {
|
||||||
|
sourceCompatibility = JavaVersion.VERSION_1_8
|
||||||
|
targetCompatibility = JavaVersion.VERSION_1_8
|
||||||
|
}
|
||||||
|
|
||||||
|
group = 'com.tearabite.ftctearabits'
|
||||||
|
version = '1.0'
|
||||||
|
|
||||||
|
sourceSets {
|
||||||
|
main {
|
||||||
|
java { srcDirs = ["src/java"] }
|
||||||
|
resources { srcDir "src/resources" }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
android {
|
||||||
|
namespace = 'com.tearabite.ftctearabits'
|
||||||
|
|
||||||
|
defaultConfig {
|
||||||
|
minSdkVersion 24
|
||||||
|
productFlavors {
|
||||||
|
biscuit {
|
||||||
|
aarMetadata {
|
||||||
|
minCompileSdk 24
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
testFixtures {
|
||||||
|
enable true
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
publishing {
|
||||||
|
publications {
|
||||||
|
mavenJava(MavenPublication) {
|
||||||
|
groupId = 'com.tearabite.ftctearabits'
|
||||||
|
artifactId = 'ftctearabits'
|
||||||
|
version = '1.0'
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
signing {
|
||||||
|
sign publishing.publications.mavenJava
|
||||||
|
}
|
||||||
|
|
||||||
|
repositories {
|
||||||
|
mavenCentral()
|
||||||
|
maven { url = 'https://maven.brott.dev/' }
|
||||||
|
}
|
||||||
|
|
||||||
|
dependencies {
|
||||||
|
implementation 'org.firstinspires.ftc:RobotCore:9.0.1'
|
||||||
|
implementation 'com.acmerobotics.roadrunner:core:1.0.0-beta6'
|
||||||
|
implementation 'com.acmerobotics.roadrunner:actions:1.0.0-beta6'
|
||||||
|
implementation 'org.firstinspires.ftc:Vision:9.0.1'
|
||||||
|
testImplementation 'org.junit.jupiter:junit-jupiter:5.9.1'
|
||||||
|
compileOnly 'org.projectlombok:lombok:1.18.30'
|
||||||
|
annotationProcessor 'org.projectlombok:lombok:1.18.30'
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,21 @@
|
||||||
|
# Add project specific ProGuard rules here.
|
||||||
|
# You can control the set of applied configuration files using the
|
||||||
|
# proguardFiles setting in build.gradle.
|
||||||
|
#
|
||||||
|
# For more details, see
|
||||||
|
# http://developer.android.com/guide/developing/tools/proguard.html
|
||||||
|
|
||||||
|
# If your project uses WebView with JS, uncomment the following
|
||||||
|
# and specify the fully qualified class name to the JavaScript interface
|
||||||
|
# class:
|
||||||
|
#-keepclassmembers class fqcn.of.javascript.interface.for.webview {
|
||||||
|
# public *;
|
||||||
|
#}
|
||||||
|
|
||||||
|
# Uncomment this to preserve the line number information for
|
||||||
|
# debugging stack traces.
|
||||||
|
#-keepattributes SourceFile,LineNumberTable
|
||||||
|
|
||||||
|
# If you keep the line number information, uncomment this to
|
||||||
|
# hide the original source file name.
|
||||||
|
#-renamesourcefileattribute SourceFile
|
||||||
|
|
@ -0,0 +1,11 @@
|
||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
|
||||||
|
<!-- Note: the actual manifest file used in your APK merges this file with contributions
|
||||||
|
from the modules on which your app depends (such as FtcRobotController, etc).
|
||||||
|
So it won't ultimately be as empty as it might here appear to be :-) -->
|
||||||
|
|
||||||
|
<!-- The package name here determines the package for your R class and your BuildConfig class -->
|
||||||
|
<manifest
|
||||||
|
xmlns:android="http://schemas.android.com/apk/res/android">
|
||||||
|
<application/>
|
||||||
|
</manifest>
|
||||||
|
|
@ -0,0 +1,4 @@
|
||||||
|
package com.tearabite.ftctearabits;
|
||||||
|
|
||||||
|
public enum Alliance { Blue, Red }
|
||||||
|
|
||||||
|
|
@ -0,0 +1,82 @@
|
||||||
|
package com.tearabite.ftctearabits.localization;
|
||||||
|
|
||||||
|
import static java.lang.Math.PI;
|
||||||
|
import static java.lang.Math.asin;
|
||||||
|
import static java.lang.Math.atan2;
|
||||||
|
import static java.lang.Math.cos;
|
||||||
|
import static java.lang.Math.pow;
|
||||||
|
import static java.lang.Math.sin;
|
||||||
|
import static java.lang.Math.tan;
|
||||||
|
|
||||||
|
import com.acmerobotics.roadrunner.Pose2d;
|
||||||
|
import com.acmerobotics.roadrunner.Vector2d;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.Quaternion;
|
||||||
|
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
|
||||||
|
import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc;
|
||||||
|
|
||||||
|
import java.security.InvalidParameterException;
|
||||||
|
|
||||||
|
public class AprilTagPoseEstimator {
|
||||||
|
|
||||||
|
private final Pose2d robotOffset;
|
||||||
|
|
||||||
|
public AprilTagPoseEstimator() {
|
||||||
|
this(new Pose2d(new Vector2d(0, 0), 0));
|
||||||
|
}
|
||||||
|
|
||||||
|
public AprilTagPoseEstimator(Pose2d robotOffset) {
|
||||||
|
this.robotOffset = robotOffset;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
public Pose2d estimatePose(AprilTagDetection detection) {
|
||||||
|
if (detection == null || detection.metadata == null || detection.metadata.fieldPosition == null || detection.ftcPose == null) {
|
||||||
|
throw new InvalidParameterException();
|
||||||
|
}
|
||||||
|
|
||||||
|
AprilTagPoseFtc ftcPose = detection.ftcPose;
|
||||||
|
VectorF fieldPosition = detection.metadata.fieldPosition;
|
||||||
|
Quaternion fieldOrientation = detection.metadata.fieldOrientation;
|
||||||
|
return estimatePose(
|
||||||
|
Math.toRadians(ftcPose.yaw),
|
||||||
|
Math.toRadians(ftcPose.bearing),
|
||||||
|
ftcPose.range,
|
||||||
|
fieldPosition.get(0),
|
||||||
|
fieldPosition.get(1),
|
||||||
|
fieldOrientation.x,
|
||||||
|
fieldOrientation.y,
|
||||||
|
fieldOrientation.z,
|
||||||
|
fieldOrientation.w);
|
||||||
|
}
|
||||||
|
|
||||||
|
private Pose2d estimatePose(double yaw, double bearing, double range, double Tx, double Ty, double Ta, double Tb, double Tc, double Td) {
|
||||||
|
double aprilTagHeading = getTYaw(Ta, Tb, Tc, Td);
|
||||||
|
double cameraRotation = ((PI / 2) - aprilTagHeading + yaw);
|
||||||
|
double cx = Tx + range * cos(bearing) * cos(cameraRotation) + range * sin(bearing) * sin(cameraRotation);
|
||||||
|
double cy = Ty + range * cos(bearing) * sin(cameraRotation) - range * sin(bearing) * cos(cameraRotation);
|
||||||
|
|
||||||
|
double aprilTagX = this.robotOffset.position.x;
|
||||||
|
double aprilTagY = this.robotOffset.position.y;
|
||||||
|
double cameraRotationOnRobot = this.robotOffset.heading.toDouble();
|
||||||
|
double robotRotation = (PI / 2) + aprilTagHeading + yaw - cameraRotationOnRobot;
|
||||||
|
double rx = cx + aprilTagX * cos(robotRotation) - aprilTagY * sin(robotRotation);
|
||||||
|
double ry = cy + aprilTagX * sin(robotRotation) + aprilTagY * cos(robotRotation);
|
||||||
|
double rh = tan(yaw - cameraRotationOnRobot);
|
||||||
|
|
||||||
|
return new Pose2d(rx, ry, rh);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static double getTPitch(double a, double b, double c, double d) {
|
||||||
|
return asin(2 * ((a * d) - (b * c)));
|
||||||
|
}
|
||||||
|
|
||||||
|
private static double getTRoll(double a, double b, double c, double d) {
|
||||||
|
return atan2(2 * ((a * c) + (b * d)), 1 - 2 * (pow(c, 2) + pow(d, 2)));
|
||||||
|
}
|
||||||
|
|
||||||
|
private static double getTYaw(double a, double b, double c, double d) {
|
||||||
|
return atan2(2 * ((a * b) + (c * d)), 1 - 2 * ((pow(b, 2) + pow(c, 2))));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,79 @@
|
||||||
|
package com.tearabite.ftctearabits.vision;
|
||||||
|
|
||||||
|
import static com.tearabite.ftctearabits.vision.Colors.WHITE;
|
||||||
|
import static com.tearabite.ftctearabits.vision.OpenCVUtil.getLargestContour;
|
||||||
|
|
||||||
|
import android.graphics.Canvas;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;
|
||||||
|
import org.firstinspires.ftc.vision.VisionProcessor;
|
||||||
|
import org.opencv.core.Core;
|
||||||
|
import org.opencv.core.Mat;
|
||||||
|
import org.opencv.core.MatOfPoint;
|
||||||
|
import org.opencv.core.Point;
|
||||||
|
import org.opencv.core.Size;
|
||||||
|
import org.opencv.imgproc.Imgproc;
|
||||||
|
|
||||||
|
import java.util.ArrayList;
|
||||||
|
|
||||||
|
import lombok.Getter;
|
||||||
|
|
||||||
|
public class BasicColorDetectionVisionProcessor implements VisionProcessor {
|
||||||
|
public static final Size BLUR_SIZE = new Size(7, 7);
|
||||||
|
public static final int ERODE_DILATE_ITERATIONS = 2;
|
||||||
|
public static final Mat STRUCTURING_ELEMENT = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5));
|
||||||
|
public static final Point ANCHOR = new Point((STRUCTURING_ELEMENT.cols() / 2f), STRUCTURING_ELEMENT.rows() / 2f);
|
||||||
|
|
||||||
|
private final Mat blurred = new Mat();
|
||||||
|
private final ScalarRange[] colorRanges;
|
||||||
|
@Getter private Detection detection;
|
||||||
|
private final Mat hsv = new Mat();
|
||||||
|
private final double ignoreSmallerThan;
|
||||||
|
private final double ignoreLargerThan;
|
||||||
|
private final Mat mask = new Mat();
|
||||||
|
private final Mat tmpMask = new Mat();
|
||||||
|
|
||||||
|
|
||||||
|
public BasicColorDetectionVisionProcessor(double ignoreSmallerThan, double ignoreLargerThan, ScalarRange... colorRanges) {
|
||||||
|
this.ignoreSmallerThan = ignoreSmallerThan;
|
||||||
|
this.ignoreLargerThan = ignoreLargerThan;
|
||||||
|
this.colorRanges = colorRanges;
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void init(int width, int height, CameraCalibration calibration) {
|
||||||
|
this.detection = new Detection(new Size(width, height), ignoreSmallerThan, ignoreLargerThan);
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public Object processFrame(Mat input, long captureTimeNanos) {
|
||||||
|
Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0);
|
||||||
|
Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV);
|
||||||
|
|
||||||
|
mask.release();
|
||||||
|
for (ScalarRange colorRange : this.colorRanges) {
|
||||||
|
Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), tmpMask);
|
||||||
|
if (mask.empty() || mask.rows() <= 0) {
|
||||||
|
Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), mask);
|
||||||
|
}
|
||||||
|
Core.add(mask, tmpMask, mask);
|
||||||
|
}
|
||||||
|
|
||||||
|
Imgproc.erode(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
|
||||||
|
Imgproc.dilate(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
|
||||||
|
|
||||||
|
ArrayList<MatOfPoint> contours = new ArrayList<>();
|
||||||
|
Imgproc.findContours(mask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
|
||||||
|
|
||||||
|
detection.setContour(getLargestContour(contours));
|
||||||
|
|
||||||
|
return input;
|
||||||
|
}
|
||||||
|
@Override
|
||||||
|
public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) {
|
||||||
|
if (detection != null && detection.isValid()) {
|
||||||
|
Point center = detection.getCenterPx();
|
||||||
|
canvas.drawCircle((float) center.x, (float) center.y, 10, WHITE);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,21 @@
|
||||||
|
package com.tearabite.ftctearabits.vision;
|
||||||
|
|
||||||
|
import android.graphics.Color;
|
||||||
|
|
||||||
|
import org.opencv.core.Scalar;
|
||||||
|
public class Colors {
|
||||||
|
public static Scalar FTC_RED_LOWER = new Scalar(165, 80, 80);
|
||||||
|
public static Scalar FTC_RED_UPPER = new Scalar(15, 255, 255);
|
||||||
|
public static ScalarRange FTC_RED_RANGE_1 = new ScalarRange(new Scalar(180, FTC_RED_UPPER.val[1], FTC_RED_UPPER.val[2]), FTC_RED_LOWER);
|
||||||
|
public static ScalarRange FTC_RED_RANGE_2 = new ScalarRange(FTC_RED_UPPER, new Scalar(0, FTC_RED_LOWER.val[1], FTC_RED_LOWER.val[2]));
|
||||||
|
public static Scalar FTC_BLUE_LOWER = new Scalar(75, 40, 80);
|
||||||
|
public static Scalar FTC_BLUE_UPPER = new Scalar(120, 255, 255);
|
||||||
|
public static ScalarRange FTC_BLUE_RANGE = new ScalarRange(FTC_BLUE_UPPER, FTC_BLUE_LOWER);
|
||||||
|
public static Scalar FTC_WHITE_LOWER = new Scalar(0, 0, 40);
|
||||||
|
public static Scalar FTC_WHITE_UPPER = new Scalar(180, 30, 255);
|
||||||
|
|
||||||
|
public static OpenCVUtil.LinePaint RED = new OpenCVUtil.LinePaint(Color.RED);
|
||||||
|
public static OpenCVUtil.LinePaint BLUE = new OpenCVUtil.LinePaint(Color.BLUE);
|
||||||
|
public static OpenCVUtil.LinePaint BLACK = new OpenCVUtil.LinePaint(Color.BLACK);
|
||||||
|
public static OpenCVUtil.LinePaint WHITE = new OpenCVUtil.LinePaint(Color.WHITE);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,15 @@
|
||||||
|
package com.tearabite.ftctearabits.vision;
|
||||||
|
|
||||||
|
import org.opencv.core.Scalar;
|
||||||
|
|
||||||
|
public class Constants {
|
||||||
|
public static Scalar RED = new Scalar(255, 0, 0);
|
||||||
|
public static Scalar GREEN = new Scalar(0, 255, 0);
|
||||||
|
public static Scalar BLUE = new Scalar(0, 0, 255);
|
||||||
|
public static Scalar WHITE = new Scalar(255, 255, 255);
|
||||||
|
public static Scalar GRAY = new Scalar(80, 80, 80);
|
||||||
|
public static Scalar BLACK = new Scalar(0, 0, 0);
|
||||||
|
public static Scalar ORANGE = new Scalar(255, 165, 0);
|
||||||
|
public static Scalar YELLOW = new Scalar(255, 255, 0);
|
||||||
|
public static Scalar PURPLE = new Scalar(128, 0, 128);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,133 @@
|
||||||
|
package com.tearabite.ftctearabits.vision;
|
||||||
|
|
||||||
|
import static com.tearabite.ftctearabits.vision.Constants.GREEN;
|
||||||
|
import static com.tearabite.ftctearabits.vision.OpenCVUtil.drawConvexHull;
|
||||||
|
import static com.tearabite.ftctearabits.vision.OpenCVUtil.drawPoint;
|
||||||
|
import static com.tearabite.ftctearabits.vision.OpenCVUtil.fillConvexHull;
|
||||||
|
import static com.tearabite.ftctearabits.vision.OpenCVUtil.getBottomLeftOfContour;
|
||||||
|
import static com.tearabite.ftctearabits.vision.OpenCVUtil.getBottomRightOfContour;
|
||||||
|
import static com.tearabite.ftctearabits.vision.OpenCVUtil.getCenterOfContour;
|
||||||
|
|
||||||
|
import org.opencv.core.Mat;
|
||||||
|
import org.opencv.core.MatOfPoint;
|
||||||
|
import org.opencv.core.Point;
|
||||||
|
import org.opencv.core.Scalar;
|
||||||
|
import org.opencv.core.Size;
|
||||||
|
import org.opencv.imgproc.Imgproc;
|
||||||
|
|
||||||
|
// Class for a Detection
|
||||||
|
public class Detection {
|
||||||
|
public static final Point INVALID_POINT = new Point(Double.MIN_VALUE, Double.MIN_VALUE);
|
||||||
|
public static final double INVALID_AREA = -1;
|
||||||
|
public static final Detection INVALID_DETECTION = new Detection(new Size(0, 0), 0);
|
||||||
|
|
||||||
|
private double minAreaPx;
|
||||||
|
private double maxAreaPx;
|
||||||
|
private final Size maxSizePx;
|
||||||
|
private double areaPx = INVALID_AREA;
|
||||||
|
private Point centerPx = INVALID_POINT;
|
||||||
|
private Point bottomLeftPx = INVALID_POINT;
|
||||||
|
private Point bottomRightPx = INVALID_POINT;
|
||||||
|
private MatOfPoint contour;
|
||||||
|
|
||||||
|
// Constructor
|
||||||
|
public Detection(Size frameSize, double minAreaFactor) {
|
||||||
|
this.maxSizePx = frameSize;
|
||||||
|
this.minAreaPx = frameSize.area() * minAreaFactor;
|
||||||
|
this.maxAreaPx = frameSize.area();
|
||||||
|
}
|
||||||
|
|
||||||
|
public Detection(Size frameSize, double minAreaFactor, double maxAreaFactor) {
|
||||||
|
this.maxSizePx = frameSize;
|
||||||
|
this.minAreaPx = frameSize.area() * minAreaFactor;
|
||||||
|
this.maxAreaPx = frameSize.area() * maxAreaFactor;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setMinArea(double minAreaFactor) {
|
||||||
|
this.minAreaPx = maxSizePx.area() * minAreaFactor;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void setMaxArea(double maxAreaFactor) {
|
||||||
|
this.minAreaPx = maxSizePx.area() * maxAreaFactor;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Draw a convex hull around the current detection on the given image
|
||||||
|
public void draw(Mat img, Scalar color) {
|
||||||
|
if (isValid()) {
|
||||||
|
drawConvexHull(img, contour, color);
|
||||||
|
drawPoint(img, centerPx, GREEN);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Draw a convex hull around the current detection on the given image
|
||||||
|
public void fill(Mat img, Scalar color) {
|
||||||
|
if (isValid()) {
|
||||||
|
fillConvexHull(img, contour, color);
|
||||||
|
drawPoint(img, centerPx, GREEN);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check if the current Detection is valid
|
||||||
|
public boolean isValid() {
|
||||||
|
return (this.contour != null) && (this.areaPx != INVALID_AREA);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the current contour
|
||||||
|
public MatOfPoint getContour() {
|
||||||
|
return contour;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the values of the current contour
|
||||||
|
public void setContour(MatOfPoint contour) {
|
||||||
|
this.contour = contour;
|
||||||
|
|
||||||
|
double area;
|
||||||
|
if (contour != null && (area = Imgproc.contourArea(contour)) > minAreaPx && area < maxAreaPx) {
|
||||||
|
this.areaPx = area;
|
||||||
|
this.centerPx = getCenterOfContour(contour);
|
||||||
|
this.bottomLeftPx = getBottomLeftOfContour(contour);
|
||||||
|
this.bottomRightPx = getBottomRightOfContour(contour);
|
||||||
|
} else {
|
||||||
|
this.areaPx = INVALID_AREA;
|
||||||
|
this.centerPx = INVALID_POINT;
|
||||||
|
this.bottomLeftPx = INVALID_POINT;
|
||||||
|
this.bottomRightPx = INVALID_POINT;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Returns the center of the Detection, normalized so that the width and height of the frame is from [-50,50]
|
||||||
|
public Point getCenter() {
|
||||||
|
if (!isValid()) {
|
||||||
|
return INVALID_POINT;
|
||||||
|
}
|
||||||
|
|
||||||
|
double normalizedX = ((centerPx.x / maxSizePx.width) * 100) - 50;
|
||||||
|
double normalizedY = ((centerPx.y / maxSizePx.height) * -100) + 50;
|
||||||
|
|
||||||
|
return new Point(normalizedX, normalizedY);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the center point in pixels
|
||||||
|
public Point getCenterPx() {
|
||||||
|
return centerPx;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the area of the Detection, normalized so that the area of the frame is 100
|
||||||
|
public double getArea() {
|
||||||
|
if (!isValid()) {
|
||||||
|
return INVALID_AREA;
|
||||||
|
}
|
||||||
|
|
||||||
|
return (areaPx / (maxSizePx.width * maxSizePx.height)) * 100;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the leftmost bottom corner of the detection
|
||||||
|
public Point getBottomLeftCornerPx() {
|
||||||
|
return bottomLeftPx;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the rightmost bottom corner of the detection
|
||||||
|
public Point getBottomRightCornerPx() {
|
||||||
|
return bottomRightPx;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,102 @@
|
||||||
|
package com.tearabite.ftctearabits.vision;
|
||||||
|
|
||||||
|
import android.graphics.Paint;
|
||||||
|
|
||||||
|
import org.opencv.core.Mat;
|
||||||
|
import org.opencv.core.MatOfInt;
|
||||||
|
import org.opencv.core.MatOfPoint;
|
||||||
|
import org.opencv.core.Point;
|
||||||
|
import org.opencv.core.Rect;
|
||||||
|
import org.opencv.core.Scalar;
|
||||||
|
import org.opencv.imgproc.Imgproc;
|
||||||
|
import org.opencv.imgproc.Moments;
|
||||||
|
|
||||||
|
import java.util.Collections;
|
||||||
|
import java.util.List;
|
||||||
|
|
||||||
|
// CV Helper Functions
|
||||||
|
public class OpenCVUtil {
|
||||||
|
|
||||||
|
public static String telem = "nothing";
|
||||||
|
|
||||||
|
// Draw a point
|
||||||
|
public static void drawPoint(Mat img, Point point, Scalar color) {
|
||||||
|
Imgproc.circle(img, point, 3, color, -1);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the center of a contour
|
||||||
|
public static Point getCenterOfContour(MatOfPoint contour) {
|
||||||
|
Moments moments = Imgproc.moments(contour);
|
||||||
|
return new Point(moments.m10 / moments.m00, moments.m01/ moments.m00);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the bottom left of a contour
|
||||||
|
public static Point getBottomLeftOfContour(MatOfPoint contour) {
|
||||||
|
Rect boundingRect = Imgproc.boundingRect(contour);
|
||||||
|
return new Point(boundingRect.x, boundingRect.y+boundingRect.height);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the bottom right of a contour
|
||||||
|
public static Point getBottomRightOfContour(MatOfPoint contour) {
|
||||||
|
Rect boundingRect = Imgproc.boundingRect(contour);
|
||||||
|
return new Point(boundingRect.x+boundingRect.width, boundingRect.y+boundingRect.height);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Draw a contour
|
||||||
|
public static void drawContour(Mat img, MatOfPoint contour, Scalar color) {
|
||||||
|
Imgproc.drawContours(img, Collections.singletonList(contour), 0, color, 2);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Draw a convex hull around a contour
|
||||||
|
public static void drawConvexHull(Mat img, MatOfPoint contour, Scalar color) {
|
||||||
|
MatOfInt hull = new MatOfInt();
|
||||||
|
Imgproc.convexHull(contour, hull);
|
||||||
|
Imgproc.drawContours(img, Collections.singletonList(convertIndexesToPoints(contour, hull)), 0, color, 2);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Draw a filled in convex hull around a contour
|
||||||
|
public static void fillConvexHull(Mat img, MatOfPoint contour, Scalar color) {
|
||||||
|
MatOfInt hull = new MatOfInt();
|
||||||
|
Imgproc.convexHull(contour, hull);
|
||||||
|
Imgproc.drawContours(img, Collections.singletonList(convertIndexesToPoints(contour, hull)), 0, color, -1);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Convert indexes to points that is used in order to draw the contours
|
||||||
|
public static MatOfPoint convertIndexesToPoints(MatOfPoint contour, MatOfInt indexes) {
|
||||||
|
int[] arrIndex = indexes.toArray();
|
||||||
|
Point[] arrContour = contour.toArray();
|
||||||
|
Point[] arrPoints = new Point[arrIndex.length];
|
||||||
|
|
||||||
|
for (int i=0;i<arrIndex.length;i++) {
|
||||||
|
arrPoints[i] = arrContour[arrIndex[i]];
|
||||||
|
}
|
||||||
|
|
||||||
|
MatOfPoint hull = new MatOfPoint();
|
||||||
|
hull.fromArray(arrPoints);
|
||||||
|
return hull;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the largest contour out of a list
|
||||||
|
public static MatOfPoint getLargestContour(List<MatOfPoint> contours) {
|
||||||
|
if (contours.size() == 0) {
|
||||||
|
return null;
|
||||||
|
}
|
||||||
|
return getLargestContours(contours, 1).get(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get the top largest contours
|
||||||
|
public static List<MatOfPoint> getLargestContours(List<MatOfPoint> contours, int numContours) {
|
||||||
|
Collections.sort(contours, (a, b) -> (int) Imgproc.contourArea(b) - (int) Imgproc.contourArea(a));
|
||||||
|
return contours.subList(0, Math.min(numContours, contours.size()));
|
||||||
|
}
|
||||||
|
|
||||||
|
public static class LinePaint extends Paint
|
||||||
|
{
|
||||||
|
public LinePaint(int color)
|
||||||
|
{
|
||||||
|
setColor(color);
|
||||||
|
setAntiAlias(true);
|
||||||
|
setStrokeCap(Paint.Cap.ROUND);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,13 @@
|
||||||
|
package com.tearabite.ftctearabits.vision;
|
||||||
|
|
||||||
|
import org.opencv.core.Scalar;
|
||||||
|
|
||||||
|
import lombok.AllArgsConstructor;
|
||||||
|
import lombok.Data;
|
||||||
|
|
||||||
|
@Data
|
||||||
|
@AllArgsConstructor
|
||||||
|
public class ScalarRange {
|
||||||
|
private Scalar upper;
|
||||||
|
private Scalar lower;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,80 @@
|
||||||
|
package com.tearabite.ftctearabits.localization;
|
||||||
|
|
||||||
|
import static org.junit.jupiter.api.Assertions.assertThrows;
|
||||||
|
import static org.junit.jupiter.api.Assertions.fail;
|
||||||
|
|
||||||
|
import com.acmerobotics.roadrunner.Pose2d;
|
||||||
|
|
||||||
|
import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||||
|
import org.firstinspires.ftc.robotcore.external.navigation.Quaternion;
|
||||||
|
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
|
||||||
|
import org.firstinspires.ftc.vision.apriltag.AprilTagMetadata;
|
||||||
|
import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc;
|
||||||
|
import org.junit.jupiter.api.Test;
|
||||||
|
import org.junit.jupiter.params.ParameterizedTest;
|
||||||
|
import org.junit.jupiter.params.provider.Arguments;
|
||||||
|
import org.junit.jupiter.params.provider.MethodSource;
|
||||||
|
|
||||||
|
import java.security.InvalidParameterException;
|
||||||
|
import java.util.stream.Stream;
|
||||||
|
|
||||||
|
class AprilTagPoseEstimatorTest {
|
||||||
|
|
||||||
|
@Test
|
||||||
|
public void estimatePose_null_throws() {
|
||||||
|
AprilTagPoseEstimator estimator = new AprilTagPoseEstimator();
|
||||||
|
assertThrows(InvalidParameterException.class, () -> estimator.estimatePose(null));
|
||||||
|
}
|
||||||
|
|
||||||
|
@ParameterizedTest
|
||||||
|
@MethodSource("provideEstimatePosTestValues")
|
||||||
|
public void estimatePos_notNull_returnsPose(AprilTagMetadata metadata, AprilTagPoseFtc poseFtc, Pose2d expectedPose) {
|
||||||
|
AprilTagPoseEstimator estimator = new AprilTagPoseEstimator(new Pose2d(-7.77, 0.505, 0));
|
||||||
|
AprilTagDetection detection = new AprilTagDetection(1, 0, 0, null, null, metadata, poseFtc, null, 0);
|
||||||
|
|
||||||
|
Pose2d estimatedPose = estimator.estimatePose(detection);
|
||||||
|
|
||||||
|
assertIsClose(estimatedPose, expectedPose);
|
||||||
|
}
|
||||||
|
|
||||||
|
private boolean isClose(double a, double b) {
|
||||||
|
return Math.abs(a - b) < 0.1;
|
||||||
|
}
|
||||||
|
|
||||||
|
private void assertIsClose(Pose2d a, Pose2d b) {
|
||||||
|
boolean isClose = isClose(a.position.x, b.position.x)
|
||||||
|
&& isClose(a.position.y, b.position.y)
|
||||||
|
&& isClose(a.heading.toDouble(), b.heading.toDouble());
|
||||||
|
|
||||||
|
if (!isClose) {
|
||||||
|
fail(String.format("Expected (%.1f, %.1f, %.1f) to be close to (%.1f, %.1f, %.1f)",
|
||||||
|
a.position.x, a.position.y, a.heading.toDouble(),
|
||||||
|
b.position.x, b.position.y, b.heading.toDouble()));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private static Stream<Arguments> provideEstimatePosTestValues() {
|
||||||
|
final AprilTagMetadata metadata = new AprilTagMetadata(
|
||||||
|
2,
|
||||||
|
"testTag",
|
||||||
|
0,
|
||||||
|
new VectorF(60.25f, 35.41f, 4f), DistanceUnit.INCH,
|
||||||
|
new Quaternion(0.3536f, -0.6124f, 0.6124f, -0.3536f, 0));
|
||||||
|
|
||||||
|
return Stream.of(
|
||||||
|
Arguments.of(
|
||||||
|
metadata,
|
||||||
|
new AprilTagPoseFtc(0, 0, 0, 0, 0, 0, 24, 0, 0),
|
||||||
|
new Pose2d(28.5, 35.9, 0)),
|
||||||
|
Arguments.of(
|
||||||
|
metadata,
|
||||||
|
new AprilTagPoseFtc(0, 0, 0, 0, 0, 0, 24, -45, 0),
|
||||||
|
new Pose2d(35.5, 18.9, 0)),
|
||||||
|
Arguments.of(
|
||||||
|
metadata,
|
||||||
|
new AprilTagPoseFtc(0, 0, 0, -45, 0, 0, 24, -45, 0),
|
||||||
|
new Pose2d(31.1, 41.3, -1))
|
||||||
|
);
|
||||||
|
}
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue