Add point visualization to calibrate pipeline

photon-core/src/main/java/org/photonvision/common/hardware/Platform.java

@@ -28,7 +28,7 @@
 @SuppressWarnings("unused")
 public enum Platform {
     // WPILib Supported (JNI)
-    WINDOWS_64("Windows x64", "windowsx86-64", false, OSType.WINDOWS, true),
+    WINDOWS_64("Windows x64", "winx86-64", false, OSType.WINDOWS, true),
     LINUX_32("Linux x86", "linuxx86-64", false, OSType.LINUX, true),
     LINUX_64("Linux x64", "linuxx86-64", false, OSType.LINUX, true),
     LINUX_RASPBIAN32(

photon-core/src/main/java/org/photonvision/common/util/ColorHelper.java

@@ -24,4 +24,7 @@ public class ColorHelper {
     public static Scalar colorToScalar(Color color) {
         return new Scalar(color.getBlue(), color.getGreen(), color.getRed());
     }
+    public static Scalar colorToScalar(Color color, double alpha) {
+        return new Scalar(color.getBlue(), color.getGreen(), color.getRed(), alpha);
+    }
 }

photon-core/src/main/java/org/photonvision/jni/PhotonJniCommon.java

@@ -18,6 +18,7 @@
 package org.photonvision.jni;
 
 import java.io.File;
+import java.io.FileOutputStream;
 import java.io.IOException;
 import java.io.InputStream;
 import java.net.URL;
@@ -46,31 +47,32 @@ protected static synchronized void forceLoad(Class<?> clazz) throws IOException
         if (logger == null) logger = new Logger(clazz, LogGroup.Camera);
 
         try {
-            File libDirectory = Path.of("lib/").toFile();
-            if (!libDirectory.exists()) {
-                Files.createDirectory(libDirectory.toPath()).toFile();
-            }
 
             // We always extract the shared object (we could hash each so, but that's a lot of work)
             var arch_name = Platform.getNativeLibraryFolderName();
-            URL resourceURL =
-                    PhotonJniCommon.class.getResource("/nativelibraries/" + arch_name + "/libmrgingham.so");
-            File libFile = Path.of("lib/libphotonlibcamera.so").toFile();
-            try (InputStream in = resourceURL.openStream()) {
-                if (libFile.exists()) Files.delete(libFile.toPath());
-                Files.copy(in, libFile.toPath());
-            } catch (Exception e) {
-                logger.error("Could not extract the native library!");
-                libraryLoaded = false;
-                return;
+            var in = clazz.getResourceAsStream("/nativelibraries/" + arch_name + "/libmrgingham_" + arch_name + System.mapLibraryName(""));
+
+            File temp = File.createTempFile("libmrgingham", "." + System.mapLibraryName(""));
+            FileOutputStream fos = new FileOutputStream(temp);
+
+            int read = -1;
+            byte[] buffer = new byte[1024];
+            while((read = in.read(buffer)) != -1) {
+                fos.write(buffer, 0, read);
             }
-            System.load(libFile.getAbsolutePath());
+            fos.close();
+            in.close();
+
+            System.load(temp.getAbsolutePath());
 
             libraryLoaded = true;
             logger.info("Successfully loaded shared object");
+
+
         } catch (UnsatisfiedLinkError e) {
-            logger.error("Couldn't load shared object");
+            logger.error("Couldn't load shared object", e);
             e.printStackTrace();
+            logger.error(System.getProperty("java.library.path"));
         }
     }
 

photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java

@@ -0,0 +1,70 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipe.impl;
+
+import java.awt.Color;
+import java.util.List;
+import org.apache.commons.lang3.tuple.Pair;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.imgproc.Imgproc;
+import org.photonvision.common.util.ColorHelper;
+import org.photonvision.vision.frame.FrameDivisor;
+import org.photonvision.vision.pipe.MutatingPipe;
+import org.photonvision.vision.target.TrackedTarget;
+
+public class DrawCalibrationPipe
+        extends MutatingPipe<
+                Pair<Mat, List<TrackedTarget>>, DrawCalibrationPipe.DrawCalibrationPipeParams> {
+    @Override
+    protected Void process(Pair<Mat, List<TrackedTarget>> in) {
+
+        var image = in.getLeft();
+
+        for (var target : in.getRight()) {
+
+            for (var c : target.getTargetCorners()) {
+                c = new Point(c.x / params.divisor.value.doubleValue(), c.y / params.divisor.value.doubleValue());
+                var r = 4;
+                var r2 = r / Math.sqrt(2);
+                var color = ColorHelper.colorToScalar(Color.RED, 0.4);
+                Imgproc.circle(image, c, r, color, 1);
+                Imgproc.line(image, 
+                    new Point(c.x-r2, c.y-r2),
+                    new Point(c.x+r2, c.y+r2),
+                    color);
+                Imgproc.line(image, 
+                    new Point(c.x+r2, c.y-r2),
+                    new Point(c.x-r2, c.y+r2),
+                    color);
+            }
+
+        }
+
+        return null;
+    }
+
+    public static class DrawCalibrationPipeParams {
+        private final FrameDivisor divisor;
+
+        public DrawCalibrationPipeParams(
+                FrameDivisor divisor) {
+            this.divisor = divisor;
+        }
+    }
+}

photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java

@@ -229,7 +229,11 @@ private Triple<Size, Mat, Mat> findBoardCorners(Pair<Mat, Mat> in) {
 
         if (params.type == UICalibrationData.BoardType.CHESSBOARD) {
             // Reduce the image size to be much more manageable
-            Imgproc.resize(inFrame, smallerInFrame, getFindCornersImgSize(inFrame));
+            if (params.divisor != FrameDivisor.NONE) {
+                Imgproc.resize(inFrame, smallerInFrame, getFindCornersImgSize(inFrame));
+            } else {
+                smallerInFrame = inFrame;
+            }
 
             if (params.useMrginghamDetector) {
                 int gridn = (int) patternSize.width; // TODO
@@ -270,7 +274,7 @@ private Triple<Size, Mat, Mat> findBoardCorners(Pair<Mat, Mat> in) {
         this.imageSize = new Size(inFrame.width(), inFrame.height());
 
         // Do sub corner pix for drawing chessboard when using OpenCV
-        if (!params.useMrginghamDetector) {
+        if (!params.useMrginghamDetector || params.divisor != FrameDivisor.NONE) {
             Imgproc.cornerSubPix(
                     inFrame, outBoardCorners, getWindowSize(outBoardCorners), zeroZone, criteria);
         }

photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java

@@ -22,9 +22,13 @@
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.List;
+import java.util.stream.Collectors;
+
 import org.apache.commons.lang3.tuple.Pair;
 import org.apache.commons.lang3.tuple.Triple;
 import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.Point;
 import org.opencv.core.Size;
 import org.opencv.imgcodecs.Imgcodecs;
 import org.photonvision.common.configuration.ConfigManager;
@@ -43,6 +47,7 @@
 import org.photonvision.vision.pipe.impl.Calibrate3dPipe;
 import org.photonvision.vision.pipe.impl.FindBoardCornersPipe;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
+import org.photonvision.vision.pipeline.result.CalibrationPipelineResult;
 
 public class Calibrate3dPipeline
         extends CVPipeline<CVPipelineResult, Calibration3dPipelineSettings> {
@@ -119,15 +124,9 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se
         var outputColorCVMat = new CVMat();
         inputColorMat.copyTo(outputColorCVMat.getMat());
 
-        var start = System.currentTimeMillis();
-
-        var findBoardResult =
+        Triple<Size, Mat, Mat> findBoardResult =
                 findBoardCornersPipe.run(Pair.of(inputColorMat, outputColorCVMat.getMat())).output;
 
-        var end = System.currentTimeMillis();
-        var dt = (end - start);
-        System.out.printf("Find corners ran in %f ms!\n", (double) dt);
-
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
 
@@ -149,12 +148,17 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se
         frame.release();
 
         // Return the drawn chessboard if corners are found, if not, then return the input image.
-        return new CVPipelineResult(
+        return new CalibrationPipelineResult(
                 sumPipeNanosElapsed,
                 fps, // Unused but here in case
-                Collections.emptyList(),
                 new Frame(
-                        new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties));
+                        new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties),
+                getCornersList()
+                        );
+    }
+
+    List<List<Point>> getCornersList() {
+        return foundCornersList.stream().map(it -> ((MatOfPoint2f)it.getRight()).toList()).collect(Collectors.toList());
     }
 
     public void deleteSavedImages() {

photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java

@@ -38,6 +38,7 @@ public class OutputStreamPipeline {
     private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
     private final Draw2dAprilTagsPipe draw2dAprilTagsPipe = new Draw2dAprilTagsPipe();
     private final Draw3dAprilTagsPipe draw3dAprilTagsPipe = new Draw3dAprilTagsPipe();
+    private final DrawCalibrationPipe drawCalibrationPipe = new DrawCalibrationPipe();
 
     private final Draw2dArucoPipe draw2dArucoPipe = new Draw2dArucoPipe();
     private final Draw3dArucoPipe draw3dArucoPipe = new Draw3dArucoPipe();
@@ -113,6 +114,8 @@ protected void setPipeParams(
 
         resizeImagePipe.setParams(
                 new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor));
+
+        drawCalibrationPipe.setParams(new DrawCalibrationPipe.DrawCalibrationPipeParams(settings.streamingFrameDivisor));
     }
 
     public CVPipelineResult process(
@@ -149,7 +152,8 @@ public CVPipelineResult process(
             sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed;
 
             if (!(settings instanceof AprilTagPipelineSettings)
-                    && !(settings instanceof ArucoPipelineSettings)) {
+                    && !(settings instanceof ArucoPipelineSettings)
+                    && !(settings instanceof Calibration3dPipelineSettings)) {
                 // If we're processing anything other than Apriltags..
                 var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw));
                 sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed;
@@ -172,7 +176,14 @@ public CVPipelineResult process(
                     pipeProfileNanos[7] = 0;
                     pipeProfileNanos[8] = 0;
                 }
+            } else if (settings instanceof Calibration3dPipelineSettings) {
+                    pipeProfileNanos[5] = 0;
+                    pipeProfileNanos[6] = 0;
 
+                    var drawOnInputResult = drawCalibrationPipe.run(Pair.of(outMat, targetsToDraw));
+                    sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed;
+
+                    pipeProfileNanos[8] = 0;
             } else if (settings instanceof AprilTagPipelineSettings) {
                 // If we are doing apriltags...
                 if (settings.solvePNPEnabled) {

photon-core/src/main/java/org/photonvision/vision/pipeline/result/CalibrationPipelineResult.java

@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.vision.pipeline.result;
+
+import java.util.List;
+import java.util.stream.Collectors;
+
+import org.opencv.core.Point;
+import org.photonvision.vision.frame.Frame;
+import org.photonvision.vision.target.TrackedTarget;
+
+
+public class CalibrationPipelineResult extends CVPipelineResult {
+
+    private static List<TrackedTarget> cornersToTarget(List<List<Point>> corners) {
+        return corners.stream().map(TrackedTarget::new).collect(Collectors.toList());
+    }
+
+    public CalibrationPipelineResult(double latencyNanos, double fps, Frame outputFrame, List<List<Point>> corners) {
+        super(latencyNanos, fps, cornersToTarget(corners), outputFrame);
+    }
+}

photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java

@@ -139,6 +139,14 @@ public TrackedTarget(
         setCameraRelativeRvec(rvec);
     }
 
+    public TrackedTarget(List<Point> corners) {
+        m_mainContour = new Contour(new MatOfPoint());
+        m_mainContour.mat.fromList(List.of(new Point(0, 0), new Point(0, 1), new Point(1, 0)));;
+        this.setTargetCorners(corners);
+        m_targetOffsetPoint = new Point();
+        m_robotOffsetPoint = new Point();
+    }
+
     public TrackedTarget(ArucoDetectionResult result, TargetCalculationParameters params) {
         m_targetOffsetPoint = new Point(result.getCenterX(), result.getCenterY());
         m_robotOffsetPoint = new Point();