From 15d21b7841ae26cd6ebb85099cc518017f4a795d Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Sat, 5 Dec 2020 12:18:07 -0800
Subject: [PATCH] Fix calibration modal bug, save calibration images (#165)

 Saves calibration images to photonvision_config/calibImgs
---
 photon-client/src/views/CamerasView.vue       |  4 +-
 .../common/configuration/ConfigManager.java   |  4 ++
 .../vision/pipe/impl/Calibrate3dPipe.java     | 11 ++++-
 .../pipe/impl/FindBoardCornersPipe.java       | 41 +++++++++++--------
 .../vision/pipeline/Calibrate3dPipeline.java  | 23 ++++++++++-
 .../vision/processes/VisionModule.java        |  3 +-
 .../vision/pipeline/Calibrate3dPipeTest.java  |  5 ++-
 7 files changed, 67 insertions(+), 24 deletions(-)

diff --git a/photon-client/src/views/CamerasView.vue b/photon-client/src/views/CamerasView.vue
index b16a31d8f..e736789bb 100644
--- a/photon-client/src/views/CamerasView.vue
+++ b/photon-client/src/views/CamerasView.vue
@@ -271,7 +271,7 @@
                 class="ml-3"
               >
                 <v-col align="center">
-                  <template v-if="calibrationInProgress">
+                  <template v-if="calibrationInProgress && !calibrationFailed">
                     <v-progress-circular
                       indeterminate
                       :size="70"
@@ -303,7 +303,7 @@
               <v-card-actions>
                 <v-spacer />
                 <v-btn
-                  v-if="!calibrationInProgress"
+                  v-if="!calibrationInProgress || calibrationFailed"
                   color="white"
                   text
                   @click="closeDialog"
diff --git a/photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java b/photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java
index d6323d351..0d7547642 100644
--- a/photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java
+++ b/photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java
@@ -361,6 +361,10 @@ public class ConfigManager {
         return Path.of(configDirectoryFile.toString(), "logs");
     }
 
+    public Path getCalibDir() {
+        return Path.of(configDirectoryFile.toString(), "calibImgs");
+    }
+
     public static final String LOG_PREFIX = "photonvision-";
     public static final String LOG_EXT = ".log";
     public static final String LOG_DATE_TIME_FORMAT = "yyyy-M-d_hh-mm-ss";
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
index 4405a136d..a2af98e36 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java
@@ -81,10 +81,17 @@ public class Calibrate3dPipe
         try {
             // FindBoardCorners pipe outputs all the image points, object points, and frames to calculate
             // imageSize from, other parameters are output Mats
+
+            var objPoints = in.stream().map(Triple::getMiddle).collect(Collectors.toList());
+            var imgPts = in.stream().map(Triple::getRight).collect(Collectors.toList());
+            if (objPoints.size() != imgPts.size()) {
+                logger.error("objpts.size != imgpts.size");
+                return null;
+            }
             calibrationAccuracy =
                     Calib3d.calibrateCameraExtended(
-                            in.stream().map(Triple::getMiddle).collect(Collectors.toList()),
-                            in.stream().map(Triple::getRight).collect(Collectors.toList()),
+                            objPoints,
+                            imgPts,
                             new Size(in.get(0).getLeft().width, in.get(0).getLeft().height),
                             cameraMatrix,
                             distortionCoefficients,
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
index f38cbe73b..213306647 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java
@@ -18,6 +18,7 @@
 package org.photonvision.vision.pipe.impl;
 
 import java.util.Objects;
+import org.apache.commons.lang3.tuple.Pair;
 import org.apache.commons.lang3.tuple.Triple;
 import org.opencv.calib3d.Calib3d;
 import org.opencv.core.*;
@@ -28,7 +29,8 @@ import org.photonvision.vision.pipe.CVPipe;
 import org.photonvision.vision.pipeline.UICalibrationData;
 
 public class FindBoardCornersPipe
-        extends CVPipe<Mat, Triple<Size, Mat, Mat>, FindBoardCornersPipe.FindCornersPipeParams> {
+        extends CVPipe<
+                Pair<Mat, Mat>, Triple<Size, Mat, Mat>, FindBoardCornersPipe.FindCornersPipeParams> {
     private static final Logger logger =
             new Logger(FindBoardCornersPipe.class, LogGroup.VisionModule);
 
@@ -91,11 +93,11 @@ public class FindBoardCornersPipe
     /**
     * Finds the corners in a given image and returns them
     *
-    * @param in Input for pipe processing.
+    * @param in Input for pipe processing. Pair of input and output mat
     * @return All valid Mats for camera calibration
     */
     @Override
-    protected Triple<Size, Mat, Mat> process(Mat in) {
+    protected Triple<Size, Mat, Mat> process(Pair<Mat, Mat> in) {
 
         // Create the object points
         createObjectPoints();
@@ -103,43 +105,50 @@ public class FindBoardCornersPipe
         return findBoardCorners(in);
     }
 
-    private Triple<Size, Mat, Mat> findBoardCorners(Mat frame) {
+    /**
+    * Find chessboard corners given a input mat and output mat to draw on
+    *
+    * @return Frame resolution, object points, board corners
+    */
+    private Triple<Size, Mat, Mat> findBoardCorners(Pair<Mat, Mat> in) {
         createObjectPoints();
 
-        // Convert the frame to grayscale to increase contrast
-        Imgproc.cvtColor(frame, frame, Imgproc.COLOR_BGR2GRAY);
+        var inFrame = in.getLeft();
+        var outFrame = in.getRight();
+
+        // Convert the inFrame to grayscale to increase contrast
+        Imgproc.cvtColor(inFrame, inFrame, Imgproc.COLOR_BGR2GRAY);
         boolean boardFound = false;
 
         if (params.type == UICalibrationData.BoardType.CHESSBOARD) {
             // This is for chessboards
-            boardFound = Calib3d.findChessboardCorners(frame, patternSize, boardCorners);
+            boardFound = Calib3d.findChessboardCorners(inFrame, patternSize, boardCorners);
         } else if (params.type == UICalibrationData.BoardType.DOTBOARD) {
             // For dot boards
             boardFound =
                     Calib3d.findCirclesGrid(
-                            frame, patternSize, boardCorners, Calib3d.CALIB_CB_ASYMMETRIC_GRID);
+                            inFrame, patternSize, boardCorners, Calib3d.CALIB_CB_ASYMMETRIC_GRID);
         }
 
         if (!boardFound) {
-            // If we can't find a chessboard/dot board, convert the frame back to BGR and return false.
-            Imgproc.cvtColor(frame, frame, Imgproc.COLOR_GRAY2BGR);
+            // If we can't find a chessboard/dot board, convert the inFrame back to BGR and return false.
 
             return null;
         }
         var outBoardCorners = new MatOfPoint2f();
         boardCorners.copyTo(outBoardCorners);
 
-        // Get the size of the frame
-        this.imageSize = new Size(frame.width(), frame.height());
+        // Get the size of the inFrame
+        this.imageSize = new Size(inFrame.width(), inFrame.height());
 
         // Do sub corner pix for drawing chessboard
-        Imgproc.cornerSubPix(frame, outBoardCorners, windowSize, zeroZone, criteria);
+        Imgproc.cornerSubPix(inFrame, outBoardCorners, windowSize, zeroZone, criteria);
 
         // convert back to BGR
-        Imgproc.cvtColor(frame, frame, Imgproc.COLOR_GRAY2BGR);
+        //        Imgproc.cvtColor(inFrame, inFrame, Imgproc.COLOR_GRAY2BGR);
         // draw the chessboard, doesn't have to be different for a dot board since it just re projects
         // the corners we found
-        Calib3d.drawChessboardCorners(frame, patternSize, outBoardCorners, true);
+        Calib3d.drawChessboardCorners(outFrame, patternSize, outBoardCorners, true);
 
         //        // Add the 3D points and the points of the corners found
         //        if (addToSnapList) {
@@ -147,7 +156,7 @@ public class FindBoardCornersPipe
         //            this.listOfImagePoints.add(boardCorners);
         //        }
 
-        return Triple.of(frame.size(), objectPoints, outBoardCorners);
+        return Triple.of(inFrame.size(), objectPoints, outBoardCorners);
     }
 
     public static class FindCornersPipeParams {
diff --git a/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java b/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
index d1ec08b8b..21ac68a12 100644
--- a/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
+++ b/photon-server/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
@@ -19,15 +19,20 @@ package org.photonvision.vision.pipeline;
 
 import com.fasterxml.jackson.core.JsonProcessingException;
 import edu.wpi.first.wpilibj.util.Units;
+import java.nio.file.Path;
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.List;
+import org.apache.commons.lang3.tuple.Pair;
 import org.apache.commons.lang3.tuple.Triple;
 import org.opencv.core.Mat;
 import org.opencv.core.Size;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.SerializationUtils;
+import org.photonvision.common.util.file.FileUtils;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.server.SocketHandler;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
@@ -62,6 +67,9 @@ public class Calibrate3dPipeline
 
     private boolean calibrating = false;
 
+    // Path to save images
+    private final Path imageDir = ConfigManager.getInstance().getCalibDir();
+
     public Calibrate3dPipeline() {
         this(25);
     }
@@ -99,7 +107,9 @@ public class Calibrate3dPipeline
         long sumPipeNanosElapsed = 0L;
 
         // Check if the frame has chessboard corners
-        var findBoardResult = findBoardCornersPipe.run(frame.image.getMat()).output;
+        var outFrame = new Mat();
+        frame.image.getMat().copyTo(outFrame);
+        var findBoardResult = findBoardCornersPipe.run(Pair.of(frame.image.getMat(), outFrame)).output;
 
         if (takeSnapshot) {
             // Set snapshot to false even if we don't find a board
@@ -107,6 +117,9 @@ public class Calibrate3dPipeline
 
             if (findBoardResult != null) {
                 foundCornersList.add(findBoardResult);
+                Imgcodecs.imwrite(
+                        Path.of(imageDir.toString(), "img" + foundCornersList.size() + ".jpg").toString(),
+                        frame.image.getMat());
 
                 // update the UI
                 broadcastState();
@@ -120,7 +133,13 @@ public class Calibrate3dPipeline
         return new CVPipelineResult(
                 MathUtils.nanosToMillis(sumPipeNanosElapsed),
                 null,
-                new Frame(new CVMat(frame.image.getMat()), frame.frameStaticProperties));
+                new Frame(new CVMat(outFrame), frame.frameStaticProperties));
+    }
+
+    public void deleteSavedImages() {
+        imageDir.toFile().mkdirs();
+        imageDir.toFile().mkdir();
+        FileUtils.deleteDirectory(imageDir);
     }
 
     public boolean hasEnough() {
diff --git a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java
index 0ae6193a1..eb0c2b4cc 100644
--- a/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java
+++ b/photon-server/src/main/java/org/photonvision/vision/processes/VisionModule.java
@@ -206,6 +206,7 @@ public class VisionModule {
 
     public void startCalibration(UICalibrationData data) {
         var settings = pipelineManager.calibration3dPipeline.getSettings();
+        pipelineManager.calibration3dPipeline.deleteSavedImages();
         settings.cameraVideoModeIndex = data.videoModeIndex;
         visionSource.getSettables().setVideoModeIndex(data.videoModeIndex);
         logger.info(
@@ -223,7 +224,7 @@ public class VisionModule {
             settings.cameraGain = -1;
         }
 
-        pipelineManager.setCalibrationMode(true);
+        setPipeline(PipelineManager.CAL_3D_INDEX);
     }
 
     public void takeCalibrationSnapshot() {
diff --git a/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java b/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
index 9543f8400..354892a84 100644
--- a/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
+++ b/photon-server/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java
@@ -25,6 +25,7 @@ import java.io.File;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
+import org.apache.commons.lang3.tuple.Pair;
 import org.apache.commons.lang3.tuple.Triple;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
@@ -62,7 +63,9 @@ public class Calibrate3dPipeTest {
         List<Triple<Size, Mat, Mat>> foundCornersList = new ArrayList<>();
 
         for (var f : frames) {
-            foundCornersList.add(findBoardCornersPipe.run(f).output);
+            var copy = new Mat();
+            f.copyTo(copy);
+            foundCornersList.add(findBoardCornersPipe.run(Pair.of(f, copy)).output);
         }
 
         Calibrate3dPipe calibrate3dPipe = new Calibrate3dPipe();