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