diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SolvePNPPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SolvePNPPipe.java index e1cfcd3e1..61a20da56 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SolvePNPPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SolvePNPPipe.java @@ -1,5 +1,12 @@ package com.chameleonvision._2.vision.pipeline.pipes; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Comparator; +import java.util.List; +import java.util.NoSuchElementException; +import java.util.stream.Collectors; + import com.chameleonvision._2.config.CameraCalibrationConfig; import com.chameleonvision._2.vision.pipeline.Pipe; import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; @@ -10,434 +17,490 @@ import edu.wpi.first.wpilibj.geometry.Translation2d; import org.apache.commons.lang3.tuple.Pair; import org.apache.commons.math3.util.FastMath; import org.opencv.calib3d.Calib3d; -import org.opencv.core.*; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.MatOfDouble; +import org.opencv.core.MatOfPoint; +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.core.Size; +import org.opencv.core.TermCriteria; import org.opencv.imgproc.Imgproc; -import java.util.*; -import java.util.stream.Collectors; +/** + * Handles detecting target corners and calculating robot-relative pose. + */ +public class SolvePNPPipe implements Pipe, Mat>, + List> { -public class SolvePNPPipe implements Pipe, Mat>, List> { + private Double tilt_angle; + private MatOfPoint3f objPointsMat = new MatOfPoint3f(); + private Mat rVec = new Mat(); + private Mat tVec = new Mat(); + private Mat rodriguez = new Mat(); + private Mat pzero_world = new Mat(); + private Mat cameraMatrix = new Mat(); + Mat rot_inv = new Mat(); + Mat kMat = new Mat(); + private MatOfDouble distortionCoefficients = new MatOfDouble(); + private List targetList = new ArrayList<>(); + Comparator leftRightComparator = Comparator.comparingDouble(point -> point.x); + Comparator verticalComparator = Comparator.comparingDouble(point -> point.y); + private double distanceDivisor = 1.0; + Mat scaledTvec = new Mat(); + MatOfPoint2f boundingBoxResultMat = new MatOfPoint2f(); + MatOfPoint2f polyOutput = new MatOfPoint2f(); + private Mat greyImg = new Mat(); + private double accuracyPercentage = 0.2; - private Double tilt_angle; - private MatOfPoint3f objPointsMat = new MatOfPoint3f(); - private Mat rVec = new Mat(); - private Mat tVec = new Mat(); - private Mat rodriguez = new Mat(); - private Mat pzero_world = new Mat(); - private Mat cameraMatrix = new Mat(); - Mat rot_inv = new Mat(); - Mat kMat = new Mat(); - private MatOfDouble distortionCoefficients = new MatOfDouble(); - private List poseList = new ArrayList<>(); - Comparator leftRightComparator = Comparator.comparingDouble(point -> point.x); - Comparator verticalComparator = Comparator.comparingDouble(point -> point.y); - private double distanceDivisor = 1.0; - Mat scaledTvec = new Mat(); - MatOfPoint2f boundingBoxResultMat = new MatOfPoint2f(); - MatOfPoint2f polyOutput = new MatOfPoint2f(); - private Mat greyImg = new Mat(); - private double accuracyPercentage = 0.2; - - public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, Rotation2d tilt) { - super(); - setCameraCoeffs(calibration); + /** + * @param settings unused :bolb: + * @param calibration the camera intrinsics and extrinsics + * @param tilt The pitch of the camera relative to horzontal. used to account for + * distances in calculate pose + */ + public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, + Rotation2d tilt) { + super(); + setCameraCoeffs(calibration); // setBoundingBoxTarget(settings.targetWidth, settings.targetHeight); - // TODO add proper year differentiation - set2020Target(true); + // TODO add proper year differentiation + set2020Target(true); - this.tilt_angle = tilt.getRadians(); + this.tilt_angle = tilt.getRadians(); + } + + public void set2020Target(boolean isHighGoal) { + if (isHighGoal) { + // tl, bl, br, tr is the order + List corners = List.of( + + new Point3(-19.625, 0, 0), + new Point3(-9.819867, -17, 0), + new Point3(9.819867, -17, 0), + new Point3(19.625, 0, 0)); + setObjectCorners(corners); + } else { + setBoundingBoxTarget(7, 11); } + } - public void set2020Target(boolean isHighGoal) { - if(isHighGoal) { - // tl, bl, br, tr is the order - List corners = List.of( + public void setBoundingBoxTarget(double targetWidth, double targetHeight) { + // order is left top, left bottom, right bottom, right top - new Point3(-19.625, 0, 0), - new Point3(-9.819867, -17, 0), - new Point3(9.819867, -17, 0), - new Point3(19.625, 0, 0)); - setObjectCorners(corners); - } else { - setBoundingBoxTarget(7, 11); - } - } + List corners = List.of( + new Point3(-targetWidth / 2.0, targetHeight / 2.0, 0.0), + new Point3(-targetWidth / 2.0, -targetHeight / 2.0, 0.0), + new Point3(targetWidth / 2.0, -targetHeight / 2.0, 0.0), + new Point3(targetWidth / 2.0, targetHeight / 2.0, 0.0) + ); + setObjectCorners(corners); + } - public void setBoundingBoxTarget(double targetWidth, double targetHeight) { - // order is left top, left bottom, right bottom, right top + public void setObjectCorners(List objectCorners) { + objPointsMat.release(); + objPointsMat = new MatOfPoint3f(); + objPointsMat.fromList(objectCorners); + } - List corners = List.of( - new Point3(-targetWidth / 2.0, targetHeight / 2.0, 0.0), - new Point3(-targetWidth / 2.0, -targetHeight / 2.0, 0.0), - new Point3(targetWidth / 2.0, -targetHeight / 2.0, 0.0), - new Point3(targetWidth / 2.0, targetHeight / 2.0, 0.0) - ); - setObjectCorners(corners); - } - - public void setObjectCorners(List objectCorners) { - objPointsMat.release(); - objPointsMat = new MatOfPoint3f(); - objPointsMat.fromList(objectCorners); - } - - public void setConfig(StandardCVPipelineSettings settings, CameraCalibrationConfig camConfig, Rotation2d tilt) { - setCameraCoeffs(camConfig); + public void setConfig(StandardCVPipelineSettings settings, CameraCalibrationConfig camConfig, + Rotation2d tilt) { + setCameraCoeffs(camConfig); // setBoundingBoxTarget(settings.targetWidth, settings.targetHeight); - // TODO add proper year differentiation - tilt_angle = tilt.getRadians(); - this.objPointsMat = settings.targetCornerMat; - this.accuracyPercentage = settings.accuracy.doubleValue(); - } + // TODO add proper year differentiation + tilt_angle = tilt.getRadians(); + this.objPointsMat = settings.targetCornerMat; + this.accuracyPercentage = settings.accuracy.doubleValue(); + } - private void setCameraCoeffs(CameraCalibrationConfig settings) { - if(settings == null) { - System.err.println("SolvePNP can only run on a calibrated resolution, and this one is not! Please calibrate to use solvePNP."); - return; - } - if(cameraMatrix != settings.getCameraMatrixAsMat()) { - cameraMatrix.release(); - settings.getCameraMatrixAsMat().copyTo(cameraMatrix); - } - if(distortionCoefficients != settings.getDistortionCoeffsAsMat()) { - distortionCoefficients.release(); - settings.getDistortionCoeffsAsMat().copyTo(distortionCoefficients); - } - this.distanceDivisor = settings.squareSize; + private void setCameraCoeffs(CameraCalibrationConfig settings) { + if (settings == null) { + System.err.println("SolvePNP can only run on a calibrated resolution, and this one is not!" + + " Please calibrate to use solvePNP."); + return; } + if (cameraMatrix != settings.getCameraMatrixAsMat()) { + cameraMatrix.release(); + settings.getCameraMatrixAsMat().copyTo(cameraMatrix); + } + if (distortionCoefficients != settings.getDistortionCoeffsAsMat()) { + distortionCoefficients.release(); + settings.getDistortionCoeffsAsMat().copyTo(distortionCoefficients); + } + this.distanceDivisor = settings.squareSize; + } - @Override - public Pair, Long> run(Pair, Mat> imageTargetPair) { - long processStartNanos = System.nanoTime(); - var targets = imageTargetPair.getLeft(); - var image = imageTargetPair.getRight(); - Imgproc.cvtColor(image, greyImg, Imgproc.COLOR_BGR2GRAY); - poseList.clear(); - for(var target: targets) { - MatOfPoint2f corners; - if(target.leftRightRotatedRect == null) { - corners = find2020VisionTarget(target, accuracyPercentage);//, imageTargetPair.getRight()); //find2020VisionTarget(target);// (target.leftRightDualTargetPair != null) ? findCorner2019(target) : findBoundingBoxCorners(target); - } else { - corners = findCorner2019(target); - } + @Override + public Pair, Long> run(Pair, Mat> imageTargetPair) { + long processStartNanos = System.nanoTime(); + var targets = imageTargetPair.getLeft(); + var image = imageTargetPair.getRight(); + Imgproc.cvtColor(image, greyImg, Imgproc.COLOR_BGR2GRAY); + targetList.clear(); + for (var target : targets) { + MatOfPoint2f corners; + // if it's a dual target use 2019, but default to 2020 + if (target.leftRightRotatedRect == null) { + corners = find2020VisionTarget(target, accuracyPercentage);//, imageTargetPair.getRight + // ()); //find2020VisionTarget(target);// (target.leftRightDualTargetPair != null) ? + // findCorner2019(target) : findBoundingBoxCorners(target); + } else { + corners = findCorner2019(target); + } // var corners = findCorner2019(target); - if(corners == null) continue; + if (corners == null) continue; -// // use best features to track -// corners = refineCornersByBestTrack(corners, greyImg, target); + // convert the corners into a Pose2d + var pose = calculatePose(corners, target); + targetList.add(pose); // TODO null check null poses. DO NOT ADD A NULL CHECK HERE, otherwise + // the order will be wrong. + } + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(targetList, processTime); + } - // refine the estimate -// corners = refineCornerEstimateSubPix(corners, greyImg); + /** + * basically we split the target's two tapes, find the min area rectangle for each, and take + * the outermost 4 corners out of the 2 rectangles + * + * @param target the target to use + * @return the 4 outermost corners. + */ + private MatOfPoint2f findCorner2019(StandardCVPipeline.TrackedTarget target) { + if (target.leftRightDualTargetPair == null) return null; - var pose = calculatePose(corners, target); - if(pose != null) poseList.add(pose); - } - long processTime = System.nanoTime() - processStartNanos; - return Pair.of(poseList, processTime); + var left = target.leftRightDualTargetPair.getLeft(); + var right = target.leftRightDualTargetPair.getRight(); + + // flip if the "left" target is to the right + if (left.x > right.x) { + var temp = left; + left = right; + right = temp; } - private MatOfPoint2f findCorner2019(StandardCVPipeline.TrackedTarget target) { - if(target.leftRightDualTargetPair == null) return null; + var points = new MatOfPoint2f(); + points.fromArray( + new Point(left.x, left.y + left.height), + new Point(left.x, left.y), + new Point(right.x + right.width, right.y), + new Point(right.x + right.width, right.y + right.height) + ); + return points; + } - var left = target.leftRightDualTargetPair.getLeft(); - var right = target.leftRightDualTargetPair.getRight(); + MatOfPoint2f target2020ResultMat = new MatOfPoint2f(); - // flip if the "left" target is to the right - if(left.x > right.x) { - var temp = left; - left = right; - right = temp; - } + private double distanceBetween(Point a, Point b) { + return FastMath.sqrt(FastMath.pow(a.x - b.x, 2) + FastMath.pow(a.y - b.y, 2)); + } - var points = new MatOfPoint2f(); - points.fromArray( - new Point(left.x, left.y + left.height), - new Point(left.x, left.y), - new Point(right.x + right.width, right.y), - new Point(right.x + right.width, right.y + right.height) - ); - return points; - } + /** + * Find the target using the outermost tape corners and a 2020 target. Uses approxPolyDP to + * approximate the target outline. + * + * @param target the target. + * @return The four outermost tape corners. + */ + private MatOfPoint2f find2020VisionTarget(StandardCVPipeline.TrackedTarget target, + double accuracyPercentage) { + if (target.rawContour.cols() < 1) return null; - MatOfPoint2f target2020ResultMat = new MatOfPoint2f(); + var centroid = target.minAreaRect.center; + Comparator distanceProvider = + Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2))); - private double distanceBetween(Point a, Point b) { - return FastMath.sqrt(FastMath.pow(a.x - b.x, 2) + FastMath.pow(a.y - b.y, 2)); - } + // algorithm from team 4915 - /** - * Find the target using the outermost tape corners and a 2020 target. - * @param target the target. - * @return The four outermost tape corners. - */ - private MatOfPoint2f find2020VisionTarget(StandardCVPipeline.TrackedTarget target, double accuracyPercentage) { - if(target.rawContour.cols() < 1) return null; + // Contour perimeter + var peri = Imgproc.arcLength(target.rawContour, true); + // approximating a shape around the contours + // Can be tuned to allow/disallow hulls + // Approx is the number of vertices + // Ramer–Douglas–Peucker algorithm + // we want a number between 0 and 0.16 out of a percentage from 0 to 100 + // so take accuracy and divide by 600 + Imgproc.approxPolyDP(target.rawContour, polyOutput, accuracyPercentage / 600.0 * peri, true); - var centroid = target.minAreaRect.center; - Comparator distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2))); - - // algorithm from team 4915 - - // Contour perimeter - var peri = Imgproc.arcLength(target.rawContour, true); - // approximating a shape around the contours - // Can be tuned to allow/disallow hulls - // Approx is the number of vertices - // Ramer–Douglas–Peucker algorithm - // we want a number between 0 and 0.16 out of a percentage from 0 to 100 - // so take accuracy and divide by 600 - Imgproc.approxPolyDP(target.rawContour, polyOutput, accuracyPercentage / 600.0 * peri, true); - - var area = Imgproc.moments(polyOutput); + var area = Imgproc.moments(polyOutput); // if (area.get_m00() < 200) { // return null; // } - var polyList = polyOutput.toList(); + var polyList = polyOutput.toList(); - polyOutput.copyTo(target.approxPoly); + polyOutput.copyTo(target.approxPoly); - // left top, left bottom, right bottom, right top - var boundingBoxCorners = findBoundingBoxCorners(target).toList(); + // left top, left bottom, right bottom, right top + var boundingBoxCorners = findBoundingBoxCorners(target).toList(); - try { + try { - // top left and top right are the poly corners closest to the bouding box tl and tr - var tl = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)))).get(); - var tr = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)))).get(); + // top left and top right are the poly corners closest to the bouding box tl and tr + var tl = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, + boundingBoxCorners.get(0)))).get(); + var tr = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, + boundingBoxCorners.get(3)))).get(); - var bl = polyList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get(); - var br = polyList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get(); + // bottom left and bottom right have to be in the correct quadrant and are the furthest + // from the center + var bl = + polyList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get(); + var br = + polyList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get(); // polyList = new ArrayList<>(polyList); // polyList.removeAll(List.of(tl, tr, bl, br)); // -// var tl2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)))).get(); -// var tr2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)))).get(); +// var tl2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> +// distanceBetween(p, boundingBoxCorners.get(0)))).get(); +// var tr2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> +// distanceBetween(p, boundingBoxCorners.get(3)))).get(); // -// var bl2 = polyList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get(); -// var br2 = polyList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get(); +// var bl2 = polyList.stream().filter(point -> point.x < centroid.x && point.y > +// centroid.y).max(distanceProvider).get(); +// var br2 = polyList.stream().filter(point -> point.x > centroid.x && point.y > +// centroid.y).max(distanceProvider).get(); - target2020ResultMat.release(); - target2020ResultMat.fromList(List.of(tl, bl, br, tr));//, tr2, br2, bl2, tl2)); + target2020ResultMat.release(); + target2020ResultMat.fromList(List.of(tl, bl, br, tr));//, tr2, br2, bl2, tl2)); - return target2020ResultMat; - } catch (NoSuchElementException e) { - return null; - } + return target2020ResultMat; + } catch (NoSuchElementException e) { + return null; + } + } + + /** + * Find the target using the outermost tape corners and a dual target. + * + * @param target the target. + * @return The four outermost tape corners. + */ + private MatOfPoint2f findDualTargetCornerMinAreaRect(StandardCVPipeline.TrackedTarget target) { + if (target.leftRightRotatedRect == null) return null; + + var centroid = target.minAreaRect.center; + Comparator distanceProvider = + Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2))); + + var left = target.leftRightRotatedRect.getLeft(); + var right = target.leftRightRotatedRect.getRight(); + + // flip if the "left" target is to the right + if (left.center.x > right.center.x) { + var temp = left; + left = right; + right = temp; } - /** - * Find the target using the outermost tape corners and a dual target. - * @param target the target. - * @return The four outermost tape corners. - */ - private MatOfPoint2f findDualTargetCornerMinAreaRect(StandardCVPipeline.TrackedTarget target) { - if(target.leftRightRotatedRect == null) return null; + var leftPoints = new Point[4]; + left.points(leftPoints); + var rightPoints = new Point[4]; + right.points(rightPoints); + ArrayList combinedList = new ArrayList<>(List.of(leftPoints)); + combinedList.addAll(List.of(rightPoints)); - var centroid = target.minAreaRect.center; - Comparator distanceProvider = Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2))); + // start looking in the top left quadrant + var tl = + combinedList.stream().filter(point -> point.x < centroid.x && point.y < centroid.y).max(distanceProvider).get(); + var tr = + combinedList.stream().filter(point -> point.x > centroid.x && point.y < centroid.y).max(distanceProvider).get(); + var bl = + combinedList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get(); + var br = + combinedList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get(); - var left = target.leftRightRotatedRect.getLeft(); - var right = target.leftRightRotatedRect.getRight(); + boundingBoxResultMat.release(); + boundingBoxResultMat.fromList(List.of(tl, bl, br, tr)); - // flip if the "left" target is to the right - if(left.center.x > right.center.x) { - var temp = left; - left = right; - right = temp; - } + return boundingBoxResultMat; + } - var leftPoints = new Point[4]; - left.points(leftPoints); - var rightPoints = new Point[4]; - right.points(rightPoints); - ArrayList combinedList = new ArrayList<>(List.of(leftPoints)); - combinedList.addAll(List.of(rightPoints)); - - // start looking in the top left quadrant - var tl = combinedList.stream().filter(point -> point.x < centroid.x && point.y < centroid.y).max(distanceProvider).get(); - var tr = combinedList.stream().filter(point -> point.x > centroid.x && point.y < centroid.y).max(distanceProvider).get(); - var bl = combinedList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get(); - var br = combinedList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get(); - - boundingBoxResultMat.release(); - boundingBoxResultMat.fromList(List.of(tl, bl, br, tr)); - - return boundingBoxResultMat; - } - - /** - * - * @param target the target to find the corners of. - * @return the corners. left top, left bottom, right bottom, right top - */ - private MatOfPoint2f findBoundingBoxCorners(StandardCVPipeline.TrackedTarget target) { + /** + * @param target the target to find the corners of. + * @return the corners. left top, left bottom, right bottom, right top + */ + private MatOfPoint2f findBoundingBoxCorners(StandardCVPipeline.TrackedTarget target) { // List> list = new ArrayList<>(); // // find the corners based on the bounding box // // order is left top, left bottom, right bottom, right top - // extract the corners - var points = new Point[4]; - target.minAreaRect.points(points); + // extract the corners + var points = new Point[4]; + target.minAreaRect.points(points); - // find the tl/tr/bl/br corners - // first, min by left/right - var list_ = Arrays.asList(points); - list_.sort(leftRightComparator); - // of this, we now have left and right - // sort to get top and bottom - var left = new ArrayList<>(List.of(list_.get(0), list_.get(1))); - left.sort(verticalComparator); - var right = new ArrayList<>(List.of(list_.get(2), list_.get(3))); - right.sort(verticalComparator); + // find the tl/tr/bl/br corners + // first, min by left/right + var list_ = Arrays.asList(points); + list_.sort(leftRightComparator); + // of this, we now have left and right + // sort to get top and bottom + var left = new ArrayList<>(List.of(list_.get(0), list_.get(1))); + left.sort(verticalComparator); + var right = new ArrayList<>(List.of(list_.get(2), list_.get(3))); + right.sort(verticalComparator); - // tl tr bl br - var tl = left.get(0); - var bl = left.get(1); - var tr = right.get(0); - var br = right.get(1); + // tl tr bl br + var tl = left.get(0); + var bl = left.get(1); + var tr = right.get(0); + var br = right.get(1); - boundingBoxResultMat.release(); - boundingBoxResultMat.fromList(List.of(tl, bl, br, tr)); + boundingBoxResultMat.release(); + boundingBoxResultMat.fromList(List.of(tl, bl, br, tr)); - return boundingBoxResultMat; + return boundingBoxResultMat; + } + + MatOfPoint2f goodFeatureToTrackRetval = new MatOfPoint2f(); + + private MatOfPoint2f refineCornersByBestTrack(MatOfPoint2f corners, Mat greyImg, + StandardCVPipeline.TrackedTarget target) { + + MatOfPoint approxf1 = new MatOfPoint(); + var origCornerList = new ArrayList<>(corners.toList()); + approxf1.fromList(origCornerList.stream() + .map(it -> new Point(it.x - target.boundingRect.x, it.y - target.boundingRect.y)) + .collect(Collectors.toList()) + ); + var croppedImage = greyImg.submat(target.boundingRect); + + Imgproc.goodFeaturesToTrack(croppedImage, approxf1, 0, 0.1, 5); + + // at this point corners is still unmodified so let's map it + List tempList = new ArrayList<>(); + + // shift all points back into global pose + var reshiftedList = + approxf1.toList().stream().map(it -> new Point(it.x + target.boundingRect.x, + it.y + target.boundingRect.y)) + .collect(Collectors.toList()); + for (Point p : origCornerList) { + // find the goodFeaturesToTrack corner closest to me + var closestPoint = + reshiftedList.stream().min(Comparator.comparingDouble(p_ -> distanceBetween(p_, p))); + if (closestPoint.isEmpty()) { + tempList.add(p); + reshiftedList.remove(p); + } else { + tempList.add(closestPoint.get()); + reshiftedList.remove(closestPoint.get()); + } } - MatOfPoint2f goodFeatureToTrackRetval = new MatOfPoint2f(); + goodFeatureToTrackRetval.fromList(tempList); + return goodFeatureToTrackRetval; + } - private MatOfPoint2f refineCornersByBestTrack(MatOfPoint2f corners, Mat greyImg, StandardCVPipeline.TrackedTarget target) { + // Set the needed parameters to find the refined corners + Size winSize = new Size(4, 4); + Size zeroZone = new Size(-1, -1); // we don't need a zero zone + TermCriteria criteria = new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 90, 0.001); - MatOfPoint approxf1 = new MatOfPoint(); - var origCornerList = new ArrayList<>(corners.toList()); - approxf1.fromList(origCornerList.stream() - .map(it -> new Point(it.x - target.boundingRect.x, it.y - target.boundingRect.y)) - .collect(Collectors.toList()) - ); - var croppedImage = greyImg.submat(target.boundingRect); + private boolean shouldRefineCorners = true; - Imgproc.goodFeaturesToTrack(croppedImage, approxf1, 0, 0.1, 5); + /** + * Refine an estimated corner position using the cornerSubPixel algorithm. + *

+ * TODO should this be here or before the points are chosen? + * + * @param corners the corners detected -- this mat is modified! + * @param greyImg the image taken by the camera as color + * @return the updated mat, same as the corner mat passed in. + */ + private MatOfPoint2f refineCornerEstimateSubPix(MatOfPoint2f corners, Mat greyImg) { + if (!shouldRefineCorners) return corners; // just return + Imgproc.cornerSubPix(greyImg, corners, winSize, zeroZone, criteria); - // at this point corners is still unmodified so let's map it - List tempList = new ArrayList<>(); + return corners; + } - // shift all points back into global pose - var reshiftedList = approxf1.toList().stream().map(it -> new Point(it.x + target.boundingRect.x, it.y + target.boundingRect.y)) - .collect(Collectors.toList()); - for(Point p: origCornerList) { - // find the goodFeaturesToTrack corner closest to me - var closestPoint = reshiftedList.stream().min(Comparator.comparingDouble(p_ -> distanceBetween(p_, p))); - if(closestPoint.isEmpty()) { - tempList.add(p); - reshiftedList.remove(p); - } else { - tempList.add(closestPoint.get()); - reshiftedList.remove(closestPoint.get()); - } - } +// NetworkTableEntry tvecE = NetworkTableInstance.getDefault().getTable("SmartDashboard") +// .getEntry("tvec"); +// NetworkTableEntry rvecE = NetworkTableInstance.getDefault().getTable("SmartDashboard") +// .getEntry("rvec"); - goodFeatureToTrackRetval.fromList(tempList); - return goodFeatureToTrackRetval; + /** + * Calculate the pose of the vision target + * + * @param imageCornerPoints the corners we found. + * @param target the target to process, mutated. + * @return the target, with the pose2d added to it. + */ + public StandardCVPipeline.TrackedTarget calculatePose(MatOfPoint2f imageCornerPoints, + StandardCVPipeline.TrackedTarget target) { + if (objPointsMat.rows() != imageCornerPoints.rows() || cameraMatrix.rows() < 2 || distortionCoefficients.cols() < 4) { + System.err.println("can't do solvePNP with invalid params!"); + return null; } - // Set the needed parameters to find the refined corners - Size winSize = new Size(4, 4); - Size zeroZone = new Size(-1, -1); // we don't need a zero zone - TermCriteria criteria = new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 90, 0.001); + imageCornerPoints.copyTo(target.imageCornerPoints); - private boolean shouldRefineCorners = true; - - /** - * Refine an estimated corner position using the cornerSubPixel algorithm. - * - * TODO should this be here or before the points are chosen? - * - * @param corners the corners detected -- this mat is modified! - * @param greyImg the image taken by the camera as color - * @return the updated mat, same as the corner mat passed in. - */ - private MatOfPoint2f refineCornerEstimateSubPix(MatOfPoint2f corners, Mat greyImg) { - if(!shouldRefineCorners) return corners; // just return - Imgproc.cornerSubPix(greyImg, corners, winSize, zeroZone, criteria); - - return corners; + try { + Calib3d.solvePnP(objPointsMat, imageCornerPoints, cameraMatrix, distortionCoefficients, + rVec, tVec); + } catch (Exception e) { + e.printStackTrace(); + return null; } -// NetworkTableEntry tvecE = NetworkTableInstance.getDefault().getTable("SmartDashboard").getEntry("tvec"); -// NetworkTableEntry rvecE = NetworkTableInstance.getDefault().getTable("SmartDashboard").getEntry("rvec"); - - public StandardCVPipeline.TrackedTarget calculatePose(MatOfPoint2f imageCornerPoints, StandardCVPipeline.TrackedTarget target) { - if(objPointsMat.rows() != imageCornerPoints.rows() || cameraMatrix.rows() < 2 || distortionCoefficients.cols() < 4) { - System.err.println("can't do solvePNP with invalid params!"); - return null; - } - - imageCornerPoints.copyTo(target.imageCornerPoints); - - try { - Calib3d.solvePnP(objPointsMat, imageCornerPoints, cameraMatrix, distortionCoefficients, rVec, tVec); - } catch (Exception e) { - e.printStackTrace(); - return null; - } - // tvecE.setString(tVec.dump()); // rvecE.setString(rVec.dump()); - // Algorithm from team 5190 Green Hope Falcons + // Algorithm from team 5190 Green Hope Falcons. Can also be found in Ligerbot's vision + // whitepaper -// var tilt_angle = 0.0; // TODO add to settings + // the left/right distance to the target, unchanged by tilt. Inches + var x = tVec.get(0, 0)[0]; - // the left/right distance to the target, unchanged by tilt - var x = tVec.get(0, 0)[0]; + // Z distance in the flat plane is given by + // Z_field = z cos theta + y sin theta. + // Z is the distance "out" of the camera (straight forward). Inches. + var z = tVec.get(2, 0)[0] * FastMath.cos(tilt_angle) + tVec.get(1, 0)[0] * FastMath.sin(tilt_angle); - // Z distance in the flat plane is given by - // Z_field = z cos theta + y sin theta - var z = tVec.get(2, 0)[0] * FastMath.cos(tilt_angle) + tVec.get(1, 0)[0] * FastMath.sin(tilt_angle); + Calib3d.Rodrigues(rVec, rodriguez); + Core.transpose(rodriguez, rot_inv); // rodrigurz.t() - // find skew of the target relative to the camera - // from ligerbots: - // rot, _ = cv2.Rodrigues(rvec) - // rot_inv = rot.transpose() - // pzero_world = numpy.matmul(rot_inv, -tvec) - // angle2 = math.atan2(pzero_world[0][0], pzero_world[2][0] + scaledTvec = matScale(tVec, -1); + Core.gemm(rot_inv, scaledTvec, 1, kMat, 0, pzero_world); - Calib3d.Rodrigues(rVec, rodriguez); - Core.transpose(rodriguez, rot_inv); // rodrigurz.t() + var angle2 = FastMath.atan2(pzero_world.get(0, 0)[0], pzero_world.get(2, 0)[0]); - scaledTvec = matScale(tVec, -1); - Core.gemm(rot_inv, scaledTvec, 1, kMat, 0, pzero_world); + // target rotation is the rotation of the target relative to straight ahead. this number + // should be unchanged if the robot purely translated left/right. + var targetRotation = -angle2; // radians - var angle2 = FastMath.atan2(pzero_world.get(0, 0)[0], pzero_world.get(2, 0)[0]); + // We want a vector that is X forward and Y left. + // We have a Z_field (out of the camera projected onto the field), and an X left/right. + // so Z_field becomes X, and X becomes Y - var targetRotation = -angle2; // radians + //noinspection SuspiciousNameCombination + var targetLocation = new Translation2d(z, -x).times(25.4 / 1000d / distanceDivisor); + target.cameraRelativePose = new Pose2d(targetLocation, new Rotation2d(targetRotation)); + target.rVector = rVec; + target.tVector = tVec; - // We want a vector that is X forward and Y left. - // We have a Z_field (out of the camera projected onto the field), and an X left/right. - // so Z_field becomes X, and X becomes Y + return target; + } - //noinspection SuspiciousNameCombination - var targetLocation = new Translation2d(z, -x).times(25.4 / 1000d / distanceDivisor); - target.cameraRelativePose = new Pose2d(targetLocation, new Rotation2d(targetRotation)); - target.rVector = rVec; - target.tVector = tVec; - - return target; - } - - /** - * Element-wise scale a matrix by a given factor - * @param src the source matrix - * @param factor by how much to scale each element - * @return the scaled matrix - */ - public Mat matScale(Mat src, double factor) { - Mat dst = new Mat(src.rows(),src.cols(),src.type()); - Scalar s = new Scalar(factor); // TODO check if we need to add more elements to this - Core.multiply(src, s, dst); - return dst; - } + /** + * Element-wise scale a matrix by a given factor + * + * @param src the source matrix + * @param factor by how much to scale each element + * @return the scaled matrix + */ + public Mat matScale(Mat src, double factor) { + Mat dst = new Mat(src.rows(), src.cols(), src.type()); + Scalar s = new Scalar(factor); // TODO check if we need to add more elements to this + Core.multiply(src, s, dst); + return dst; + } }