From a24a0e93c256d85e2a53826d8ec26a7966bcd8c1 Mon Sep 17 00:00:00 2001 From: astatin3 Date: Wed, 11 Dec 2024 23:50:27 -0700 Subject: [PATCH] Write code --- README.md | 94 ++------------ .../vision/pipeline/CustomTestPipeline.java | 115 +++++++++++++++--- photon-targeting/build.gradle | 2 +- 3 files changed, 108 insertions(+), 103 deletions(-) diff --git a/README.md b/README.md index f3c7bd5..c0e4f0f 100644 --- a/README.md +++ b/README.md @@ -1,86 +1,10 @@ -# PhotonVision +# VIO - PhotonVision +This is a project that aims to improve FRC vision accuracy using Visual Odometry (VO) and similar methods -[![CI](https://github.com/PhotonVision/photonvision/workflows/CI/badge.svg)](https://github.com/PhotonVision/photonvision/actions?query=workflow%3ACI) [![codecov](https://codecov.io/gh/PhotonVision/photonvision/branch/master/graph/badge.svg)](https://codecov.io/gh/PhotonVision/photonvision) [![Discord](https://img.shields.io/discord/725836368059826228?color=%23738ADB&label=Join%20our%20Discord&logo=discord&logoColor=white)](https://discord.gg/wYxTwym) - -PhotonVision is the free, fast, and easy-to-use computer vision solution for the *FIRST* Robotics Competition. You can read an overview of our features [on our website](https://photonvision.org). You can find our comprehensive documentation [here](https://docs.photonvision.org). - -The latest release of platform-specific jars and images is found [here](https://github.com/PhotonVision/photonvision/releases). - -If you are interested in contributing code or documentation to the project, please [read our getting started page for contributors](https://docs.photonvision.org/en/latest/docs/contributing/index.html) and **[join the Discord](https://discord.gg/wYxTwym) to introduce yourself!** We hope to provide a welcoming community to anyone who is interested in helping. - -## Authors - - - - - -## Documentation - -- Our main documentation page: [docs.photonvision.org](https://docs.photonvision.org) -- Photon UI demo: [demo.photonvision.org](https://demo.photonvision.org) (or [manual link](https://photonvision.github.io/photonvision/built-client/)) -- Javadocs: [javadocs.photonvision.org](https://javadocs.photonvision.org) (or [manual link](https://photonvision.github.io/photonvision/built-docs/javadoc/)) -- C++ Doxygen [cppdocs.photonvision.org](https://cppdocs.photonvision.org) (or [manual link](https://photonvision.github.io/photonvision/built-docs/doxygen/html/)) - -## Building - -Gradle is used for all C++ and Java code, and NPM is used for the web UI. Instructions to compile PhotonVision yourself can be found [in our docs](https://docs.photonvision.org/en/latest/docs/contributing/building-photon.html#compiling-instructions). - -You can run one of the many built in examples straight from the command line, too! They contain a fully featured robot project, and some include simulation support. The projects can be found inside the [`photonlib-java-examples`](photonlib-java-examples) and [`photonlib-cpp-examples`](photonlib-cpp-examples) subdirectories, respectively. Instructions for running these examples directly from the repo are found [in the docs](https://docs.photonvision.org/en/latest/docs/contributing/building-photon.html#running-examples). - -## Gradle Arguments - -Note that these are case sensitive! - -* `-PArchOverride=foobar`: builds for a target system other than your current architecture. [Valid overrides](https://github.com/wpilibsuite/wpilib-tool-plugin/blob/main/src/main/java/edu/wpi/first/tools/NativePlatforms.java) are: - * winx32 - * winx64 - * winarm64 - * macx64 - * macarm64 - * linuxx64 - * linuxarm64 - * linuxathena -- `-PtgtIP`: Specifies where `./gradlew deploy` should try to copy the fat JAR to -- `-Pprofile`: enables JVM profiling - -If you're cross-compiling, you'll need the wpilib toolchain installed. This can be done via Gradle: for example `./gradlew installArm64Toolchain` or `./gradlew installRoboRioToolchain` - -## Out-of-Source Dependencies - -PhotonVision uses the following additional out-of-source repositories for building code. - -- Base system images for Raspberry Pi & Orange Pi: https://github.com/PhotonVision/photon-image-modifier -- C++ driver for Raspberry Pi CSI cameras: https://github.com/PhotonVision/photon-libcamera-gl-driver -- JNI code for [mrcal](https://mrcal.secretsauce.net/): https://github.com/PhotonVision/mrcal-java -- Custom build of OpenCV with GStreamer/Protobuf/other custom flags: https://github.com/PhotonVision/thirdparty-opencv -- JNI code for aruco-nano: https://github.com/PhotonVision/aruconano-jni - -## Additional packages - -For now, using mrcal requires installing these additional packages on Linux systems: - -``` -sudo apt install libcholmod3 liblapack3 libsuitesparseconfig5 -``` - -## Acknowledgments - -PhotonVision was forked from [Chameleon Vision](https://github.com/Chameleon-Vision/chameleon-vision/). Thank you to everyone who worked on the original project. - -* [WPILib](https://github.com/wpilibsuite) - Specifically [cscore](https://github.com/wpilibsuite/allwpilib/tree/master/cscore), [CameraServer](https://github.com/wpilibsuite/allwpilib/tree/master/cameraserver), [NTCore](https://github.com/wpilibsuite/allwpilib/tree/master/ntcore), and [OpenCV](https://github.com/wpilibsuite/thirdparty-opencv). - -* [Apache Commons](https://commons.apache.org/) - Specifically [Commons Math](https://commons.apache.org/proper/commons-math/), and [Commons Lang](https://commons.apache.org/proper/commons-lang/) - -* [Javalin](https://javalin.io/) - -* [JSON](https://json.org) - -* [FasterXML](https://github.com/FasterXML) - Specifically [jackson](https://github.com/FasterXML/jackson) - -## License - -PhotonVision is licensed under the [GNU General Public License](https://www.gnu.org/licenses/gpl-3.0.html). - -## Meeting Notes - -Our [meeting notes](https://github.com/PhotonVision/photonvision/wiki/PhotonVision-Meeting-Notes) can be found in the wiki section of this repository. +Things to accomplish: +- [x] Get custom pipeline running +- [ ] 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 +- [ ] 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-core/src/main/java/org/photonvision/vision/pipeline/CustomTestPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/CustomTestPipeline.java index da59014..b023dbd 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/CustomTestPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/CustomTestPipeline.java @@ -1,9 +1,13 @@ package org.photonvision.vision.pipeline; +import edu.wpi.first.math.geometry.Transform3d; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.*; import org.opencv.core.Point; -import org.opencv.core.Scalar; +import org.opencv.features2d.*; import org.opencv.imgproc.Imgproc; import org.photonvision.common.util.ColorHelper; +import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; import org.photonvision.vision.pipe.CVPipe; @@ -13,8 +17,15 @@ import org.photonvision.vision.pipe.impl.CalculateFPSPipe; import org.photonvision.vision.pipeline.result.CVPipelineResult; import java.awt.*; +import java.util.ArrayList; import java.util.List; +import static org.opencv.calib3d.Calib3d.findEssentialMat; +import static org.opencv.calib3d.Calib3d.recoverPose; +import static org.opencv.core.CvType.CV_16U; +import static org.opencv.features2d.Features2d.drawKeypoints; +import static org.opencv.video.Video.calcOpticalFlowPyrLK; + public class CustomTestPipeline extends CVPipeline { private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.GREYSCALE; private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe(); @@ -37,45 +48,115 @@ public class CustomTestPipeline extends CVPipeline 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]; -// } +// if(frameStaticProperties.cameraCalibration != null) { +// visualOdometry = new VisualOdometry(frameStaticProperties.cameraCalibration.getCameraIntrinsicsMat()); // } +// blurDetectionPipe.setParams(new BlurDetectionPipe.BlurDetectionParams(100)); } + 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"; + if (frame.type != FrameThresholdType.GREYSCALE) { // We asked for a GREYSCALE frame, but didn't get one -- best we can do is give up + Imgproc.putText(frame.processedImage.getMat(), "Not Greyscale", new Point(10,50), Imgproc.FONT_HERSHEY_TRIPLEX, 1, FONT_COLOR); return new CVPipelineResult(frame.sequenceID, 0, 0, List.of(), frame); } -// var pr = blurPipe.run(frame.processedImage.getMat()); -// total_proc_time += pr.nanosElapsed; + if(frameStaticProperties.cameraCalibration == null) { + // The camera must be calibrated + Imgproc.putText(frame.processedImage.getMat(), "Not Calibrated", new Point(10,50), Imgproc.FONT_HERSHEY_TRIPLEX, 1, FONT_COLOR); + return new CVPipelineResult(frame.sequenceID, 0, 0, List.of(), frame); + } - double blurAmount = blurDetectionPipe.run(frame.processedImage.getMat()).output; - System.out.println(blurAmount); + Mat frame_mat = frame.processedImage.getMat(); + + MatOfKeyPoint points = new MatOfKeyPoint(); + detector.detect(frame_mat, points); + points.convertTo(points, CV_16U); + + if(prev_points != null && points.depth() > 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); +// +// +// +// +// 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]); + } + prev_points = points; + + + + drawKeypoints(frame_mat, points, frame_mat); + +// Transform3d pos = - Imgproc.putText(frame.processedImage.getMat(), "BLUR: " + blurAmount, new Point(10,50), Imgproc.FONT_HERSHEY_TRIPLEX, 1, FONT_COLOR); var fps = calculateFPSPipe.run(null).output; return new CVPipelineResult(frame.sequenceID, total_proc_time, fps, List.of(), frame); } + public void featureTracking(Mat img_1, Mat img_2, MatOfPoint2f points1, MatOfPoint2f points2, MatOfByte status) { + + // this function automatically gets rid of points for which tracking fails + + MatOfFloat err = new MatOfFloat(); + Size window_size = new Size(21, 21); + TermCriteria term_criteria = new TermCriteria(TermCriteria.COUNT + TermCriteria.EPS, 30, 0.01); + + calcOpticalFlowPyrLK(img_1, img_2, points1, points2, status, err, window_size, 3, term_criteria, 0, 0.001); + + // 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++; +// } +// } +} + + 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() { super.release(); diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 8c0bc49..9b02ed2 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -242,4 +242,4 @@ nativeConfig.dependencies.add wpilibTools.deps.wpilib("ntcore") nativeConfig.dependencies.add wpilibTools.deps.wpilib("hal") nativeConfig.dependencies.add wpilibTools.deps.wpilib("cscore") nativeConfig.dependencies.add wpilibTools.deps.wpilibOpenCv("frc" + openCVYear, wpi.versions.opencvVersion.get()) -nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag") +nativeConfig.dependencies.add wpilibTools.deps.wpilib("apriltag") \ No newline at end of file