Use mrcal to undistort points

build.gradle

@@ -29,7 +29,7 @@ ext {
     javalinVersion = "5.6.2"
     photonGlDriverLibVersion = "dev-v2023.1.0-9-g75fc678"
     frcYear = "2024"
-    mrcalVersion = "dev-v2024.0.0-7-gc976aaa";
+    mrcalVersion = "dev-v2024.0.0-14-g2dc249f";
 
     pubVersion = versionString
     isDev = pubVersion.startsWith("dev")

photon-client/src/components/cameras/CameraCalibrationInfoCard.vue

@@ -4,7 +4,7 @@ import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
 import { useStateStore } from "@/stores/StateStore";
 import { ref } from "vue";
 import loadingImage from "@/assets/images/loading.svg";
-import { getResolutionString, parseJsonFile } from "@/lib/PhotonUtils";
+import { getResolutionString, parseJsonFile, parseTextFile } from "@/lib/PhotonUtils";
 
 const props = defineProps<{
   videoFormat: VideoFormat;
@@ -53,7 +53,35 @@ const openUploadPhotonCalibJsonPrompt = () => {
 const importCalibration = async () => {
   const files = importCalibrationFromPhotonJson.value.files;
   if (files.length === 0) return;
-  const uploadedJson = files[0];
+  const uploadedJson: File = files[0];
+
+  if (uploadedJson.name.endsWith(".cameramodel")) {
+    // Parsing a mrcal cameramodel -- punt all validation to the backend
+    const data = await parseTextFile(uploadedJson);
+
+    const lensmodelMatch = data.match(".*lensmodel':\\ *'(.*)'.*");
+    if (lensmodelMatch?.length === 2) {
+      console.log(lensmodelMatch[1])
+    } else {
+      console.log("No lensmodel")
+    }
+
+    const intrinsicsMatch = data.match(".*intrinsics':(.*),");
+    if (intrinsicsMatch?.length === 2) {
+      // console.log(intrinsicsMatch[1])
+      const m = JSON.parse(intrinsicsMatch[1]
+        // Trin illegal end?
+        .replace(",]", "]")  
+      );
+      console.log(m)
+    } else {
+      console.log("No intrinsics")
+    }
+
+    // TODO hot replace the camera cal results!
+
+    return;
+  }
 
   const data = await parseJsonFile<CameraCalibrationResult>(uploadedJson);
 
@@ -131,7 +159,7 @@ const getObservationDetails = (): ObservationDetails[] | undefined => {
         <input
           ref="importCalibrationFromPhotonJson"
           type="file"
-          accept=".json"
+          accept=".json, .cameramodel"
           style="display: none"
           @change="importCalibration"
         />

photon-client/src/lib/PhotonUtils.ts

@@ -18,3 +18,16 @@ export const parseJsonFile = async <T extends Record<string, any>>(file: File):
     fileReader.readAsText(file);
   });
 };
+
+export const parseTextFile = async (file: File): Promise<string> => {
+  return new Promise((resolve, reject) => {
+    const fileReader = new FileReader();
+    fileReader.onload = (event) => {
+      const target: FileReader | null = event.target;
+      if (target === null) reject();
+      else resolve(target.result as string);
+    };
+    fileReader.onerror = (error) => reject(error);
+    fileReader.readAsText(file);
+  });
+};

photon-core/src/main/java/org/opencv/core/MatOfPoint2d.java

@@ -0,0 +1,81 @@
+package org.opencv.core;
+
+import java.util.Arrays;
+import java.util.List;
+
+/**
+ * Copy of cv::MatofPoint2f, but with doubles
+ */
+public class MatOfPoint2d extends Mat {
+    // 32FC2
+    private static final int _depth = CvType.CV_64F;
+    private static final int _channels = 2;
+
+    public MatOfPoint2d() {
+        super();
+    }
+
+    protected MatOfPoint2d(long addr) {
+        super(addr);
+        if( !empty() && checkVector(_channels, _depth) < 0 )
+            throw new IllegalArgumentException("Incompatible Mat");
+        //FIXME: do we need release() here?
+    }
+
+    public static MatOfPoint2f fromNativeAddr(long addr) {
+        return new MatOfPoint2f(addr);
+    }
+
+    public MatOfPoint2d(Mat m) {
+        super(m, Range.all());
+        if( !empty() && checkVector(_channels, _depth) < 0 )
+            throw new IllegalArgumentException("Incompatible Mat");
+        //FIXME: do we need release() here?
+    }
+
+    public MatOfPoint2d(Point...a) {
+        super();
+        fromArray(a);
+    }
+
+    public void alloc(int elemNumber) {
+        if(elemNumber>0)
+            super.create(elemNumber, 1, CvType.makeType(_depth, _channels));
+    }
+
+    public void fromArray(Point...a) {
+        if(a==null || a.length==0)
+            return;
+        int num = a.length;
+        alloc(num);
+        double buff[] = new double[num * _channels];
+        for(int i=0; i<num; i++) {
+            Point p = a[i];
+            buff[_channels*i+0] = p.x;
+            buff[_channels*i+1] = p.y;
+        }
+        put(0, 0, buff); //TODO: check ret val!
+    }
+
+    public Point[] toArray() {
+        int num = (int) total();
+        Point[] ap = new Point[num];
+        if(num == 0)
+            return ap;
+        double buff[] = new double[num * _channels];
+        get(0, 0, buff); //TODO: check ret val!
+        for(int i=0; i<num; i++)
+            ap[i] = new Point(buff[i*_channels], buff[i*_channels+1]);
+        return ap;
+    }
+
+    public void fromList(List<Point> lp) {
+        Point ap[] = lp.toArray(new Point[0]);
+        fromArray(ap);
+    }
+
+    public List<Point> toList() {
+        Point[] ap = toArray();
+        return Arrays.asList(ap);
+    }
+}

photon-core/src/main/java/org/photonvision/common/logging/LogGroup.java

@@ -23,5 +23,6 @@ public enum LogGroup {
     VisionModule,
     Data,
     General,
-    Config
+    Config,
+    Vision
 }

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

@@ -0,0 +1,81 @@
+/*
+ * 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.util.List;
+
+import org.opencv.calib3d.Calib3d;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint2d;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.Point;
+import org.opencv.core.Size;
+import org.opencv.core.TermCriteria;
+import org.opencv.imgproc.Imgproc;
+import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.Logger;
+import org.photonvision.estimation.OpenCVHelp;
+import org.photonvision.mrcal.MrCalJNI;
+import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
+import org.photonvision.vision.pipe.CVPipe;
+import org.photonvision.vision.pipe.MutatingPipe;
+
+/** Represents a pipeline that blurs the image. */
+public class UndistortPointsPipe extends CVPipe<List<Point>, List<Point>, UndistortPointsPipe.UndistortParams> {
+    private static final Logger logger = new Logger(UndistortPointsPipe.class, LogGroup.Vision);
+
+    public static class UndistortParams {
+
+        final CameraCalibrationCoefficients cal;
+        final boolean useMrcal;
+
+        public UndistortParams(CameraCalibrationCoefficients calibration, boolean useMrcal) {
+            this.cal = calibration;
+            this.useMrcal = useMrcal;
+        }
+    }
+
+    @Override
+    protected List<Point> process(List<Point> in) {
+        if (params.useMrcal) {
+            var src = new MatOfPoint2d(in.toArray(new Point[0]));
+
+            var dst = new MatOfPoint2d();
+            dst.alloc(src.rows());
+
+            var ret = MrCalJNI.undistort_mrcal(src.nativeObj, dst.nativeObj, params.cal.cameraIntrinsics.getAsMatOfDouble().nativeObj, params.cal.distCoeffs.getAsMatOfDouble().nativeObj, 0, -1, -1, -1, -1);
+
+            if (!ret) {
+                logger.error("Could not undistort with mrcal!");
+            }
+
+            return dst.toList();
+        } else {
+            var distMat = new MatOfPoint2f(in.toArray(new Point[0]));
+            var undistMat = new MatOfPoint2f();
+            // Arbitrary precision for undistort internal iteration
+            double EPSILON = 1e-6;
+            Calib3d.undistortImagePoints(distMat, undistMat, params.cal.cameraIntrinsics.getAsMatOfDouble(), params.cal.distCoeffs.getAsMatOfDouble(),
+                new TermCriteria(TermCriteria.COUNT + TermCriteria.EPS, 30, EPSILON));
+            var ret = undistMat.toList();
+            distMat.release();
+            undistMat.release();
+            return ret;
+        }
+    }
+}

photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java

@@ -17,6 +17,7 @@
 
 package org.photonvision.vision.pipeline;
 
+import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotNull;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
@@ -25,17 +26,24 @@
 import java.io.IOException;
 import java.nio.file.Path;
 import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.Test;
 import org.junitpioneer.jupiter.cartesian.CartesianTest;
 import org.junitpioneer.jupiter.cartesian.CartesianTest.Enum;
 import org.junitpioneer.jupiter.cartesian.CartesianTest.Values;
 import org.opencv.calib3d.Calib3d;
 import org.opencv.core.Mat;
+import org.opencv.core.MatOfDouble;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.MatOfPoint2d;
+import org.opencv.core.MatOfPoint2f;
+import org.opencv.core.Point;
 import org.opencv.core.Size;
 import org.opencv.imgcodecs.Imgcodecs;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.LogLevel;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.TestUtils;
+import org.photonvision.mrcal.MrCalJNI;
 import org.photonvision.mrcal.MrCalJNILoader;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.camera.QuirkyCamera;
@@ -75,6 +83,43 @@ private CalibrationDatasets(String path, Size image, Size chessboard) {
         }
     }
 
+    @Test
+    public void testMeme() {
+        Point[] inputData = {
+            new Point(0, 0),
+            new Point(1, 0),
+            new Point(0, 1)
+        };
+        var src = new MatOfPoint2d(inputData);
+
+        var dst = new MatOfPoint2d();
+        dst.alloc(src.rows());
+
+
+        var cameraMat = new MatOfDouble(
+            100, 0, 50,
+            0, 100, 20,
+            0, 0, 1
+        ).reshape(0, 3);
+        var distCoeffs = new MatOfDouble(0.17802570252202954,-1.461379065131586,0.001019661566461145,0.0003215220840230439,2.7249642067580533).reshape(0, 1);
+
+        for (int i = 0; i < 100; i++) {
+            var start = System.nanoTime();
+            var ret = MrCalJNI.undistort_mrcal(src.nativeObj, dst.nativeObj, cameraMat.nativeObj, distCoeffs.nativeObj, 0, -1, -1, -1, -1);
+            var end = System.nanoTime();
+
+            System.out.println("dt: " + (end - start)/1e-6+"ms!");
+            assertTrue(ret);
+        }
+        System.out.println(src.dump());
+        System.out.println(dst.dump());
+
+        var pts = dst.toList();
+        assertEquals(-0.2848261640274217, pts.get(0).x, 0.001);
+        assertEquals(-0.1402225466498805, pts.get(0).y, 0.001);
+
+    }
+
     /**
      * Run camera calibration on a given dataset
      *