From 5cc735ecf74a6670192a4bf694075b38abf67d95 Mon Sep 17 00:00:00 2001 From: astatin3 Date: Sun, 15 Dec 2024 15:54:25 -0700 Subject: [PATCH] Add VO - Needs debugging --- README.md | 4 +- .../dashboard/tabs/CustomTestTab.vue | 64 ++-- photon-client/src/types/PipelineTypes.ts | 42 ++- .../pipe/impl/VisualOdometryParams.java | 50 +++ .../vision/pipe/impl/VisualOdometryPipe.java | 246 ++++++++++++++ .../vision/pipeline/CustomTestPipeline.java | 316 +++++++++++++----- .../pipeline/CustomTestPipelineSettings.java | 76 ++++- 7 files changed, 672 insertions(+), 126 deletions(-) create mode 100644 photon-core/src/main/java/org/photonvision/vision/pipe/impl/VisualOdometryParams.java create mode 100644 photon-core/src/main/java/org/photonvision/vision/pipe/impl/VisualOdometryPipe.java diff --git a/README.md b/README.md index c0e4f0f..41be519 100644 --- a/README.md +++ b/README.md @@ -3,8 +3,8 @@ This is a project that aims to improve FRC vision accuracy using Visual Odometry Things to accomplish: - [x] Get custom pipeline running -- [ ] Basic VO (look at https://github.com/thehummingbird/robotics_demos/blob/main/monocular_vo) +- [needs debugging] Basic VO (look at https://github.com/thehummingbird/robotics_demos/blob/main/monocular_vo) - [ ] Inertial odometry using pidgen and sensor fusion (look at https://www.thinkautonomous.ai/blog/visual-inertial-odometry/) -- [ ] Sensor fusion for physical odometry from RoboRiO +- [?] Sensor fusion for physical odometry from RoboRiO - [ ] Mesh based localisation? https://youtube.com/watch?v=vMsV04emXHU https://dlnext.acm.org/doi/10.1007/978-3-031-20047-2_34 - [ ] Optimise and refine. \ No newline at end of file diff --git a/photon-client/src/components/dashboard/tabs/CustomTestTab.vue b/photon-client/src/components/dashboard/tabs/CustomTestTab.vue index b9ad1cc..d29180b 100644 --- a/photon-client/src/components/dashboard/tabs/CustomTestTab.vue +++ b/photon-client/src/components/dashboard/tabs/CustomTestTab.vue @@ -25,37 +25,59 @@ const interactiveCols = computed(() => diff --git a/photon-client/src/types/PipelineTypes.ts b/photon-client/src/types/PipelineTypes.ts index 96f464f..b27fad4 100644 --- a/photon-client/src/types/PipelineTypes.ts +++ b/photon-client/src/types/PipelineTypes.ts @@ -319,9 +319,25 @@ export const DefaultObjectDetectionPipelineSettings: ObjectDetectionPipelineSett export interface CustomTestPipelineSettings extends PipelineSettings { pipelineType: PipelineType.CustomTest; - test1: number; - test2: number; - test3: number; + + featureThreshold: number + minFeatures: number, + imageDifferenceThreshold: number, + essentialMatProb: number + essentialMatThreshold: number, + + hammingDist: number; + numIterations: number; + decimate: number; + blur: number; + decisionMargin: number; + refineEdges: boolean; + debug: boolean; + threads: number; + tagFamily: AprilTagFamily; + doMultiTarget: boolean; + doSingleTargetAlways: boolean; + } export type ConfigurableCustomTestPipelineSettings = Partial< Omit @@ -337,10 +353,24 @@ export const DefaultCustomTestPipelineSettings: CustomTestPipelineSettings = { cameraAutoExposure: true, ledMode: false, + featureThreshold: 1, + minFeatures: 500, + imageDifferenceThreshold: 150, + essentialMatProb: 0.999, + essentialMatThreshold: 1., - test1: 1, - test2: 2, - test3: 3 + + hammingDist: 0, + numIterations: 40, + decimate: 1, + blur: 0, + decisionMargin: 35, + refineEdges: true, + debug: false, + threads: 4, + tagFamily: AprilTagFamily.Family36h11, + doMultiTarget: false, + doSingleTargetAlways: false }; export interface Calibration3dPipelineSettings extends PipelineSettings { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/VisualOdometryParams.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/VisualOdometryParams.java new file mode 100644 index 0000000..57cb45f --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/VisualOdometryParams.java @@ -0,0 +1,50 @@ +package org.photonvision.vision.pipe.impl; + +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.photonvision.vision.pipeline.CustomTestPipelineSettings; +import org.photonvision.vision.pipeline.PipelineType; + +public class VisualOdometryParams { + public int featureThreshold = 1; + public int minFeatures = 500; + public int imageDifferenceThreshold = 150; + public double essentialMatProb = 0.999; + public double essentialMatThreshold = 1.; + public double focal = 1.; + public Point pp = new Point(); + public Mat cam_mat = new Mat(); + + @Override + public int hashCode() { + final int prime = 31; + int result = super.hashCode(); + long temp; + + temp = Double.doubleToLongBits(featureThreshold); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(minFeatures); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(imageDifferenceThreshold); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(essentialMatProb); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(essentialMatThreshold); + result = prime * result + (int) (temp ^ (temp >>> 32)); + + return result; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) return true; + if (!super.equals(obj)) return false; + if (getClass() != obj.getClass()) return false; + VisualOdometryParams other = (VisualOdometryParams) obj; + if(featureThreshold != other.featureThreshold) return false; + if(minFeatures != other.minFeatures) return false; + if(essentialMatProb != other.essentialMatProb) return false; + if(essentialMatThreshold != other.essentialMatThreshold) return false; + return true; + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/VisualOdometryPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/VisualOdometryPipe.java new file mode 100644 index 0000000..72378c2 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/VisualOdometryPipe.java @@ -0,0 +1,246 @@ +package org.photonvision.vision.pipe.impl; + +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import org.opencv.core.*; +import org.opencv.features2d.FastFeatureDetector; +import org.opencv.imgproc.Imgproc; +import org.photonvision.vision.pipe.CVPipe; + +import java.util.ArrayList; +import java.util.LinkedList; +import java.util.List; + +import static org.opencv.calib3d.Calib3d.*; +import static org.opencv.core.Core.meanStdDev; +import static org.opencv.features2d.Features2d.drawKeypoints; +import static org.opencv.video.Video.calcOpticalFlowPyrLK; + +public class VisualOdometryPipe extends CVPipe{ + private static final Scalar WHITE = new Scalar(255, 255, 255); + private static final Scalar BLUE = new Scalar(255, 0, 0); + private static final Scalar RED = new Scalar(0, 0, 255); + public boolean hasReset = false; + + FastFeatureDetector fast = FastFeatureDetector.create(); + + + MatOfByte status = new MatOfByte(); + + Mat E = new Mat(); + Mat R = new Mat(); + Mat t = new Mat(); + +// Mat R_f = new Mat(); +// Mat t_f = new Mat(); + + Mat prevImage = new Mat(); + MatOfPoint2f prevFeatures = new MatOfPoint2f(); + MatOfPoint2f currFeatures = new MatOfPoint2f(); + + List currFeaturesList, prevFeaturesList; + +// private Transform3d lastPosition = new Transform3d(); + + public void release() { + status.release(); + E.release(); + R.release(); + t.release(); + prevImage.release(); + prevFeatures.release(); + currFeatures.release();; + currFeatures = null; + prevFeatures = null; + } + + @Override + protected Transform3d process(Mat frame_mat) { + Transform3d tf = estimate(frame_mat); + + if(keypoints != null) + drawKeypoints(frame_mat, keypoints, frame_mat, RED); + + if(currFeaturesList != null && prevFeaturesList != null) + for(int i = 0; i < currFeaturesList.size(); i++){ + Point curPoint = currFeaturesList.get(i); + Point lastPoint = prevFeaturesList.get(i); + + Imgproc.circle(frame_mat, curPoint, 5, WHITE, 1); + Imgproc.line(frame_mat, curPoint, lastPoint, BLUE, 1); + } + + if(tf != null) { + return tf; + } + return new Transform3d(); + } + + + private double featureTracking(Mat prevImage, Mat currImage, MatOfPoint2f prevFeatures, MatOfPoint2f currFeatures, MatOfByte status){ + // 트래킹에 실패한 포인트들은 버린다. + MatOfFloat err = new MatOfFloat(); + Size winSize = new Size(21, 21); + TermCriteria termcrit = new TermCriteria(TermCriteria.COUNT+TermCriteria.EPS, 30, 0.01); + + calcOpticalFlowPyrLK(prevImage, currImage, prevFeatures, currFeatures, status, err, winSize, 3, termcrit, 0, 0.01); + + double weight = 0; + // KLT 트래킹에 실패하거나 프레임 바깥으로 벗어난 포인트들은 버린다. + int indexCorrection = 0; + byte[] statusArray = status.toArray(); + Point[] currFeaturesArray = currFeatures.toArray(); + Point[] prevFeaturesArray = prevFeatures.toArray(); + + currFeaturesList = new LinkedList<>(currFeatures.toList()); + prevFeaturesList = new LinkedList<>(prevFeatures.toList()); + + for(int i = 0; i < statusArray.length; i++){ + Point pt = currFeaturesArray[i]; + + if((statusArray[i] == 0) || (pt.x < 0) || (pt.y < 0)){ + prevFeaturesList.remove(i - indexCorrection); + currFeaturesList.remove(i - indexCorrection); + + indexCorrection++; + } else { + Point before = prevFeaturesList.get(i - indexCorrection); + Point after = currFeaturesList.get(i - indexCorrection); + weight += (after.x - before.x) * (after.x - before.x) + (after.y - before.y)*(after.y - before.y); +// Point before = prevFeaturesArray[i - indexCorrection]; +// weight += ((int)pt.x - before.x)*((int)pt.x - before.x) + ((int)pt.y - before.y)*((int)pt.y - before.y); + } + } + + // currFeatures, prevFeatures를 필터한 특징점으로 교체함 + currFeatures.fromList(currFeaturesList); + prevFeatures.fromList(prevFeaturesList); + + if(prevFeaturesArray.length == 0) + return 0; + return weight / prevFeaturesArray.length; + } + + MatOfKeyPoint keypoints; + + private MatOfPoint2f featureDetection(Mat image) { +// System.out.println("New points!"); + keypoints = new MatOfKeyPoint(); + + fast.detect(image, keypoints); + + KeyPoint[] kps = keypoints.toArray(); + ArrayList arrayOfPoints = new ArrayList<>(); + + for(int i = 0; i < kps.length; i++){ + arrayOfPoints.add(kps[i].pt); + } + + MatOfPoint2f matOfPoint = new MatOfPoint2f(); + matOfPoint.fromList(arrayOfPoints); + + return matOfPoint; + } + + private Transform3d estimate(Mat currImage) { + Point[] prevFeaturesArray = prevFeatures.toArray(); + + if(prevImage.empty()){ + prevImage = currImage.clone(); + prevFeatures = featureDetection(prevImage); + System.out.println(4); + return null; + } else if (prevFeaturesArray.length < params.minFeatures) { + prevFeatures = featureDetection(prevImage); + prevImage = currImage.clone(); +// if(prevFeaturesArray.length <= 0){ +// System.out.println("Can't detect features."); +// } + System.out.println(3); + return null; + } + + double weight = featureTracking(prevImage, currImage, prevFeatures, currFeatures, status); + + if(prevFeaturesArray.length <= 0){ + prevFeatures = featureDetection(currImage); + prevImage = currImage.clone(); + System.out.println(2); + return null; + } + + if(weight < params.imageDifferenceThreshold) { + prevImage = currImage.clone(); + currFeatures.copyTo(prevFeatures); + System.out.println(1); + return null; + } + + try { + E = findEssentialMat(prevFeatures, currFeatures, params.cam_mat);// params.focal, params.pp);//, RANSAC, params.essentialMatProb, params.essentialMatThreshold); +// System.out.println(E.size()); + + double sum =0; + for(int x =0; x { private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.GREYSCALE; private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe(); - private final BlurDetectionPipe blurDetectionPipe = new BlurDetectionPipe(); - private final BlurPipe blurPipe = new BlurPipe(); + private final AprilTagDetectionPipe aprilTagDetectionPipe = new AprilTagDetectionPipe(); + private final AprilTagPoseEstimatorPipe singleTagPoseEstimatorPipe = new AprilTagPoseEstimatorPipe(); + private final MultiTargetPNPPipe multiTagPNPPipe = new MultiTargetPNPPipe(); + private final VisualOdometryPipe visualOdometryPipe = new VisualOdometryPipe(); + + private List previousAprilTags; + + Mat cam_mat; public CustomTestPipeline() { super(PROCESSING_TYPE); settings = new CustomTestPipelineSettings(); @@ -45,27 +55,101 @@ public class CustomTestPipeline extends CVPipeline 1 + quadParams.minClusterPixels = 5; + // these are the same as the values in WPILib 2025 + // setting them here to prevent upstream changes from changing behavior of the detector + quadParams.maxNumMaxima = 10; + quadParams.criticalAngle = 45 * Math.PI / 180.0; + quadParams.maxLineFitMSE = 10.0f; + quadParams.minWhiteBlackDiff = 5; + quadParams.deglitch = false; + + aprilTagDetectionPipe.setParams( + new AprilTagDetectionPipeParams(settings.tagFamily, config, quadParams)); + + if (frameStaticProperties.cameraCalibration != null) { + var cameraMatrix = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat(); + if (cameraMatrix != null && cameraMatrix.rows() > 0) { + var cx = cameraMatrix.get(0, 2)[0]; + var cy = cameraMatrix.get(1, 2)[0]; + var fx = cameraMatrix.get(0, 0)[0]; + var fy = cameraMatrix.get(1, 1)[0]; + + singleTagPoseEstimatorPipe.setParams( + new AprilTagPoseEstimatorPipe.AprilTagPoseEstimatorPipeParams( + new AprilTagPoseEstimator.Config(tagWidth, fx, fy, cx, cy), + frameStaticProperties.cameraCalibration, + settings.numIterations)); + + // TODO global state ew + var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout(); + multiTagPNPPipe.setParams( + new MultiTargetPNPPipe.MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl, tagModel)); + } + } } private static final Scalar FONT_COLOR = ColorHelper.colorToScalar(Color.RED); - AKAZE detector = AKAZE.create(); - - MatOfKeyPoint prev_points; - @Override protected CVPipelineResult process(Frame frame, CustomTestPipelineSettings settings) { - long total_proc_time = 0; - - String status = "ERROR"; + long sumPipeNanosElapsed = 0L; if (frame.type != FrameThresholdType.GREYSCALE) { // We asked for a GREYSCALE frame, but didn't get one -- best we can do is give up @@ -79,86 +163,154 @@ public class CustomTestPipeline extends CVPipeline 0 && prev_points.depth() > 0){ -// System.out.println("Calc!"); - Mat cam_mat = frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat(); - Mat E, R = new Mat(), t = new Mat(), mask = new Mat(); - - int npoints = points.checkVector(2); - - System.out.println("npoints >= 0" + (npoints >= 0)); - System.out.println("points2.checkVector(2) == npoints" + (prev_points.checkVector(2) == npoints)); - System.out.println("points1.type() == points2.type()" + (points.type() == prev_points.type())); - E = findEssentialMat(prev_points, points, cam_mat, Calib3d.RANSAC); - - -// recoverPose(E, points, prev_points, cam_mat, R, t, mask); +// Transform3d tf = visualOdometryPipe.run(frame.processedImage.getMat()).output; // +// var fps = calculateFPSPipe.run(null).output; // +// List result = new ArrayList<>(); // +// TrackedTarget target = new TrackedTarget(); +// target.setBestCameraToTarget3d(tf); +//// target.tran // -// System.out.println(t.width() + ", " + t.height()); -//// System.out.println(pos.get(0,3)[0] + ", " + pos.get(1,3)[0] + ", " + pos.get(2,3)[0]); +// result.add(target); +// +// return new CVPipelineResult(frame.sequenceID, total_proc_time, fps, result, frame); + + CVPipe.CVPipeResult> tagDetectionPipeResult; + tagDetectionPipeResult = aprilTagDetectionPipe.run(frame.processedImage); + sumPipeNanosElapsed += tagDetectionPipeResult.nanosElapsed; + + List detections = tagDetectionPipeResult.output; + List usedDetections = new ArrayList<>(); + List targetList = new ArrayList<>(); + + // Filter out detections based on pipeline settings + for (AprilTagDetection detection : detections) { + // TODO this should be in a pipe, not in the top level here (Matt) + if (detection.getDecisionMargin() < settings.decisionMargin) continue; + if (detection.getHamming() > settings.hammingDist) continue; + + usedDetections.add(detection); + + // Populate target list for multitag + // (TODO: Address circular dependencies. Multitag only requires corners and IDs, this should + // not be necessary.) + TrackedTarget target = + new TrackedTarget( + detection, + null, + new TrackedTarget.TargetCalculationParameters( + false, null, null, null, null, frameStaticProperties)); + + targetList.add(target); } - prev_points = points; + // Do multi-tag pose estimation + Optional multiTagResult = Optional.empty(); + if (settings.solvePNPEnabled && settings.doMultiTarget) { + var multiTagOutput = multiTagPNPPipe.run(targetList); + sumPipeNanosElapsed += multiTagOutput.nanosElapsed; + multiTagResult = multiTagOutput.output; + } + // Do single-tag pose estimation + if (settings.solvePNPEnabled) { + // Clear target list that was used for multitag so we can add target transforms + targetList.clear(); + // TODO global state again ew + var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout(); - drawKeypoints(frame_mat, points, frame_mat); + for (AprilTagDetection detection : usedDetections) { + AprilTagPoseEstimate tagPoseEstimate = null; + // Do single-tag estimation when "always enabled" or if a tag was not used for multitag + if (settings.doSingleTargetAlways + || !(multiTagResult.isPresent() + && multiTagResult.get().fiducialIDsUsed.contains((short) detection.getId()))) { + var poseResult = singleTagPoseEstimatorPipe.run(detection); + sumPipeNanosElapsed += poseResult.nanosElapsed; + tagPoseEstimate = poseResult.output; + } -// Transform3d pos = + // If single-tag estimation was not done, this is a multi-target tag from the layout + if (tagPoseEstimate == null && multiTagResult.isPresent()) { + // compute this tag's camera-to-tag transform using the multitag result + var tagPose = atfl.getTagPose(detection.getId()); + if (tagPose.isPresent()) { + var camToTag = + new Transform3d( + new Pose3d().plus(multiTagResult.get().estimatedPose.best), tagPose.get()); + // match expected AprilTag coordinate system + camToTag = + CoordinateSystem.convert(camToTag, CoordinateSystem.NWU(), CoordinateSystem.EDN()); + // (AprilTag expects Z axis going into tag) + camToTag = + new Transform3d( + camToTag.getTranslation(), + new Rotation3d(0, Math.PI, 0).plus(camToTag.getRotation())); + tagPoseEstimate = new AprilTagPoseEstimate(camToTag, camToTag, 0, 0); + } + } + // populate the target list + // Challenge here is that TrackedTarget functions with OpenCV Contour + TrackedTarget target = + new TrackedTarget( + detection, + tagPoseEstimate, + new TrackedTarget.TargetCalculationParameters( + false, null, null, null, null, frameStaticProperties)); - var fps = calculateFPSPipe.run(null).output; + var correctedBestPose = + MathUtils.convertOpenCVtoPhotonTransform(target.getBestCameraToTarget3d()); + var correctedAltPose = + MathUtils.convertOpenCVtoPhotonTransform(target.getAltCameraToTarget3d()); - return new CVPipelineResult(frame.sequenceID, total_proc_time, fps, List.of(), frame); - } + target.setBestCameraToTarget3d( + new Transform3d(correctedBestPose.getTranslation(), correctedBestPose.getRotation())); + target.setAltCameraToTarget3d( + new Transform3d(correctedAltPose.getTranslation(), correctedAltPose.getRotation())); - public void featureTracking(Mat img_1, Mat img_2, MatOfPoint2f points1, MatOfPoint2f points2, MatOfByte status) { + targetList.add(target); + } + } - // this function automatically gets rid of points for which tracking fails + if(!targetList.isEmpty()) { + previousAprilTags = targetList; + visualOdometryPipe.hasReset = false; + } else if(previousAprilTags != null) { + if(!visualOdometryPipe.hasReset){ + visualOdometryPipe.reset(); + visualOdometryPipe.hasReset = true; + } - MatOfFloat err = new MatOfFloat(); - Size window_size = new Size(21, 21); - TermCriteria term_criteria = new TermCriteria(TermCriteria.COUNT + TermCriteria.EPS, 30, 0.01); + CVPipe.CVPipeResult VOResult = visualOdometryPipe.run(frame.processedImage.getMat()); + sumPipeNanosElapsed += VOResult.nanosElapsed; - calcOpticalFlowPyrLK(img_1, img_2, points1, points2, status, err, window_size, 3, term_criteria, 0, 0.001); +// System.out.println("X: " + VOResult.output.getTranslation()); - // getting rid of points for which the KLT tracking failed or those who have gone outside the frame -// int index_correction = 0; -// for (int i = 0; i < status.depth(); i++) { -// Point pt = points2.toArray()[i - index_correction]; -// if ((status.toArray()[i] == 0) || (pt.x < 0) || (pt.y < 0)) -// { -// if ((pt.x < 0) || (pt.y < 0)) -// { -// status.toArray()[i] = 0; -// } -// points1 -// points1.erase(points1.begin() + (i - index_correction)); -// points2.erase(points2.begin() + (i - index_correction)); -// index_correction++; -// } -// } -} + for(int i = 0; i < previousAprilTags.size(); i++){ + TrackedTarget old = previousAprilTags.get(i); + old.setBestCameraToTarget3d(old.getBestCameraToTarget3d().plus(VOResult.output)); + + } + + } + + var fpsResult = calculateFPSPipe.run(null); + var fps = fpsResult.output; + + return new CVPipelineResult( + frame.sequenceID, sumPipeNanosElapsed, fps, previousAprilTags, frame); - void featureDetection(Mat img_1, MatOfKeyPoint points1) { - // uses FAST for feature detections -// MatOfKeyPoint keypoints_1; -// int fast_threshold = 20; -// boolean non_max_suppression = true; - detector.detect(img_1, points1); } @Override public void release() { + aprilTagDetectionPipe.release(); + singleTagPoseEstimatorPipe.release(); + visualOdometryPipe.release(); super.release(); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/CustomTestPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/CustomTestPipelineSettings.java index 3a69eaa..7967e20 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/CustomTestPipelineSettings.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/CustomTestPipelineSettings.java @@ -1,30 +1,62 @@ package org.photonvision.vision.pipeline; import com.fasterxml.jackson.annotation.JsonTypeName; +import org.photonvision.vision.apriltag.AprilTagFamily; +import org.photonvision.vision.target.TargetModel; @JsonTypeName("CustomTestPipelineSettings") public class CustomTestPipelineSettings extends AdvancedPipelineSettings { - public int test1 = 1; - public int test2 = 2; - public int test3 = 3; + public int featureThreshold = 1; + public int minFeatures = 500; + public int imageDifferenceThreshold = 150; + public double essentialMatProb = 0.999; + public double essentialMatThreshold = 1.; + + public AprilTagFamily tagFamily = AprilTagFamily.kTag36h11; + public int decimate = 1; + public double blur = 0; + public int threads = 4; // Multiple threads seems to be better performance on most platforms + public boolean debug = false; + public boolean refineEdges = true; + public int numIterations = 40; + public int hammingDist = 0; + public int decisionMargin = 35; + public boolean doMultiTarget = false; + public boolean doSingleTargetAlways = false; + public CustomTestPipelineSettings() { super(); pipelineType = PipelineType.CustomTest; } + @Override public int hashCode() { final int prime = 31; int result = super.hashCode(); - long temp1; - temp1 = Double.doubleToLongBits(test1); - result = prime * result + (int) (temp1 ^ (temp1 >>> 32)); - long temp2; - temp2 = Double.doubleToLongBits(test1); - result = prime * result + (int) (temp2^ (temp2 >>> 32)); - long temp3; - temp3 = Double.doubleToLongBits(test1); - result = prime * result + (int) (temp3 ^ (temp3 >>> 32)); + long temp; + + temp = Double.doubleToLongBits(featureThreshold); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(minFeatures); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(imageDifferenceThreshold); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(essentialMatProb); + result = prime * result + (int) (temp ^ (temp >>> 32)); + temp = Double.doubleToLongBits(essentialMatThreshold); + result = prime * result + (int) (temp ^ (temp >>> 32)); + + result = prime * result + (int) (temp ^ (temp >>> 32)); + result = prime * result + threads; + result = prime * result + (debug ? 1231 : 1237); + result = prime * result + (refineEdges ? 1231 : 1237); + result = prime * result + numIterations; + result = prime * result + hammingDist; + result = prime * result + decisionMargin; + result = prime * result + (doMultiTarget ? 1231 : 1237); + result = prime * result + (doSingleTargetAlways ? 1231 : 1237); + return result; } @@ -34,9 +66,23 @@ public class CustomTestPipelineSettings extends AdvancedPipelineSettings { if (!super.equals(obj)) return false; if (getClass() != obj.getClass()) return false; CustomTestPipelineSettings other = (CustomTestPipelineSettings) obj; - if(test1 != ((CustomTestPipelineSettings) obj).test1) return false; - if(test2 != ((CustomTestPipelineSettings) obj).test2) return false; - if(test3 != ((CustomTestPipelineSettings) obj).test3) return false; + if(featureThreshold != other.featureThreshold) return false; + if(minFeatures != other.minFeatures) return false; + if(essentialMatProb != other.essentialMatProb) return false; + if(essentialMatThreshold != other.essentialMatThreshold) return false; + + if (tagFamily != other.tagFamily) return false; + if (decimate != other.decimate) return false; + if (Double.doubleToLongBits(blur) != Double.doubleToLongBits(other.blur)) return false; + if (threads != other.threads) return false; + if (debug != other.debug) return false; + if (refineEdges != other.refineEdges) return false; + if (numIterations != other.numIterations) return false; + if (hammingDist != other.hammingDist) return false; + if (decisionMargin != other.decisionMargin) return false; + if (doMultiTarget != other.doMultiTarget) return false; + if (doSingleTargetAlways != other.doSingleTargetAlways) return false; + return true; } }