mirror of
https://github.com/Astatin3/photonvision-2025.0.0-beta-6.git
synced 2026-06-08 16:18:03 -06:00
Write code
This commit is contained in:
astatin3
@@ -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
|
||||
|
||||
[](https://github.com/PhotonVision/photonvision/actions?query=workflow%3ACI) [](https://codecov.io/gh/PhotonVision/photonvision) [](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
|
||||
|
||||
<a href="https://github.com/PhotonVision/photonvision/graphs/contributors">
|
||||
<img src="https://contrib.rocks/image?repo=PhotonVision/photonvision" />
|
||||
</a>
|
||||
|
||||
## 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.
|
||||
+98
-17
@@ -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<CVPipelineResult, CustomTestPipelineSettings> {
|
||||
private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.GREYSCALE;
|
||||
private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe();
|
||||
@@ -37,45 +48,115 @@ public class CustomTestPipeline extends CVPipeline<CVPipelineResult, CustomTestP
|
||||
// super.setPipeParamsImpl();
|
||||
this.released = false;
|
||||
blurPipe.setParams(new BlurPipe.BlurParams(5));
|
||||
blurDetectionPipe.setParams(new BlurDetectionPipe.BlurDetectionParams(100));
|
||||
|
||||
|
||||
// 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];
|
||||
// }
|
||||
// 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();
|
||||
|
||||
@@ -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")
|
||||
Reference in New Issue
Block a user