diff --git a/photon-core/src/main/java/org/photonvision/calibrator/ArrayUtils.java b/photon-core/src/main/java/org/photonvision/calibrator/ArrayUtils.java new file mode 100644 index 0000000000..88c1898f69 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/calibrator/ArrayUtils.java @@ -0,0 +1,373 @@ +/* + * 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 . + */ + +package org.photonvision.calibrator; + +import java.util.zip.Deflater; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; + +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* ArrayUtils class */ +/* ArrayUtils class */ +/* ArrayUtils class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +public class ArrayUtils { + private static final Logger logger = new Logger(ArrayUtils.class, LogGroup.General); + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* argmax */ + /* argmax */ + /* argmax */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * Find index of the maximum value in an array + * + * @param array an array + * @return the argmax (lowest index in case of duplicate values) + */ + static int argmax(double[] array) { + int locationOfExtreme = 0; + double extreme = array[locationOfExtreme]; + + for (int i = 1; i < array.length; i++) { + if (array[i] > extreme) { + extreme = array[i]; + locationOfExtreme = i; + } + } + return locationOfExtreme; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* argmin */ + /* argmin */ + /* argmin */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * Find index of the minimum value in an array + * + * @param array an array + * @return the argmin (lowest index in case of duplicate values) + */ + static int argmin(double[] array) { + int locationOfExtreme = 0; + double extreme = array[locationOfExtreme]; + + for (int i = 1; i < array.length; i++) { + if (array[i] < extreme) { + extreme = array[i]; + locationOfExtreme = i; + } + } + return locationOfExtreme; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* isAllTrue */ + /* isAllTrue */ + /* isAllTrue */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * Test for all values in a boolean array are true + * + * @param array + * @return true if all true or false if any false + */ + static boolean isAllTrue(boolean[] array) { + for (boolean element : array) { + if (!element) return false; + } + return true; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* brief */ + /* brief */ + /* brief */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * Partially print an OpenCV Mat + * + *

Convenience method of brief(Mat, int, int, int, int) with defaults of first 4 rows, last 4 + * rows, first 12 values in a row, last 12 values in a row. + * + *

Each ".r." or ".c." represents 2000 items or the remainder. + * + * @param mat OpenCV Mat to be partially printed + * @return String of the values in the corners of the Mat if large or entire Mat if small + */ + static String brief(Mat mat) { + return brief(mat, 4, 4, 12, 12); + } + + /** + * Partially print an OpenCV Mat + * + *

Each ".r." or ".c." represents 2000 items or the remainder. + * + * @param mat OpenCV Mat to be partially printed + *

+ * @param firstRows - count first few rows + * @param lastRows - count last few rows + * @param firstRowData - count first few column/channel values + * @param lastRowData - count last few column/channel values + * @return String of the values in the corners of the Mat if large or entire Mat if small + */ + static String brief(Mat mat, int firstRows, int lastRows, int firstRowData, int lastRowData) { + final int acknowledgeRowsSkipped = 2000; // skip count to print an icon + final int acknowledgeRowDataSkipped = 2000; // skip count to print an icon + + StringBuilder sb = new StringBuilder(); + sb.append(mat); + sb.append("\n"); + + double[] matRowD = null; + float[] matRowF = null; + byte[] matRowB = null; + int[] matRowI = null; + short[] matRowS = null; + + switch (CvType.depth(mat.type())) { + case CvType.CV_64F: // double + matRowD = new double[mat.channels() * mat.cols()]; + break; + + case CvType.CV_32F: // float + matRowF = new float[mat.channels() * mat.cols()]; + break; + + case CvType.CV_8U: // byte + case CvType.CV_8S: + matRowB = new byte[mat.channels() * mat.cols()]; + break; + + case CvType.CV_32S: // int + matRowI = new int[mat.channels() * mat.cols()]; + break; + + case CvType.CV_16U: // short + case CvType.CV_16S: + matRowS = new short[mat.channels() * mat.cols()]; + break; + + default: + logger.error( + "Print Mat Error - Unknown OpenCV Mat depth. Not printing requested data. " + + CvType.depth(mat.type())); + return "Print Mat Error"; + } + + int printCountRow = 0; + int printCountColChan = 0; + boolean skippedRow = false; + + for (int row = 0; row < mat.rows(); row++) { + if (row >= firstRows && row < mat.rows() - lastRows) { + skippedRow = true; + if (printCountRow % acknowledgeRowsSkipped == 0) { + printCountRow = 0; + sb.append(".r."); + } + printCountRow++; + continue; + } + if (skippedRow) { + sb.append("\n"); + skippedRow = false; + } + + switch (CvType.depth(mat.type())) { + case CvType.CV_64F: // double + mat.get(row, 0, matRowD); + break; + + case CvType.CV_32F: // float + mat.get(row, 0, matRowF); + break; + + case CvType.CV_8U: // byte + case CvType.CV_8S: + mat.get(row, 0, matRowB); + break; + + case CvType.CV_32S: // int + mat.get(row, 0, matRowI); + break; + + case CvType.CV_16U: // short + case CvType.CV_16S: + mat.get(row, 0, matRowS); + break; + + default: + return "ArrayUtils.brief(Mat) should not be here."; + } + printCountColChan = 0; + for (int colChan = 0; colChan < mat.cols() * mat.channels(); colChan++) { + if (colChan >= firstRowData && colChan < mat.cols() * mat.channels() - lastRowData) { + if (printCountColChan % acknowledgeRowDataSkipped == 0) { + printCountColChan = 0; + sb.append(".c. "); + } + printCountColChan++; + continue; + } + switch (CvType.depth(mat.type())) { + case CvType.CV_64F: // double + sb.append(matRowD[colChan]); + break; + + case CvType.CV_32F: // float + sb.append(matRowF[colChan]); + break; + + case CvType.CV_8U: // byte + case CvType.CV_8S: + sb.append(matRowB[colChan]); + break; + + case CvType.CV_32S: // int + sb.append(matRowI[colChan]); + break; + + case CvType.CV_16U: // short + case CvType.CV_16S: + sb.append(matRowS[colChan]); + break; + + default: + return "ArrayUtils.brief(Mat) should not be here."; + } + sb.append(" "); + } + sb.append("\n"); + } + return sb.toString(); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* intToByteArray */ + /* intToByteArray */ + /* intToByteArray */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * Convert one 4-byte int variable to the equivalent four element byte array The 4 bytes are + * located starting at "offset" from the designated output array + * + * @param toBeConvertedInt - effectively considered an unsigned int; unless you like the + * complemented value of a negative number you should limit this to positive numbers + * @param dst - array segment for the converted int; array length must be >= offset + 4 + * @param offset - starting element to be changed in the output array (number of changed elements + * is always 4 for the int type) + * @return byte array of the input + */ + static void intToByteArray(int toBeConvertedInt, byte[] dst, int offset) { + dst[offset + 3] = (byte) (toBeConvertedInt & 0xff); // least significant byte + + toBeConvertedInt >>= 8; + dst[offset + 2] = (byte) (toBeConvertedInt & 0xff); + + toBeConvertedInt >>= 8; + dst[offset + 1] = (byte) (toBeConvertedInt & 0xff); + + toBeConvertedInt >>= 8; + dst[offset + 0] = (byte) (toBeConvertedInt); // most significant byte + } + + /** + * @param bytesToCompress + * @return + */ + static byte[] compress(byte[] bytesToCompress) { + Deflater deflater = new Deflater(); + deflater.setInput(bytesToCompress); + deflater.finish(); + + byte[] bytesCompressed = + new byte + [bytesToCompress + .length]; // assumes compressed data no longer than input - not always true but it + // is for the ChArUcoBoard + + int numberOfBytesAfterCompression = deflater.deflate(bytesCompressed); + + byte[] returnValues = new byte[numberOfBytesAfterCompression]; + + System.arraycopy(bytesCompressed, 0, returnValues, 0, numberOfBytesAfterCompression); + + return returnValues; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* ArrayUtils constructor */ + /* ArrayUtils constructor */ + /* ArrayUtils constructor */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + private ArrayUtils() { + throw new UnsupportedOperationException("This is a utility class"); + } +} +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* End ArrayUtils class */ +/* End ArrayUtils class */ +/* End ArrayUtils class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ + +/* +CV_8U - 8-bit unsigned integers ( 0..255 ) +CV_8S - 8-bit signed integers ( -128..127 ) +CV_16U - 16-bit unsigned integers ( 0..65535 ) +CV_16S - 16-bit signed integers ( -32768..32767 ) +CV_32S - 32-bit signed integers ( -2147483648..2147483647 ) +CV_32F - 32-bit floating-point numbers ( -FLT_MAX..FLT_MAX, INF, NAN ) +CV_64F - 64-bit floating-point numbers ( -DBL_MAX..DBL_MAX, INF, NAN ) + */ diff --git a/photon-core/src/main/java/org/photonvision/calibrator/BoardPreview.java b/photon-core/src/main/java/org/photonvision/calibrator/BoardPreview.java new file mode 100644 index 0000000000..22deb999cb --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/calibrator/BoardPreview.java @@ -0,0 +1,303 @@ +/* + * 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 . + */ + +// This project and file are derived in part from the "Pose Calib" project by +// @author Pavel Rojtberg +// It is subject to his license terms in the PoseCalibLICENSE file. + +// projects a 2D object (image) according to parameters - generate styled board image +package org.photonvision.calibrator; + +import org.opencv.calib3d.Calib3d; +import org.opencv.core.Core; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.Scalar; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; + +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* BoardPreview class */ +/* BoardPreview class */ +/* BoardPreview class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +class BoardPreview { + private static final Logger logger = new Logger(BoardPreview.class, LogGroup.General); + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* project_img */ + /* project_img */ + /* project_img */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * @param img image to project + * @param sz size of the final image + * @param K + * @param rvec + * @param t + * @param flags + * @return + */ + private static Mat project_img(Mat img, Size sz, Mat K, Mat rvec, Mat t, int flags) + // force user to specify flags=cv2.INTER_LINEAR to use default + { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + // logger.debug("img " + img); + // logger.debug("sz " + sz); + // logger.debug("K " + K + "\n" + K.dump()); + // logger.debug("rvec " + rvec + rvec.dump()); + // logger.debug("t " + t + t.dump()); + // logger.debug("flags " + flags); + + // construct homography + Mat R = new Mat(); + Calib3d.Rodrigues(rvec, R); + + // logger.debug("R " + R + "\n" + R.dump()); + Mat transform = + new Mat(3, 3, CvType.CV_64FC1); // rotation matrix R and a translation matrix T (t) + transform.put( + 0, + 0, + R.get(0, 0)[0], + R.get(0, 1)[0], + R.get(0, 2)[0], // 1st row r,second row r, third row t + R.get(1, 0)[0], + R.get(1, 1)[0], + R.get(1, 2)[0], + t.get(0, 0)[0], + t.get(0, 1)[0], + t.get(0, 2)[0]); + Core.transpose(transform, transform); + Mat H = new Mat(); + Core.gemm(K, transform, 1., new Mat(), 0., H); + Core.divide(H, new Scalar(H.get(2, 2)[0]), H); + + // logger.debug("transform " + transform + "\n" + transform.dump()); + // logger.debug("R " + R + "\n" + R.dump()); + // logger.debug("H " + H + "\n" + H.dump()); + + Mat imgProjected = new Mat(); + + Imgproc.warpPerspective(img, imgProjected, H, sz, flags); + + // draw axes on the warped Guidance Board + // these are drawn in the wrong corner and drawing before the warpPerspective doesn't work - + // tiny image in worse spot + // these axes diagram cover the desired posed (warped) Guidance Board before it is processed. + // Maybe draw them later and they are in a different position + Core.flip( + imgProjected, + imgProjected, + 0); // flip to get axes origin in correct corner BUT the estimated origin is reversed from + // where it belongs + Calib3d.drawFrameAxes( + imgProjected, K, new Mat(), R, t, 300.f); // may need rotation vector rvec instead of R + Core.flip(imgProjected, imgProjected, 0); // put the img back right + + transform.release(); + R.release(); + H.release(); + + // logger.debug("returning imgProjected\n" + ArrayUtils.brief(imgProjected)); + + return imgProjected; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* BoardPreview constructor */ + /* BoardPreview constructor */ + /* BoardPreview constructor */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + private Size SIZE = + new Size( + 640., + 480.); // different than camera res okay and it runs a little faster if smaller. Resize at + // end makes images match + private Size sz; + private Mat img = new Mat(); + private Mat shadow; // used for overlap score + private Mat maps; // 2D version used for remap function + private Mat Knew = new Mat(); + + BoardPreview(Mat img) { + logger.debug("Starting ----------------------------------------"); + + img.copyTo(this.img); + + // at this point the img appears correctly if displayed on a screen or printed + Core.flip(this.img, this.img, 0); // flipped when printing + // at this point the img is flipped upside down. This is needed because the + // Imgproc.warpPerspective + // flips the img upside down likely because it's acting like it's projecting through a camera + // aperture/lens which flips the scene upside down. So after the Imgproc.warpPerspective the img + // is + // again upside right. + + // light to much to see low exposure camera images behind it so reduce almost to nothing. Can't + // set to 0 - messes up other places that check 0 or not 0 + // set black pixels to gray; non-black pixels stay the same + + // process entire Mat for efficiency + byte[] img1ChannelBuff = + new byte + [this.img.rows() + * this.img.cols() + * this.img.channels()]; // temp buffer for more efficient access + this.img.get(0, 0, img1ChannelBuff); // get the row, all channels + for (int index = 0; index < img1ChannelBuff.length; index++) // process each element of the row + { + // if the camera image is dimmer then the guidance board needs to be dimmer. + // if the camera image is bright then the guidance board needs to be bright. + if (img1ChannelBuff[index] == 0) // is it black? + { + img1ChannelBuff[index] = + Cfg.guidanceBlack; // 0 messes up the shadow board logic that relies on non-zero + // pixels. Need major surgery to fix + } else { + img1ChannelBuff[index] = Cfg.guidanceWhite; + } + } + this.img.put(0, 0, img1ChannelBuff); + + Imgproc.cvtColor(this.img, this.img, Imgproc.COLOR_GRAY2BGR); + + // set blue and red channels to black (0) so gray/white becomes a shade of green (green channel + // was not changed) + byte[] img3ChannelBuff = + new byte + [this.img.rows() + * this.img.cols() + * this.img.channels()]; // temp buffers for efficient access + // process one row at a time for efficiency + this.img.get(0, 0, img3ChannelBuff); // get the row, all channels + for (int index = 0; + index < img3ChannelBuff.length; + index += 3) // process each triplet (channels) of the row + { + img3ChannelBuff[index] = 0; // B + img3ChannelBuff[index + 2] = 0; // R + } + this.img.put(0, 0, img3ChannelBuff); + + // used for overlap score + this.shadow = Mat.ones(this.img.rows(), this.img.cols(), CvType.CV_8UC1); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* create_maps */ + /* create_maps */ + /* create_maps */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + void create_maps(Mat K, Mat cdist, Size sz) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + // logger.debug("camera matrix K " + K + "\n" + K.dump()); + // logger.debug("cdist " + cdist.dump()); + // logger.debug("sz " + sz); + + // cdist initialized in its constructor instead of setting to 0 here if null; did 5 not 4 for + // consistency with rest of code + this.sz = sz; + Mat scale = Mat.zeros(3, 3, K.type()); // assuming it's K.rows(), K.cols() + scale.put(0, 0, this.SIZE.width / sz.width); + scale.put(1, 1, this.SIZE.height / sz.height); + scale.put(2, 2, 1.); + // Core.gemm(scale, K, 1., new Mat(), 0.,K); + // FIXME Knew and K are suspect. What's the right answer to making Knew without trashing K? + // Guessing here but seems okay. + Core.gemm(scale, K, 1., new Mat(), 0., this.Knew); + sz = this.SIZE; + this.Knew = + Calib3d.getOptimalNewCameraMatrix( + Knew, cdist, sz, 1.); // .2% higher than older Python OpenCV for same input + + this.maps = Distortion.make_distort_map(Knew, sz, cdist, this.Knew); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* project */ + /* project */ + /* project */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + Mat project(Mat r, Mat t, boolean useShadow, int inter) + // force users to specify useShadow=false and inter=Imgproc.INTER_NEAREST instead of + // defaulting + // no default allowed in Java and I don't feel like making a bunch of overloaded methods for + // this conversion + { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + // logger.debug("r " + r.dump()); + // logger.debug("t " + t.dump()); + // logger.debug("useShadow " + useShadow); + // logger.debug("inter " + inter); + // logger.debug("sz " + this.sz); + + Mat img = new Mat(); + + img = + project_img( + useShadow ? this.shadow : this.img, this.SIZE, this.Knew, r, t, Imgproc.INTER_LINEAR); + + // logger.debug("maps " + this.maps + "\n" + ArrayUtils.brief(maps)); + // Can be one map for XY or two maps X and Y. python had 2 and this has 1 + // Imgproc.remap(img, img, maps[0]/*X*/, maps[1]/*Y*/, inter);// maybe X Mat and Y Mat somehow; + // separate channels? + + Imgproc.remap( + img, img, this.maps, new Mat(), + inter); // 1st arg can be XY with no 2nd arg (original has separate X and Y arguments) + // logger.debug("img after remap " + img + "\n" + ArrayUtils.brief(img)); + + // maps (2, 480, 640) + Imgproc.resize(img, img, this.sz, 0, 0, inter); + + // logger.debug("returning img after resize " + img + "\n" + ArrayUtils.brief(img)); + + return img; + } +} +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* End BoardPreview class */ +/* End BoardPreview class */ +/* End BoardPreview class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ diff --git a/photon-core/src/main/java/org/photonvision/calibrator/Calibrator.java b/photon-core/src/main/java/org/photonvision/calibrator/Calibrator.java new file mode 100644 index 0000000000..6562f649c9 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/calibrator/Calibrator.java @@ -0,0 +1,506 @@ +/* + * 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 . + */ + +// This project and file are derived in part from the "Pose Calib" project by +// @author Pavel Rojtberg +// It is subject to his license terms in the PoseCalibLICENSE file. + +package org.photonvision.calibrator; + +import java.util.ArrayList; +import java.util.List; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.Core; +import org.opencv.core.CvException; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.Size; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; + +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* Calibrator class */ +/* Calibrator class */ +/* Calibrator class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +class Calibrator { + private static final Logger logger = new Logger(Calibrator.class, LogGroup.General); + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* Calibrator constructor */ + /* Calibrator constructor */ + /* Calibrator constructor */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + private Size img_size; + private int nintr = 9; // "fx", "fy", "cx", "cy", "k1", "k2", "p1", "p2", "k3" + // unknowns not used + private Mat Kin; + private Mat K = new Mat(); + private Mat cdist = Mat.zeros(1, 5, CvType.CV_64FC1); + private int flags = Calib3d.CALIB_USE_LU; + + // calibration data + List keyframes = new ArrayList<>(20); + // private int N_pts = 0; // not used - only for Jacobian covariance that was removed + private double reperr = Double.NaN; + private double[] varIntrinsics = {0., 0., 0., 0., 0., 0., 0., 0., 0.}; // nintr length + private double[] pose_var = {0., 0., 0., 0., 0., 0.}; // rotation x, y, z and translation x, y, z + private double[] + disp_idx = // index of dispersion. One for each intrinsic this program handles (nintr=9) + { + Double.NaN, + Double.NaN, + Double.NaN, + Double.NaN, + Double.NaN, + Double.NaN, + Double.NaN, + Double.NaN, + Double.NaN + }; + + // getters + Size img_size() { + return this.img_size; + } + + int nintr() { + return this.nintr; + } + + double reperr() { + return this.reperr; + } + + double[] varIntrinsics() { + return this.varIntrinsics; + } + + double[] pose_var() { + return this.pose_var; + } + + int flags() { + return this.flags; + } + + Mat K() { + return this.K; + } + + Mat cdist() { + return this.cdist; + } + + Calibrator(Size img_size) { + logger.debug("Starting ----------------------------------------"); + this.img_size = img_size; + // initial fake camera matrix to get things started + // initial K matrix + // with aspect ratio of 1 and pp at center. Focal length is empirical. + this.Kin = Mat.zeros(3, 3, CvType.CV_64FC1); + this.Kin.put(0, 0, Cfg.initialFocalLength); + this.Kin.put(1, 1, Cfg.initialFocalLength); + this.Kin.put(2, 2, 1.); + this.Kin = Calib3d.getDefaultNewCameraMatrix(this.Kin, img_size, true); + this.Kin.copyTo(this.K); + // logger.debug("K/Kin\n" + this.K.dump() + "\n" + this.Kin.dump()); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* get_intrinsics */ + /* get_intrinsics */ + /* get_intrinsics */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + private double[] get_intrinsics() { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + double[] intrinsics = { + this.K.get(0, 0)[0], + this.K.get(1, 1)[0], + this.K.get(0, 2)[0], + this.K.get(1, 2)[0], + this.cdist.get(0, 0)[0], + this.cdist.get(0, 1)[0], + this.cdist.get(0, 2)[0], + this.cdist.get(0, 3)[0], + this.cdist.get(0, 4)[0] + }; + // logger.debug("K\n" + K.dump()); + // logger.debug(java.util.Arrays.toString(intrinsics)); + return intrinsics; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* calibrate */ + /* calibrate */ + /* calibrate */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + double[] calibrate(List keyframes) + // force use of keyframes instead of default None + { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + int flags = this.flags; + + if (keyframes.isEmpty()) // pose #1 hasn't been captured yet + { + keyframes.addAll( + this.keyframes); // gives size 1 keyframe for the first 2 poses so hard to tell the 2nd + // pose is in action since it's still #1 + } + + if (keyframes.isEmpty()) { + logger.error("keyframes empty?"); + } + + int nkeyframes = keyframes.size(); + + // initialization process + if (nkeyframes <= 1) + // first pose hasn't been captured yet so it's 0 + the initial keyframe from above making 1 + // second pose has the previous captured pose #1 and the second pose hasn't been captured yet + // making it #1, also. + // thus, RKT saved the pose that was last used. + // Probably could have determined what to do by saving the number of keyframes on entry to this + // method the first is 0 and the second is 1. + + // The flags were modified from the original under the assumption that this is what the author + // meant to do but could not get it to + // work right because of a (likely) bug that trashed the initial K values. Hope the two fixes + // are right - seems to work better this way. + { + switch (PoseGeneratorDist.pose) { + case ORBITAL: + // restrict early orbital calibrations to K matrix parameters and don't mess with + // distortion + flags |= Calib3d.CALIB_FIX_ASPECT_RATIO; + flags |= + Calib3d.CALIB_ZERO_TANGENT_DIST + | Calib3d.CALIB_FIX_K1 + | Calib3d.CALIB_FIX_K2 + | Calib3d.CALIB_FIX_K3; + break; + + case PLANAR_FULL_SCREEN: + // restrict early planar calibrations to distortion and don't mess with focal length + flags |= Calib3d.CALIB_FIX_PRINCIPAL_POINT | Calib3d.CALIB_FIX_FOCAL_LENGTH; + break; + + default: + logger.error("unknown initial pose " + PoseGeneratorDist.pose); + break; + } + } + + calibrateCameraReturn res = calibrateCamera(keyframes, this.img_size, flags, this.Kin); + + this.reperr = res.reperr; + res.K.copyTo(this.K); + res.cdist.copyTo(this.cdist); + List rvecsList = res.rvecsList; + List tvecsList = res.tvecsList; + Mat variances = res.varianceIntrinsics; + + variances.get( + 0, 0, this.varIntrinsics); // convert from Mat to double array; this.varIntrinsics replaces + // self.PCov in Python + + this.pose_var = compute_pose_var(rvecsList, tvecsList); + + this.disp_idx = index_of_dispersion(this.get_intrinsics(), this.varIntrinsics); + + return this.disp_idx; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* index_of_dispersion */ + /* index_of_dispersion */ + /* index_of_dispersion */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + private static double[] index_of_dispersion(double[] mean, double[] variance) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + // computes index of dispersion: + // https://en.wikipedia.org/wiki/Index_of_dispersion + // compute the 9 VMR's + if (mean.length != variance.length) { + logger.error("mean and variance not the same size"); + } + double[] VMR = new double[9]; + for (int i = 0; i < mean.length; i++) { + VMR[i] = variance[i] / (Math.abs(mean[i]) > 0 ? Math.abs(mean[i]) : 1.); + } + + return VMR; + } + + /** + * mean_extr_var and estimate_pt_std NOT USED -- NOT CONVERTED mean_extr_var and estimate_pt_std + * NOT USED -- NOT CONVERTED mean_extr_var and estimate_pt_std NOT USED -- NOT CONVERTED + */ + + /** + * Jc2J and compute_state_cov replaced by OpenCV std dev -- NOT CONVERTED Jc2J and + * compute_state_cov replaced by OpenCV std dev -- NOT CONVERTED Jc2J and compute_state_cov + * replaced by OpenCV std dev -- NOT CONVERTED + */ + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* compute_pose_var */ + /* compute_pose_var */ + /* compute_pose_var */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * Compute variances and prints the euler angles in degrees for debugging + * + * @param rvecs data 1 - List of triplets + * @param tvecs data 2 - List of triplets + * @return variances of data 1 and data 2 - a sextet + */ + private static double[] compute_pose_var(List rvecs, List tvecs) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + double[] ret = new double[6]; // return + + List reulers = new ArrayList<>(50); + + // temporary Mats for inside loop but create them only once for efficiency + Mat dst = new Mat(); + Mat mtxR = new Mat(); + Mat mtxQ = new Mat(); + + for (Mat r : rvecs) { + Calib3d.Rodrigues(r, dst); + double[] reuler = Calib3d.RQDecomp3x3(dst, mtxR, mtxQ); // always returns reuler.length = 3 + // logger.debug("\nreuler degrees " + java.util.Arrays.toString(reuler) + "\nr " + + // r.t().dump()); + // workaround for the given board so r_x does not oscilate between +-180° + reuler[0] = reuler[0] % 360.; + reulers.add(reuler); + } + + double[] varReuler = simpleVariance(reulers); + + for (int i = 0; i < 3; i++) { + ret[i] = varReuler[i]; + } + + List translations = new ArrayList<>(50); + for (Mat t : tvecs) { + double[] tvec = new double[3]; + t.get(0, 0, tvec); + + for (int i = 0; i < 3; i++) { + tvec[i] /= + 10.; // [mm] (not sure if this is the unity measure of arbitrary but consistent units + // or millimeters; why divide by 10?) + } + translations.add(tvec); + } + + double[] varTvecs = simpleVariance(translations); + for (int i = 3; i < 6; i++) { + ret[i] = varTvecs[i - 3]; + } + + dst.release(); + mtxR.release(); + mtxQ.release(); + + return ret; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* simpleVariance */ + /* simpleVariance */ + /* simpleVariance */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * Variance of each axis of a list of 3-D points [Could be generalized to any number of dimensions + * with little change.] + * + * @param data + * @return variances in a 3 element array + */ + private static double[] simpleVariance(List data) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + // always 3 components x, y, z so do all 3 at once + double[] sum = {0., 0., 0.}; + double[] sumSqr = {0., 0., 0.}; + for (double[] datum : data) { + for (int i = 0; i < 3; i++) { + sum[i] += datum[i]; + } + } + + double[] mean = {sum[0] / data.size(), sum[1] / data.size(), sum[2] / data.size()}; + + for (double[] datum : data) { + for (int i = 0; i < 3; i++) { + sumSqr[i] += Math.pow(datum[i] - mean[i], 2); + } + } + + double[] variance = {sumSqr[0] / data.size(), sumSqr[1] / data.size(), sumSqr[2] / data.size()}; + + return variance; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* calibrateCamera */ + /* calibrateCamera */ + /* calibrateCamera */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * @param keyframes + * @param img_size + * @param flags + * @param K + * @return + * @throws Exception + */ + calibrateCameraReturn calibrateCamera(List keyframes, Size img_size, int flags, Mat K) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + // split keyframes into its two separate components, image points and object points, for OpenCV + // calibrateCamera + // we put them together when detected then we take them apart for calibration. + + List pts2dFrames = new ArrayList<>(40); // image points + List pts3dFrames = new ArrayList<>(40); // object points + int N = 0; // count total number of points + + for (keyframe keyframe : keyframes) { + pts2dFrames.add(keyframe.p2d()); + pts3dFrames.add(keyframe.p3d()); + N += + keyframe + .p2d() + .rows(); // total points - all frames (poses/views) and all points in those poses + } + + if (N <= 4) { + logger.error("not enough total points"); + return new calibrateCameraReturn(); + } + + Mat cdist = new Mat(); + List rvecs = new ArrayList<>(); + List tvecs = new ArrayList<>(); + Mat stdDeviationsIntrinsics = new Mat(); + double reperr = Double.NaN; + // logger.debug("K input to calibration\n" + K.dump()); + // logger.debug(UserGuidance.formatFlags(flags)); + try { + reperr = + Calib3d.calibrateCameraExtended( + pts3dFrames, + pts2dFrames, + img_size, + K, + cdist, + rvecs, + tvecs, + stdDeviationsIntrinsics, + new Mat(), + new Mat(), + flags, + Cfg.calibrateCameraCriteria); + + // logger.debug("camera matrix K " + K + "\n" + K.dump()); + // logger.debug("distortion coefficients " + cdist.dump() + cdist); + // logger.debug("repError " + reperr); + } catch (CvException error) { + logger.error("Calib3d.calibrateCameraExtended error", error); + } + // logger.debug("K output from calibration\n" + K.dump()); + Mat varianceIntrinsics = new Mat(); + Core.multiply( + stdDeviationsIntrinsics, + stdDeviationsIntrinsics, + varianceIntrinsics); // variance = stddev squared + + // logger.debug("cdist " + cdist.dump() + ", N = " + N); + + stdDeviationsIntrinsics.release(); + + return new calibrateCameraReturn(reperr, K, cdist, rvecs, tvecs, varianceIntrinsics); + } + + class calibrateCameraReturn { + double reperr; + Mat K; + Mat cdist; + List rvecsList; + List tvecsList; + Mat varianceIntrinsics; + + calibrateCameraReturn() {} + + calibrateCameraReturn( + double reperr, Mat K, Mat cdist, List rvecs, List tvecs, Mat varianceIntrinsics) { + this.reperr = reperr; + this.K = K; + this.cdist = cdist; + this.rvecsList = rvecs; + this.tvecsList = tvecs; + this.varianceIntrinsics = varianceIntrinsics; + // N_pts not used + } + } +} +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* End Calibrator class */ +/* End Calibrator class */ +/* End Calibrator class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ diff --git a/photon-core/src/main/java/org/photonvision/calibrator/Cfg.java b/photon-core/src/main/java/org/photonvision/calibrator/Cfg.java new file mode 100644 index 0000000000..0172eb67e5 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/calibrator/Cfg.java @@ -0,0 +1,147 @@ +/* + * 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 . + */ + +// This project and file are derived in part from the "Pose Calib" project by +// @author Pavel Rojtberg +// It is subject to his license terms in the PoseCalibLICENSE file. + +package org.photonvision.calibrator; + +import edu.wpi.first.cscore.VideoMode.PixelFormat; +import org.opencv.core.Scalar; +import org.opencv.core.TermCriteria; +import org.opencv.objdetect.Objdetect; + +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* Cfg class */ +/* Cfg class */ +/* Cfg class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +public class Cfg { + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* Cfg constructor */ + /* Cfg constructor */ + /* Cfg constructor */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + // SHOULD BE USER SPECIFIED + static final boolean writeBoard = true; + static final String boardFile = "ChArUcoBoard"; + static boolean logDetectedCorners = false; // save to files captured images and detected corners + static String cornersLog = "corners.vnl"; + static int resXDPM = 9843; // board printing pixels per meter 9843 = 250 DPI + static int resYDPM = 9843; // board printing pixels per meter 9843 = 250 DPI + /////////////////////////// + + // SHOULD COME FROM PV CAMERA + // Checks for the specified camera - laptop internal or USB external and uses it if present. + // 0 internal if no external or if external connected after boot-up + // 0 external if connected at boot-up + // 1 internal if external connected at boot-up + // 1 is external if connected after boot-up + static int camId = 0; + static PixelFormat pixelFormat = PixelFormat.kYUYV; + static final int displayPort = 1185; + static int fps = 10; + static final int exposureManual = 70; + static final int brightness = 70; + + // a few icky int-float-double conversion scattered throughout the program. + // camera image size and thus user display screen size + public static int image_width = 1280; + public static int image_height = 720; + ///////////////////////////// + + // ALL THE FOLLOWING STUFF MIGHT NEVER NEED TO BE CHANGED + static final int garbageCollectionFrames = + 500; // camera frames - periodically do garbage collection because Java doesn't know there are + // big Mats to be released + static final double initialFocalLength = 1000.; // fx and fy, aspect ratio = 1 (fy/fx) + // ChArUco Board pixels = (board_x*square_len, board_y*square_len) + static final int board_x = 9; + static final int board_y = 6; + static final int square_len = 280; + static final int marker_len = 182; + static final int dictionary = 0; + // intensity of the green guidance board + // suggest "white" [-100]; (dull) -128 to -1 (bright) + // suggest "black" [1]; (somewhat transparent) 1 to 64 (more obscuring) + static final byte guidanceWhite = + -50; // green actually; (black) 0 to 127 (medium), (medium) -128 to -1 (bright) + static final byte guidanceBlack = + 1; // ; (dark) 1 to 127, -128 to -1 (bright); must be much less than guidanceWhite and NOT 0 + static final Scalar progressInsertCameraGrey = new Scalar(170.); + static final Scalar progressInsertGuidanceGrey = new Scalar(105.); + + // config for convergence criteria + static final int pt_min_markers = 1; + static final boolean tryRefineMarkers = true; + static final int cornerRefinementMaxIterations = 2000; + static final int cornerRefinementMethod = Objdetect.CORNER_REFINE_CONTOUR; + static final boolean checkAllOrders = true; + static final float errorCorrectionRate = 3.0f; + static final float minRepDistance = 10.0f; + + static final double mean_flow_max = 3.; // exclusive, larger is more movement allowed + static final double pose_close_to_tgt_min = + 0.85; // exclusive, - minimum Jaccard score between shadow and actual img for auto capture; + // larger is less deviation allowed + static final double MAX_OVERLAP = + 0.9; // maximum fraction of distortion mask overlapping with this pose before pose considered + // not contributing enough to help fill distortion mask + static final double minCorners = + 6; // min for solvePnP (original needed 4 (or 5 w/o solvePnP)) but another place requires many + // more + public static final double var_terminate = + 0.1; // min variance to terminate an intrinsic's iterations [mm is whatever unit of measure?] + + static final double DBL_EPSILON = Math.ulp(1.); + static final TermCriteria calibrateCameraCriteria = + new TermCriteria(TermCriteria.COUNT + TermCriteria.EPS, 30, DBL_EPSILON); + + static final float FLT_EPSILON = Math.ulp(1.f); + static final TermCriteria solvePnPRefineVVSCriteria = + new TermCriteria(TermCriteria.COUNT + TermCriteria.EPS, 20, FLT_EPSILON); + static final double solvePnPRefineVVSLambda = 1.; + + static final TermCriteria undistortPointsIterCriteria = + new TermCriteria( + TermCriteria.COUNT + TermCriteria.EPS, + 20, + FLT_EPSILON); // default cv::TermCriteria(cv::TermCriteria::COUNT, 5, 0.01) + + ///////////////////////////////////////////////////////// + private Cfg() { + throw new UnsupportedOperationException("This is a utility class"); + } +} +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* End Cfg class */ +/* End Cfg class */ +/* End Cfg class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ diff --git a/photon-core/src/main/java/org/photonvision/calibrator/ChArucoDetector.java b/photon-core/src/main/java/org/photonvision/calibrator/ChArucoDetector.java new file mode 100644 index 0000000000..7fb80c47ba --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/calibrator/ChArucoDetector.java @@ -0,0 +1,756 @@ +/* + * 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 . + */ + +// This project and file are derived in part from the "Pose Calib" project by +// @author Pavel Rojtberg +// It is subject to his license terms in the PoseCalibLICENSE file. + +package org.photonvision.calibrator; + +import java.io.File; +import java.io.FileNotFoundException; +import java.io.FileOutputStream; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; +import java.util.zip.CRC32; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.MatOfDouble; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.MatOfPoint3f; +import org.opencv.core.Scalar; +import org.opencv.core.Size; +import org.opencv.objdetect.CharucoBoard; +import org.opencv.objdetect.CharucoDetector; +import org.opencv.objdetect.CharucoParameters; +import org.opencv.objdetect.DetectorParameters; +import org.opencv.objdetect.Dictionary; +import org.opencv.objdetect.Objdetect; +import org.opencv.objdetect.RefineParameters; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; + +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* ChArucoDetector class */ +/* ChArucoDetector class */ +/* ChArucoDetector class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +public class ChArucoDetector { + private static final Logger logger = new Logger(ChArucoDetector.class, LogGroup.General); + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* ChArucoDetector constructor */ + /* ChArucoDetector constructor */ + /* ChArucoDetector constructor */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + // configuration + private Size board_sz = new Size(Cfg.board_x, Cfg.board_y); + private double square_len = Cfg.square_len; + private double marker_len = Cfg.marker_len; + Size img_size = new Size(Cfg.image_width, Cfg.image_height); + + // per frame data + // p3d is the object coordinates of the perfect undistorted ChArUco Board corners that the camera + // is pointing at. + // In this case the board is flat at its Z origin (say, the wall upon which it is mounted) so the + // Z coordinate is always 0. + // p2d is the coordinates of the corresponding board corners as they are located in the camera + // image, + // distorted by perspective (pose) and camera intrinsic parameters and camera distortion. + private final Mat p3d = + new Mat(); // 3 dimensional currentObjectPoints, the physical target ChArUco Board + private final Mat p2d = + new Mat(); // 2 dimensional currentImagePoints, the likely distorted board on the flat camera + // sensor frame posed relative to the target + private int N_pts = 0; + private boolean pose_valid = false; + // private Mat raw_img = null; // not used + + /// Charuco Board + private final Dictionary dictionary = Objdetect.getPredefinedDictionary(Objdetect.DICT_4X4_50); + private final Size boardImageSize = + new Size(Cfg.board_x * Cfg.square_len, Cfg.board_y * Cfg.square_len); + final Mat boardImage = new Mat(); + private final CharucoBoard board = + new CharucoBoard(this.board_sz, Cfg.square_len, Cfg.marker_len, this.dictionary); + private CharucoDetector + detector; // the OpenCV detector spelled almost the same - fooled me too many times!!!!! + + private Mat rvec = new Mat(); + private Mat tvec = new Mat(); + + private boolean intrinsic_valid = false; + private Mat K; + private Mat cdist; + + private final Mat ccorners = new Mat(); // currentCharucoCorners + private final Mat cids = new Mat(); // currentCharucoIds + + // optical flow calculation + private Mat last_ccorners = new Mat(); // previous ChArUcoBoard corners + private Mat last_cids = new Mat(); // previous ChArUcoBoard ids + private double mean_flow = + Double.MAX_VALUE; // mean flow of the same corners that are detected in consecutive frames + + // (relaxed from original) + + // getters + CharucoBoard board() { + return board; + } + + int N_pts() { + return N_pts; + } + + public Size board_sz() { + return board_sz; + } + + boolean pose_valid() { + return this.pose_valid; + } + + Mat rvec() { + return rvec; + } + + Mat tvec() { + return tvec; + } + + double mean_flow() { + return this.mean_flow; + } + + Mat ccorners() { + return ccorners; + } + + Mat cids() { + return cids; + } + + public ChArucoDetector() throws FileNotFoundException, IOException { + logger.debug("Starting ----------------------------------------"); + + /// create board + this.board.generateImage(this.boardImageSize, this.boardImage); + + if (Cfg.writeBoard) { + // write ChArUco Board to file for print to use for calibration + + /* PNG */ + final String boardFilePNG = Cfg.boardFile + ".png"; + FileOutputStream outputStreamPNG = new FileOutputStream(new File(boardFilePNG)); + logger.info("ChArUcoBoard to be printed is in file " + boardFilePNG); + + byte[] boardByte = + new byte + [this.boardImage.rows() + * this.boardImage + .cols()]; // assumes 1 channel Mat [ 1680*2520*CV_8UC1, isCont=true, + // isSubmat=false, nativeObj=0x294e475cc20, dataAddr=0x294e55f7080 ] + + CRC32 crc32 = new CRC32(); + + // SIGNATURE + final byte[] signaturePNG = { + (byte) 0x89, + (byte) 0x50, + (byte) 0x4e, + (byte) 0x47, + (byte) 0x0d, + (byte) 0x0a, + (byte) 0x1a, + (byte) 0x0a // PNG magic number + }; + outputStreamPNG.write(signaturePNG); + + // HEADER + byte[] IHDR = { + (byte) 0x00, + (byte) 0x00, + (byte) 0x00, + (byte) 0x0d, // length + (byte) 0x49, + (byte) 0x48, + (byte) 0x44, + (byte) 0x52, // IHDR + (byte) 0x00, + (byte) 0x00, + (byte) 0x00, + (byte) 0x00, // data width place holder + (byte) 0x00, + (byte) 0x00, + (byte) 0x00, + (byte) 0x00, // data height place holder + (byte) 0x08, // bit depth + (byte) 0x00, // color type - grey scale + (byte) 0x00, // compression method + (byte) 0x00, // filter method (default/only one?) + (byte) 0x00, // interlace method + (byte) 0x00, + (byte) 0x00, + (byte) 0x00, + (byte) 0x00 // crc place holder + }; + // fetch the length data for the IHDR + int ihdrWidthOffset = 8; + int ihdrHeightOffset = 12; + ArrayUtils.intToByteArray(boardImage.cols(), IHDR, ihdrWidthOffset); + ArrayUtils.intToByteArray(boardImage.rows(), IHDR, ihdrHeightOffset); + + crc32.reset(); + crc32.update( + IHDR, 4, IHDR.length - 8); // skip the beginning 4 for length and ending 4 for crc + ArrayUtils.intToByteArray((int) crc32.getValue(), IHDR, IHDR.length - 4); + outputStreamPNG.write(IHDR); + + // PHYSICAL RESOLUTION + byte[] PHYS = // varies with the requested resolution [pixels per meter] + { + (byte) 0x00, + (byte) 0x00, + (byte) 0x00, + (byte) 0x09, // length + (byte) 0x70, + (byte) 0x48, + (byte) 0x59, + (byte) 0x73, // pHYs + (byte) 0x00, + (byte) 0x00, + (byte) 0x00, + (byte) 0x00, // x res [pixels per unit] place holder + (byte) 0x00, + (byte) 0x00, + (byte) 0x00, + (byte) 0x00, // y res [pixels per unit] place holder + (byte) 0x01, // units [unit is meter] + (byte) 0x00, + (byte) 0x00, + (byte) 0x00, + (byte) 0x00 // crc place holder + }; + int physXresOffset = 8; + int physYresOffset = 12; + ArrayUtils.intToByteArray(Cfg.resXDPM, PHYS, physXresOffset); + ArrayUtils.intToByteArray(Cfg.resYDPM, PHYS, physYresOffset); + + crc32.reset(); + crc32.update( + PHYS, 4, PHYS.length - 8); // skip the beginning 4 for length and ending 4 for crc + ArrayUtils.intToByteArray((int) crc32.getValue(), PHYS, PHYS.length - 4); + outputStreamPNG.write(PHYS); + + // DATA + // The complete filtered PNG image is represented by a single zlib datastream that is stored + // in a number of IDAT chunks. + + // create the filtered, compressed datastream + + boardImage.get(0, 0, boardByte); // board from OpenCV Mat + + // filter type begins each row so step through all the rows adding the filter type to each row + byte[] boardByteFilter = new byte[boardImage.rows() + boardByte.length]; + int flatIndex = 0; + int flatIndexFilter = 0; + for (int row = 0; row < boardImage.rows(); row++) { + boardByteFilter[flatIndexFilter++] = 0x00; // filter type none begins each row + for (int col = 0; col < boardImage.cols(); col++) { + boardByteFilter[flatIndexFilter++] = boardByte[flatIndex++]; + } + } + // complete filtered PNG image is represented by a single zlib compression datastream + byte[] boardCompressed = ArrayUtils.compress(boardByteFilter); + + // chunk the compressed datastream + // chunking not necessary for the ChArUcoBoard but it's potentially good for other uses + int chunkSize = 0; + int chunkSizeMax = 100_000; // arbitrary "small" number + int dataWritten = 0; + + while (dataWritten < boardCompressed.length) // chunk until done + { + chunkSize = + Math.min( + chunkSizeMax, + boardCompressed.length - dataWritten); // max or what's left in the last chunk + + byte[] IDAT = new byte[4 + 4 + chunkSize + 4]; // 4 length + 4 "IDAT" + chunk length + 4 CRC + + ArrayUtils.intToByteArray( + chunkSize, IDAT, 0); // stash length of the chunk data in first 4 bytes + IDAT[4] = (byte) ("IDAT".charAt(0)); + IDAT[5] = (byte) ("IDAT".charAt(1)); + IDAT[6] = (byte) ("IDAT".charAt(2)); + IDAT[7] = (byte) ("IDAT".charAt(3)); + for (int i = 0; i < chunkSize; i++) { + IDAT[8 + i] = + boardCompressed[ + dataWritten + i]; // stash data from where we left off to its place in the chunk + } + + crc32.reset(); + crc32.update( + IDAT, 4, IDAT.length - 8); // skip the beginning 4 for length and ending 4 for crc + ArrayUtils.intToByteArray( + (int) crc32.getValue(), IDAT, IDAT.length - 4); // crc in last 4 bytes + + outputStreamPNG.write(IDAT); + dataWritten += chunkSize; + } + + // END + final byte[] IEND = { + (byte) 0x00, (byte) 0x00, (byte) 0x00, (byte) 0x00, // length + (byte) 0x49, (byte) 0x45, (byte) 0x4e, (byte) 0x44, // IEND + (byte) 0xae, (byte) 0x42, (byte) 0x60, (byte) 0x82 // crc + }; + + outputStreamPNG.write(IEND); + + outputStreamPNG.close(); + } + /// end create board + + /// board detector + final DetectorParameters detectParams = new DetectorParameters(); + final RefineParameters refineParams = new RefineParameters(); + final CharucoParameters charucoParams = new CharucoParameters(); + + charucoParams.set_minMarkers(Cfg.pt_min_markers); // 2 default + charucoParams.set_tryRefineMarkers(Cfg.tryRefineMarkers); // false default + // charucoParams.set_cameraMatrix(); + // charucoParams.set_distCoeffs(); + detectParams.set_cornerRefinementMaxIterations(Cfg.cornerRefinementMaxIterations); // 30 default + detectParams.set_cornerRefinementMethod(Cfg.cornerRefinementMethod); // 0 default + refineParams.set_checkAllOrders(Cfg.checkAllOrders); // true default + refineParams.set_errorCorrectionRate(Cfg.errorCorrectionRate); // 3.0 default + refineParams.set_minRepDistance(Cfg.minRepDistance); // 10.0 default + + detector = new CharucoDetector(this.board, charucoParams, detectParams, refineParams); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* set_intrinsics */ + /* set_intrinsics */ + /* set_intrinsics */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + public void set_intrinsics(Calibrator calib) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + this.intrinsic_valid = true; + this.K = calib.K(); + this.cdist = calib.cdist(); + // logger.debug("K\n" + this.K.dump() + "\n" + calib.K().dump()); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* draw_axis */ + /* draw_axis */ + /* draw_axis */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * Draw axes on the detected ChArUcoBoard from the camera image + * + * @param img + */ + public void draw_axis(Mat img) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + Calib3d.drawFrameAxes( + img, this.K, this.cdist, this.rvec, this.tvec, (float) this.square_len * 2.5f, 2); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* detect_pts */ + /* detect_pts */ + /* detect_pts */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + public void detect_pts(Mat img) throws Exception { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + final List markerCorners = new ArrayList<>(); + final Mat markerIds = new Mat(); + this.N_pts = 0; + this.mean_flow = Double.MAX_VALUE; + + try { + detector.detectBoard(img, this.ccorners, this.cids, markerCorners, markerIds); + } catch (Exception e) // shouldn't happen + { + logger.error( + "detectBoard error\n" + img + "\n" + this.ccorners.dump() + "\n" + this.cids.dump(), e); + return; // skipping this image frame + } + + // double check detect results since there was some unknown rare failure to get the N_pts set + // right + if (this.cids.rows() != this.ccorners.rows()) // shouldn't happen + { + logger.error( + "detectBoard has inconsistent number of outputs\n" + + this.ccorners.dump() + + "\n" + + this.cids.dump()); + return; // skipping this image frame + } + + if (!this.cids + .empty()) // normal that points might not be detected (not aiming at board) so check before + // using them + { + this.N_pts = this.cids.rows(); + } + + // logger.debug("N_pts " + this.N_pts); + + if (this.N_pts <= 0) // the less than shouldn't happen (maybe use the min N_pts from Cfg?) + { + return; // skipping this image frame + } + + // logger.debug("detected ccorners\n" + this.ccorners.dump()); + // logger.debug("detected cids\n" + this.cids.dump()); + + // reformat the Mat to a List for matchImagePoints + final List ccornersList = new ArrayList<>(); + for (int i = 0; i < this.ccorners.total(); i++) { + ccornersList.add(this.ccorners.row(i)); + } + + // display the detected cids on the board (debugging) + // Objdetect.drawDetectedCornersCharuco(img, ccorners, cids); + + board.matchImagePoints( + ccornersList, this.cids, this.p3d, this.p2d); // p2d same data as ccornersList + // oddly this method returns 3 channels instead of 2 for imgPoints and there isn't much to do + // about it and it works in solvePnP + // after copying to MatOfPoint2f. A waste of cpu and memory. + + // logger.debug("p3d\n" + this.p3d.dump()); // data okay here + // logger.debug("p2d\n" + this.p2d.dump()); // data okay here + + if (this.p3d.empty() || this.p2d.empty()) // shouldn't happen + { + this.N_pts = 0; + this.mean_flow = Double.MAX_VALUE; + logger.error("p3d or p2d empty from matchImagePoints"); + return; // skipping this image frame + } + + // compute mean flow of the image for the check for stillness elsewhere + computeMeanFlow(); + + this.ccorners.copyTo(this.last_ccorners); + this.cids.copyTo(this.last_cids); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* computeMeanFlow */ + /* computeMeanFlow */ + /* computeMeanFlow */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * Compare current and previous corner detections to see how far they are displaced from each + * other - flow The lists of current and previous corners must be the same and then the + * displacement is computed + */ + public void computeMeanFlow() { + // cids: int 1 col, 1 channel (cid); ccorners: float 1 col, 2 channels (x, y) + + this.mean_flow = Double.MAX_VALUE; // first assume default flow is big + + if (cids.rows() <= 0 + || cids.rows() != last_cids.rows()) // handle first time or number of rows differ + { + return; // cids lists lengths differ so can't compute flow so assume it's big + } + + // lists' lengths are the same so check the individual elements (cids) for equality + + // get all the last cids and ccorners and all the current cids and ccorners in arrays. + // do all the computations in the arrays. + + // check that the lists of current and previous cids match + // assume the cids and last_cids are in the same order (it's ascending but that doesn't matter) + + int[] last_cidsArray = new int[last_cids.rows()]; // assume 1 col 1 channel + int[] cidsArray = new int[cids.rows()]; + + this.last_cids.get(0, 0, last_cidsArray); + this.cids.get(0, 0, cidsArray); + + for (int row = 0; row < cidsArray.length; row++) { + if (cidsArray[row] != last_cidsArray[row]) { + return; // cids differ so can't compute flow so assume it's big + } + } + + // previous and current cids lists match so compute flow of each corner + + float[] last_ccornersArray = + new float[last_ccorners.rows() * last_ccorners.channels()]; // assume 1 col + float[] ccornersArray = new float[ccorners.rows() * ccorners.channels()]; + + this.last_ccorners.get(0, 0, last_ccornersArray); + this.ccorners.get(0, 0, ccornersArray); + + this.mean_flow = + 0.; // starting at 0 for a summation process but that will change to flow or max value + for (int rowChannel = 0; + rowChannel < ccornersArray.length; + rowChannel += 2) // step by 2 assumes 2 channels (x, y) per point + { + double diffX = + ccornersArray[rowChannel] - last_ccornersArray[rowChannel]; // X channel (current - last) + double diffY = + ccornersArray[rowChannel + 1] + - last_ccornersArray[rowChannel + 1]; // Y channel (current - last) + + this.mean_flow += + Math.sqrt(Math.pow(diffX, 2) + Math.pow(diffY, 2)); // sum the L2 norm (Frobenious) + } + + this.mean_flow /= ccornersArray.length; // mean of the sum of the norms + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* detect */ + /* detect */ + /* detect */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + public void detect(Mat img) { + try { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + // raw_img never used - not converted + this.detect_pts(img); + + if (this.intrinsic_valid) { + this.update_pose(); + } + } catch (Exception e) { + logger.error("WTF", e); + } + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* get_pts3d */ + /* get_pts3d */ + /* get_pts3d */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + public Mat get_pts3d() { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + return this.p3d; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* get_calib_pts */ + /* get_calib_pts */ + /* get_calib_pts */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + public keyframe get_calib_pts() { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + return new keyframe(this.ccorners.clone(), this.get_pts3d().clone()); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* update_pose */ + /* update_pose */ + /* update_pose */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + public void update_pose() throws Exception { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + if (this.N_pts + < Cfg.minCorners) // original had 4; solvePnp wants 6 sometimes, and UserGuidance wants many + // more + { + // logger.debug("too few corners " + (this.N_pts == 0 ? "- possibly blurred by movement or bad + // aim" : this.N_pts)); + Main.fewCorners = true; + this.pose_valid = false; + return; + } + + MatOfPoint3f p3dReTyped = new MatOfPoint3f(this.p3d); + MatOfPoint2f p2dReTyped = new MatOfPoint2f(this.p2d); + MatOfDouble distReTyped = new MatOfDouble(this.cdist); + + // logger.debug("p3d\n" + p3dReTyped.dump()); + // logger.debug("p2d\n" + p2dReTyped.dump()); + + Mat rvec = + new Mat(); // neither previous pose nor guidance board pose helped the solvePnP (made pose + // estimate worse) + Mat tvec = new Mat(); // so don't give solvePnP a starting pose estimate + + Mat inLiers = new Mat(); + + this.pose_valid = + Calib3d.solvePnPRansac( + p3dReTyped, + p2dReTyped, + this.K, + distReTyped, + rvec, + tvec, + false, + 100, + 8.0f, + 0.99, + inLiers, + Calib3d.SOLVEPNP_ITERATIVE); + + // logger.debug("inliers " + inLiers.rows() + " of " + p3dReTyped.rows() + " " + inLiers); + + if (!this.pose_valid) { + // logger.debug("pose not valid"); + return; + } + + // remove outliers code below commented out because it didn't seem to help. Could be resurrected + // but needs to be tested better. + // compress the object and image mats with only the in liers + // if the same use the original mats if inliers < all then Compression + + // if (inLiers.rows() == p3dReTyped.rows()) + // { + Calib3d.solvePnPRefineVVS( + p3dReTyped, + p2dReTyped, + this.K, + distReTyped, + rvec, + tvec, + Cfg.solvePnPRefineVVSCriteria, + Cfg.solvePnPRefineVVSLambda); + // } + // else + // { + // MatOfPoint3f p3dInLiers = new MatOfPoint3f(); + // p3dInLiers.alloc(inLiers.rows()); + // MatOfPoint2f p2dInLiers = new MatOfPoint2f(); + // p2dInLiers.alloc(inLiers.rows()); + + // float[] p3dArray = new float[p3dReTyped.rows()*p3dReTyped.channels()]; + // float[] p2dArray = new float[p2dReTyped.rows()*p2dReTyped.channels()]; + // float[] p3dInLiersArray = new float[inLiers.rows()*p3dInLiers.channels()]; + // float[] p2dInLiersArray = new float[inLiers.rows()*p2dInLiers.channels()]; + // int[] inLiersArray = new int[inLiers.rows()]; + + // p3dReTyped.get(0, 0, p3dArray); + // p2dReTyped.get(0, 0, p2dArray); + // inLiers.get(0, 0, inLiersArray); + + // int detectedCornerIndex; + // for (int inLierIndex = 0; inLierIndex < inLiers.rows()*p3dReTyped.channels(); inLierIndex += + // p3dReTyped.channels()) + // { + // detectedCornerIndex = + // inLiersArray[inLierIndex/p3dReTyped.channels()]*p3dReTyped.channels(); + // p3dInLiersArray[inLierIndex ] = p3dArray[detectedCornerIndex ]; + // p3dInLiersArray[inLierIndex + 1] = p3dArray[detectedCornerIndex+1]; + // p3dInLiersArray[inLierIndex + 2] = p3dArray[detectedCornerIndex+2]; + // } + // for (int inLierIndex = 0; inLierIndex < inLiers.rows()*p2dReTyped.channels(); inLierIndex += + // p2dReTyped.channels()) + // { + // detectedCornerIndex = + // inLiersArray[inLierIndex/p2dReTyped.channels()]*p2dReTyped.channels(); + // p2dInLiersArray[inLierIndex ] = p2dArray[detectedCornerIndex ]; + // p2dInLiersArray[inLierIndex + 1] = p2dArray[detectedCornerIndex + 1]; + // } + // p3dInLiers.put(0, 0, p3dInLiersArray); + // p2dInLiers.put(0, 0, p2dInLiersArray); + + // Calib3d.solvePnPRefineVVS( + // p3dInLiers, p2dInLiers, + // this.K, distReTyped, + // rvec, tvec, + // criteria, 1. + // ); + // } + + // FIXME negating "x" makes the shadow for jaccard the right orientation for some unknown + // reason! Python doesn't need this. + // I thought it was related to not having the "flip()" as the BoardPreview needs because the + // "warpPerspective" flips + // the image, but I tried that flip and it was the wrong axis. Still a mystery + // removing this and flipping + Core.multiply(rvec, new Scalar(-1., 1., 1.), rvec); + + this.rvec = rvec.t(); // t() like ravel(), solvePnp returns r and t as Mat(3, 1, ) + this.tvec = tvec.t(); // and the rest of the program uses Mat(1, 3, ) + + // logger.debug("out rvec\n" + this.rvec.dump()); + // logger.debug("out tvec\n" + this.tvec.dump()); + } +} +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* End ChArucoDetector class */ +/* End ChArucoDetector class */ +/* End ChArucoDetector class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ diff --git a/photon-core/src/main/java/org/photonvision/calibrator/Distortion.java b/photon-core/src/main/java/org/photonvision/calibrator/Distortion.java new file mode 100644 index 0000000000..0293e07103 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/calibrator/Distortion.java @@ -0,0 +1,404 @@ +/* + * 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 . + */ + +// This project and file are derived in part from the "Pose Calib" project by +// @author Pavel Rojtberg +// It is subject to his license terms in the PoseCalibLICENSE file. + +package org.photonvision.calibrator; + +import java.util.ArrayList; +import java.util.List; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.Core; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.MatOfDouble; +import org.opencv.core.MatOfPoint; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.MatOfPoint3f; +import org.opencv.core.Rect; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; + +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* Distortion class */ +/* Distortion class */ +/* Distortion class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +class Distortion { + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* get_bounds */ + /* get_bounds */ + /* get_bounds */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + private static Rect get_bounds(Mat thresh, Mat mask) { + // seems like a better strategy would be to see what contour actually contributes the most and + // not just check the largest ones + // and use true area of contour and not just the number of points in the contour + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + // logger.debug("thresh " + thresh); + // logger.debug("mask " + mask); + + List contours = + new ArrayList<>(20); // arbitrary initial size - what is a better guess? + Mat hierarchy = new Mat(); + Imgproc.findContours( + thresh, contours, hierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE); + + // logger.debug(contours.size() + " contours"); + // look for the largest object that is not masked + // This is essentially a Sort and Filter. It's not very efficient but that makes it easier + // by not having to reorder the contours. The list is expected to be very short so it's not + // a problem. + + // outer loop to allow the contours list to be trimmed by the filter + // since Java disallows changing a list within an iterator + while (contours.size() > 0) { + // look for contour with largest area + double areaContourMax = -1.; // indicate no max at the start + int mx = -1; // indicate no max at the start + for (int i = 0; i < contours.size(); i++) { + // long areaContour = contours.get(i).total(); // original area was just the count of the + // points in the contour + double areaContour = + Imgproc.contourArea( + contours.get( + i)); // this might be better area but watch for area = 0.0 if only 2 points in + // contour + + if (areaContour >= areaContourMax) { + // new max and its location + areaContourMax = areaContour; + mx = i; + } + // logger.debug("Contour " + (mx+1) + " of " + contours.size() + ", area max so far " + + // areaContourMax + // + ", contour size " + contours.get(mx).size(mx) + "\n" + contours.get(mx).dump()); + } + // Now have contour with largest area so check that area not already covered, + // that is, it's not masked. + // If already mostly masked then this contour doesn't contribute + // enough so delete it. Else it's good and return it. + Rect aabb = Imgproc.boundingRect(contours.get(mx)); + int x = aabb.x; + int y = aabb.y; + int w = aabb.width; + int h = aabb.height; + // logger.debug("processing Rect aabb " + aabb); + + if (!mask.empty() // amount of mask already filled where this contour would fill + && (double) Core.countNonZero(mask.submat(y, y + h, x, x + w)) / (double) (w * h) + > Cfg.MAX_OVERLAP) { + contours.remove( + mx); // largest contour wouldn't contribute enough in the right places so skip it + continue; + } + // logger.debug("returning aabb " + aabb); // best contributing contour for the pose + return aabb; // best contour in list so return it + } + + // logger.debug("returning null aabb"); // pose doesn't contribute enough + + return null; // no contours met the criteria + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* make_distort_map */ + /* make_distort_map */ + /* make_distort_map */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * creates a map for distorting an image as a opposed to the default behaviour of undistorting + * + * @param K + * @param sz width, height + * @param dist + * @param Knew + * @return + */ + static Mat make_distort_map(Mat K, Size sz, Mat dist, Mat Knew) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + // logger.debug("camera matrix K " + K + "\n" + K.dump()); + // logger.debug("sz " + sz); + // logger.debug("distortion coefficients dist " + dist.dump() + dist); + // logger.debug("Knew " + Knew.dump()); // null pointer (or empty?) Knew + + // pts = np.array(np.meshgrid(range(sz[0]), range(sz[1]))).T.reshape(-1, 1, 2) + // inclusive 0, to not included final, step; fills one column down the rows then the next column + // and down the rows + // then transposes and reshapes to long and skinny + // build grid in the correct final order (and shape) so the pair of transposes aren't needed + int w = (int) sz.width; // columns + int h = (int) sz.height; // rows + int c = 2; // MatOfPoint2f channels, x and y axes + // make smaller 2-D Mat of x,y points from full size image Mat + // the values in the smaller Mat are the original x, y coordinates from the larger Mat + + MatOfPoint2f pts = new MatOfPoint2f(); + pts.alloc(h * w); + // make 2d meshgrid but flattened to 1 dimension - lots of rows and 1 column each element is + // essentially Point2f + // created mesh grid with flipped rows/cols so the transpose isn't needed like the np.meshgrid + float[] ptsMeshGrid = + new float[w * c]; // intermediate 1d version of 2d points for a column; X, Y pair + int indexRectangularRowChannel; // one row of the rectangular grid + int indexLinearRow = 0; // rows in the long skinny Mat + for (int y = 0; y < h; y++) // traverse all rows of the rectangle grid + { + indexRectangularRowChannel = 0; + for (int x = 0; x < w; x++) // traverse all columns of the rectangle grid + { + ptsMeshGrid[indexRectangularRowChannel++] = x; + ptsMeshGrid[indexRectangularRowChannel++] = y; + } + pts.put(indexLinearRow, 0, ptsMeshGrid); + indexLinearRow += + indexRectangularRowChannel + / c; // next starting row of the very long skinny Mat for the rectangular grid row + } + // grid is built + + MatOfPoint2f dpts = new MatOfPoint2f(); + + Calib3d.undistortPointsIter( + pts, dpts, K, dist, new Mat(), Knew, Cfg.undistortPointsIterCriteria); + + Mat dpts2D = dpts.reshape(2, h); + + // logger.debug("pts " + pts + "\n" + ArrayUtils.brief(pts)); + // logger.debug("dpts " + dpts + "\n" + ArrayUtils.brief(dpts)); + // logger.debug("returning dpts2D " + dpts2D + ArrayUtils.brief(dpts2D)); + // logger.debug("maybe returning Knew\n" + Knew.dump()); + + pts.release(); + dpts.release(); + + return dpts2D; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* sparse_undistort_map */ + /* sparse_undistort_map */ + /* sparse_undistort_map */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + // same output as initUndistortRectifyMap, but sparse + // @param sz: width, height + // @return: distorted points, original points + static List sparse_undistort_map(Mat K, Size sz, Mat dist, Mat Knew, int step) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + // best I can tell step is always 20 (subsample) and never 1 so this should not be executed + if (step == 1) + throw new IllegalArgumentException("step = 1 full image sampling not converted and tested"); + // make smaller 2-D Mat of x,y points from full size image Mat + // the values in the smaller Mat are the original x, y coordinates from the larger Mat + + // inclusive 0 to step + int w = (int) sz.width / step; // columns + int h = (int) sz.height / step; // rows + int c = 2; // x and y axes + // OpenCV methods require linear 1-column Mats flattened from the 2d mesh grid + int indexLinearRow; + MatOfPoint2f pts = new MatOfPoint2f(); + pts.alloc(h * w); + float[] ptsMeshGrid = new float[w * h * c]; // intermediate 2d points + + indexLinearRow = 0; + for (float y = 0.f; y < h; y += step) { + for (float x = 0.f; x < w; x += step) { + ptsMeshGrid[indexLinearRow++] = x; + ptsMeshGrid[indexLinearRow++] = y; + } + } + pts.put(0, 0, ptsMeshGrid); + + MatOfPoint2f ptsUndistorted = new MatOfPoint2f(); // intermediate 2d points + MatOfPoint3f pts3d = new MatOfPoint3f(); // 3d points; Z = 0 added to the 2d to make 3d + Mat zero = Mat.zeros(3, 1, CvType.CV_32FC1); + + Calib3d.undistortPointsIter( + pts, + ptsUndistorted, + Knew, + new Mat(), + new Mat(), + new Mat(), + Cfg.undistortPointsIterCriteria); // undistort the 2d points + + Calib3d.convertPointsToHomogeneous(ptsUndistorted, pts3d); // now convert 2d to 3d homogeneous + // n by 2 or 3 dimensions in or 2 or 3 dimensions by n in; always nx1x 3 or 4 channels out + // a dimension is either a Mat row or column or 1 row or column and 2 or 3 channels + // 1xnx2, 1xnx3, nx1x2, nx1x3, nx2x1, nx3x1 in; always nx1x3 or nx1x4 out + + MatOfDouble distOfDouble = new MatOfDouble(dist); // convert as required for the projectPoints() + + MatOfPoint2f dpts = new MatOfPoint2f(); + + Calib3d.projectPoints( + pts3d, zero, zero, K, distOfDouble, dpts); // project points in 3d back to a 2d screen + + int[] shape = {w, h}; + Mat dpts2D = dpts.reshape(c, shape); + Mat pts2D = pts.reshape(c, shape); + // reformat sparse flat MatOfPoint2f to 2-D 2-channel rectangular subsample map + List maps = new ArrayList(2); + + pts.release(); + ptsUndistorted.release(); + pts3d.release(); + zero.release(); + distOfDouble.release(); + dpts.release(); + + maps.add(dpts2D); + maps.add(pts2D); + + return maps; + } + + /** + * get_diff_heatmap NOT USED -- NOT CONVERTED get_diff_heatmap NOT USED -- NOT CONVERTED + * get_diff_heatmap NOT USED -- NOT CONVERTED + */ + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* loc_from_dist */ + /* loc_from_dist */ + /* loc_from_dist */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * compute location based on distortion strength + * + * @param pts: sampling locations + * @param dpts: distorted points + * @param mask: mask for ignoring locations + * @param lower: find location with minimal distortion instead + * @param thres: distortion strength to use as threshold [%] + * @return + */ + static Rect loc_from_dist( + Mat pts, Mat dpts, Mat mask, boolean lower, double thres) // force specifying all parameters + { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + // logger.debug("pts " + pts); + // logger.debug("dpts " + dpts); + // logger.debug("mask " + mask); + // logger.debug("lower " + lower); + // logger.debug("thres " + thres); + Mat diffpts = new Mat(); + Core.subtract(pts, dpts, diffpts); + // logger.debug("diffpts " + diffpts); + + Mat normMat = new Mat(pts.rows(), pts.cols(), CvType.CV_32FC1); + // logger.debug("normMat empty " + normMat); + + for (int row = 0; row < pts.rows(); row++) + for (int col = 0; col < pts.cols(); col++) { + float[] point = new float[2]; // get the 2 channels of data x in 0 and y in 1 + diffpts.get(row, col, point); + float norm = + (float) + Math.sqrt(Math.pow(point[0], 2) + Math.pow(point[1], 2)); // L2 norm (Frobenious) + normMat.put(row, col, norm); + } + // logger.debug("normMat filled " + normMat); + + normMat = normMat.reshape(0, mask.rows()) /*.t()*/; + // logger.debug("normMat reshaped " + normMat); + + Mat diff = new Mat(); + Core.normalize(normMat, diff, 0, 255, Core.NORM_MINMAX, CvType.CV_8U); + // logger.debug("diff " + diff.dump()); + // logger.debug("normMat normalized=diff " + diff); + + Rect bounds = null; + + Mat thres_img = new Mat(); + while ((bounds == null) && (thres >= 0.) && (thres <= 1.)) { + if (lower) { + thres += 0.05; + Imgproc.threshold(diff, thres_img, thres * 255., 255., Imgproc.THRESH_BINARY_INV); + } else { + thres -= 0.05; + Imgproc.threshold(diff, thres_img, thres * 255., 255., Imgproc.THRESH_BINARY); + } + // logger.debug("thres_img " + thres_img /*+ "\n" + brief(thres_img.dump())*/); + + bounds = get_bounds(thres_img, mask); + + if (bounds == null) { + continue; + } + + if (bounds.width * bounds.height == 0) // ensure area is not 0 + { + bounds = null; + } + } + + normMat.release(); + diff.release(); + diffpts.release(); + + // logger.debug("bounds " + (bounds == null ? "is null" : bounds)); + + return bounds; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* Distortion constructor */ + /* Distortion constructor */ + /* Distortion constructor */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + private Distortion() { + throw new UnsupportedOperationException("This is a utility class"); + } +} +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* End Distortion class */ +/* End Distortion class */ +/* End Distortion class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ diff --git a/photon-core/src/main/java/org/photonvision/calibrator/Main.java b/photon-core/src/main/java/org/photonvision/calibrator/Main.java new file mode 100644 index 0000000000..e084fe49eb --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/calibrator/Main.java @@ -0,0 +1,794 @@ +/* + * 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 . + */ + +// This project and file are derived in part from the "Pose Calib" project by +// @author Pavel Rojtberg +// It is subject to his license terms in the PoseCalibLICENSE file. + +// Calibrate Camera with efficient camera Pose Guidance provided on screen to the user + +package org.photonvision.calibrator; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.cscore.CvSource; +import edu.wpi.first.cscore.MjpegServer; +import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.cscore.VideoMode; +import edu.wpi.first.cscore.VideoMode.PixelFormat; +import edu.wpi.first.cscore.VideoProperty; +import java.io.FileNotFoundException; +import java.io.PrintWriter; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.Scanner; +import java.util.concurrent.atomic.AtomicInteger; +import org.apache.commons.cli.CommandLine; +import org.apache.commons.cli.CommandLineParser; +import org.apache.commons.cli.DefaultParser; +import org.apache.commons.cli.HelpFormatter; +import org.apache.commons.cli.Options; +import org.apache.commons.cli.ParseException; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.Core; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.MatOfInt; +import org.opencv.core.Point; +import org.opencv.core.Point3; +import org.opencv.core.Scalar; +import org.opencv.core.Size; +import org.opencv.imgcodecs.Imgcodecs; +import org.opencv.imgproc.Imgproc; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.LogLevel; +import org.photonvision.common.logging.Logger; + +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* Main class */ +/* Main class */ +/* Main class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +public class Main { + private static final String VERSION = "beta 12.7"; // change this + static final Logger logger = new Logger(Main.class, LogGroup.General); + + private static int frameNumber = 0; + static String frame = "00000 "; + private static PrintWriter vnlog = null; + static Mat progressInsert; + static boolean fewCorners = true; + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* handle keystrokes */ + /* handle keystrokes */ + /* handle keystrokes */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + // keyboard mapping returns from OpenCV waitKey + private static final int keyTerminateOpenCV = 81; + private static final int keyCaptureOpenCV = 67; + private static final int keyMirrorToggleOpenCV = 77; + + // keyboard mapping returns from Java Scanner + private static final int keyTerminateScanner = 113; + private static final int keyCaptureScanner = 99; + private static final int keyMirrorToggleScanner = 109; + private static final int timedOut = -1; // timed out no key pressed + + // Java Scanner alternative to OpenCV keyboard usage that is not in PV headless (AWT is missing) + // Turn Scanner keys into OpenCV keys to ease transition back and forth between PV terminal and + // OpenCV waitKey + AtomicInteger dokeystroke = new AtomicInteger(-1); + + class Keystroke implements Runnable { + final Logger logger = new Logger(Main.Keystroke.class, LogGroup.General); + + public void run() { + try (Scanner keyboard = new Scanner(System.in)) { + while (!Thread.interrupted()) { + System.out.println( + "Pose should auto capture otherwise, press c (capture), m (mirror), q (quit) then the Enter key"); + String entered = keyboard.next(); + int keyScanner = entered.charAt(0); + logger.debug("user entered action " + keyScanner); + // map Scanner character codes to OpenCV character codes + if (keyScanner == keyCaptureScanner) { + dokeystroke.set(keyCaptureOpenCV); + } + if (keyScanner == keyMirrorToggleScanner) { + dokeystroke.set(keyMirrorToggleOpenCV); + } + if (keyScanner == keyTerminateScanner) { + dokeystroke.set(keyTerminateOpenCV); + } + } + } catch (Exception e) { + logger.error( + "Terminal keyboard closed prematurely (Ctrl-c) or doesn't exist (jar file not run from command line; don't double click the jar to start it)"); + } + } + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* handleArgs */ + /* handleArgs */ + /* handleArgs */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + private static boolean handleArgs(String[] args) throws ParseException { + final var options = new Options(); + options.addOption("h", "help", false, "Show this help text and exit"); + options.addOption("width", true, "camera image width (1280)"); + options.addOption("height", true, "camera image height (720)"); + options.addOption("dpmX", true, "print width pixels per meter (9843=250 DPI)"); + options.addOption("dpmY", true, "print height pixels per meter (9843=250 DPI"); + options.addOption( + "pxFmt", true, "pixel format (kYUYV) " + Arrays.toString(PixelFormat.values())); + options.addOption("cameraId", true, "camera id (0)"); + options.addOption("fps", true, "frames per second (10)"); + + CommandLineParser parser = new DefaultParser(); + CommandLine cmd = parser.parse(options, args); + + if (cmd.hasOption("help")) { + HelpFormatter formatter = new HelpFormatter(); + formatter.printHelp("java -jar .jar [options]", options); + return false; // exit program + } + + if (cmd.hasOption("width")) { + Cfg.image_width = Integer.parseInt(cmd.getOptionValue("width")); + } + + if (cmd.hasOption("height")) { + Cfg.image_height = Integer.parseInt(cmd.getOptionValue("height")); + } + + if (cmd.hasOption("dpmX")) { + Cfg.resXDPM = Integer.parseInt(cmd.getOptionValue("dpmX")); + } + + if (cmd.hasOption("dpmY")) { + Cfg.resYDPM = Integer.parseInt(cmd.getOptionValue("dpmY")); + } + + if (cmd.hasOption("pxFmt")) { + Cfg.pixelFormat = PixelFormat.valueOf(cmd.getOptionValue("pxFmt")); + } + + if (cmd.hasOption("cameraId")) { + Cfg.camId = Integer.parseInt(cmd.getOptionValue("cameraId")); + } + + if (cmd.hasOption("fps")) { + Cfg.fps = Integer.parseInt(cmd.getOptionValue("fps")); + } + + return true; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* main */ + /* main */ + /* main */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + public static void main(String[] args) throws Exception { + Logger.setLevel(LogGroup.General, LogLevel.DEBUG); + logger.info("Pose Guidance Camera Calibration version " + VERSION); + // get the parameters for the user provided options + logger.debug("Command Line Args " + Arrays.toString(args)); + + try { + if (!handleArgs(args)) { + System.exit(0); + } + } catch (ParseException e) { + logger.error("Failed to parse command-line options!", e); + System.exit(0); + } + + org.photonvision.common.util.TestUtils.loadLibraries(); + + progressInsert = new Mat(); + + // keyboard handler for PV environment using the web interface + Main MainInstance = null; + Keystroke keystroke; + Thread keyboardThread; + MainInstance = new Main(); + keystroke = MainInstance.new Keystroke(); + keyboardThread = new Thread(keystroke, "keys"); + keyboardThread.setDaemon(true); + keyboardThread.start(); + + // output display of the camera image with guidance board + CvSource networkDisplay = null; + MjpegServer mjpegServer; + networkDisplay = + new CvSource( + "calibPV", /*VideoMode.*/ + PixelFormat.kMJPEG, + Cfg.image_height, + Cfg.image_width, + Cfg.fps); + mjpegServer = new MjpegServer("GuidanceView", Cfg.displayPort); + logger.info("View Guidance Board On Port " + Cfg.displayPort); + mjpegServer.setSource(networkDisplay); + Mat out = new Mat(); // user display Mat + + // camera input + final UsbCamera camera = + CameraServer.startAutomaticCapture( + Cfg.camId); // gives access to camera parameters on port 181 or above + // final UsbCamera camera = new UsbCamera("mycamera", Cfg.camId); // same camera as above but no + // interaction on port 181 or above + for (VideoMode vm : camera.enumerateVideoModes()) { + logger.debug( + "Camera mode choices " + + vm.getPixelFormatFromInt(vm.pixelFormat.getValue()) + + " " + + vm.width + + "x" + + vm.height + + " " + + vm.fps + + " fps"); + } + for (VideoProperty vp : camera.enumerateProperties()) { + logger.debug( + "camera property choices " + + vp.get() + + " " + + vp.getName() + + " " + + VideoProperty.getKindFromInt(vp.get())); + } + VideoMode videoMode = + new VideoMode(Cfg.pixelFormat, Cfg.image_width, Cfg.image_height, Cfg.fps); + logger.debug( + "Setting camera mode " + + VideoMode.getPixelFormatFromInt(Cfg.pixelFormat.getValue()) + + " " + + Cfg.image_width + + "x" + + Cfg.image_height + + " " + + Cfg.fps + + "fps"); + try { + if (!camera.setVideoMode(videoMode)) + throw new IllegalArgumentException("set video mode returned false"); + } catch (Exception e) { + logger.error("camera set video mode error; mode is unchanged", e); + } + logger.info( + "camera " + Cfg.camId + " properties can be seen and changed on port 1181 or higher"); + CvSink cap = + CameraServer.getVideo(camera); // Get a CvSink. This will capture Mats from the camera + cap.setSource(camera); + Mat _img = new Mat(); // this follows the camera input but ... + Mat img = + new Mat( + Cfg.image_height, + Cfg.image_width, + CvType.CV_8UC3); // set by user config - need camera to return this size, too + + ChArucoDetector tracker = new ChArucoDetector(); + UserGuidance ugui = new UserGuidance(tracker, Cfg.var_terminate); + + // runtime variables + boolean mirror = false; + boolean save = false; // indicator for user pressed the "c" key to capture (save) manually + String endMessage = + "logic error"; // status of calibration at the end - if this initial value isn't reset, + // indicates a screw-up in the code + + grabFrameLoop: + while (!Thread.interrupted()) { + frameNumber++; + frame = String.format("%05d ", frameNumber); + if (frameNumber % Cfg.garbageCollectionFrames == 0) // periodically cleanup old Mats + { + System.runFinalization(); + System.gc(); + } + boolean force = + false; // force add frame to calibration (no support yet still images else (force = !live) + + long status = cap.grabFrame(_img, 0.5); + if (status != 0) { + if (_img.height() != Cfg.image_height + || _img.width() + != Cfg + .image_width) // enforce camera matches user spec for testing and no good camera + // setup + { + logger.warn( + "camera size incorrect " + + _img.width() + + "x" + + _img.height() + + "; user specified to calibrate at " + + Cfg.image_width + + "x" + + Cfg.image_height + + " - ignoring it"); + continue; + } + _img.copyTo(img); + } else { + logger.warn("grabFrame error " + cap.getError()); + force = false; // useless now with the continue below + continue; // pretend frame never happened - rkt addition; original reprocessed previous + // frame + } + + tracker.detect(img); + + if (save) { + save = false; + force = true; + } + + img.copyTo(out); // out has the camera image at his point + + ugui.draw( + out, + mirror); // this adds the guidance board to the camera image (out) to make the new out + + boolean capturedPose = ugui.update(force); // calibrate + + if (capturedPose && Cfg.logDetectedCorners) { + logDetectedCorners(img, ugui); + } + + displayOverlay(out, ugui); + + int k; + networkDisplay.putFrame(out); + k = MainInstance.dokeystroke.getAndSet(timedOut); + + if (ugui.converged()) // are we there yet? + { + ugui.write(); // write all the calibration data + + endMessage = "CALIBRATED"; + + break grabFrameLoop; // the end - rkt addition; the original kept looping somehow + } + + if (k == timedOut) { + continue; // no key press to process + } + + // have a key + switch (k) { + case keyTerminateOpenCV: // terminate key pressed to stop loop immediately + endMessage = "CANCELLED"; + break grabFrameLoop; + + case keyMirrorToggleOpenCV: // mirror/no mirror key pressed + mirror = !mirror; + break; + + case keyCaptureOpenCV: // capture frame key pressed + save = true; + break; + + default: // unassigned key + break; + } + } // end grabFrameLoop + + Imgproc.putText( + out, + endMessage, + new Point(50, 250), + Imgproc.FONT_HERSHEY_SIMPLEX, + 2.8, + new Scalar(0, 0, 0), + 5); + Imgproc.putText( + out, + endMessage, + new Point(50, 250), + Imgproc.FONT_HERSHEY_SIMPLEX, + 2.8, + new Scalar(255, 255, 255), + 3); + Imgproc.putText( + out, + endMessage, + new Point(50, 250), + Imgproc.FONT_HERSHEY_SIMPLEX, + 2.8, + new Scalar(255, 255, 0), + 2); + + for (int runOut = 0; + runOut < 10; + runOut++) // last frame won't display so repeat it a bunch of times to see it; q lags these + // 2 seconds + { + networkDisplay.putFrame(out); + Thread.sleep(200L); + } + networkDisplay.close(); + + if (vnlog != null) { + vnlog.close(); + } + logger.debug("End of running main"); + System.exit(0); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* displayOverlay */ + /* displayOverlay */ + /* displayOverlay */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + public static void displayOverlay(Mat out, UserGuidance ugui) { + Imgproc.putText( + out, + Main.frame, + new Point(0, 20), + Imgproc.FONT_HERSHEY_SIMPLEX, + .8, + new Scalar(0, 0, 0), + 2); + Imgproc.putText( + out, + Main.frame, + new Point(0, 20), + Imgproc.FONT_HERSHEY_SIMPLEX, + .8, + new Scalar(255, 255, 255), + 1); + + if (ugui.user_info_text().length() > 0) // is there a message to display? + { + // if ( ! (ugui.user_info_text().equals("initialization"))) // stop spamming "initialization" + // to log + // { + // logger.log(Level.WARNING,ugui.user_info_text()); + // } + String message1 = ugui.user_info_text(); + Imgproc.putText( + out, + message1, + new Point(80, 20), + Imgproc.FONT_HERSHEY_SIMPLEX, + .8, + new Scalar(0, 0, 0), + 2); + Imgproc.putText( + out, + message1, + new Point(80, 20), + Imgproc.FONT_HERSHEY_SIMPLEX, + .8, + new Scalar(255, 255, 255), + 1); + } + + if (fewCorners) { + String message2 = "moving or bad aim\n"; + Imgproc.putText( + out, + message2, + new Point(80, 40), + Imgproc.FONT_HERSHEY_SIMPLEX, + .8, + new Scalar(0, 0, 0), + 2); + Imgproc.putText( + out, + message2, + new Point(80, 40), + Imgproc.FONT_HERSHEY_SIMPLEX, + .8, + new Scalar(255, 255, 255), + 1); + fewCorners = false; + } + + // ugui.tgt_r() ugui.tgt_t guidance board target rotation and translation from pose generation + Mat rotationMatrix = new Mat(); + double[] rotationDegrees; + double[] translation = new double[3]; + + // if Guidance Board has a pose then display it (end of program is missing this pose) + if (!ugui.tgt_r().empty() && !ugui.tgt_t().empty()) { + Calib3d.Rodrigues(ugui.tgt_r(), rotationMatrix); + rotationDegrees = + Calib3d.RQDecomp3x3( + rotationMatrix, new Mat(), new Mat()); // always returns reuler.length = 3 + rotationDegrees[0] -= 180.; + + ugui.tgt_t().get(0, 0, translation); + + Imgproc.putText( + out, + String.format( + "r{%4.0f %4.0f %4.0f} t{%4.0f %4.0f %4.0f}Guidance", + rotationDegrees[0], + rotationDegrees[1], + rotationDegrees[2], + translation[0], + translation[1], + translation[2]), + new Point(250, 60), + Imgproc.FONT_HERSHEY_SIMPLEX, + .6, + new Scalar(0, 0, 0), + 2); + Imgproc.putText( + out, + String.format( + "r{%4.0f %4.0f %4.0f} t{%4.0f %4.0f %4.0f}Guidance", + rotationDegrees[0], + rotationDegrees[1], + rotationDegrees[2], + translation[0], + translation[1], + translation[2]), + new Point(250, 60), + Imgproc.FONT_HERSHEY_SIMPLEX, + .6, + new Scalar(255, 255, 255), + 1); + } + + // if camera has a ChArUco Board pose then display it (if camera not on target this pose is + // missing) + if (!ugui.tracker.rvec().empty() && !ugui.tracker.tvec().empty()) { + Calib3d.Rodrigues(ugui.tracker.rvec(), rotationMatrix); + rotationDegrees = + Calib3d.RQDecomp3x3( + rotationMatrix, new Mat(), new Mat()); // always returns reuler.length = 3 + rotationDegrees[1] = -rotationDegrees[1]; + rotationDegrees[2] = -rotationDegrees[2]; + + ugui.tracker.tvec().get(0, 0, translation); + // translation[1] = -translation[1]; + translation[0] = (double) (((int) (translation[0]) + 5) / 10 * 10); + translation[1] = (double) (((int) (translation[1]) + 5) / 10 * 10); + translation[2] = (double) (((int) (translation[2]) + 5) / 10 * 10); + Imgproc.putText( + out, + String.format( + "r{%4.0f %4.0f %4.0f} t{%4.0f %4.0f %4.0fCamera", + rotationDegrees[0], + rotationDegrees[1], + rotationDegrees[2], + translation[0], + translation[1], + translation[2]), + new Point(250, 80), + Imgproc.FONT_HERSHEY_SIMPLEX, + .6, + new Scalar(0, 0, 0), + 2); + Imgproc.putText( + out, + String.format( + "r{%4.0f %4.0f %4.0f} t{%4.0f %4.0f %4.0f}Camera", + rotationDegrees[0], + rotationDegrees[1], + rotationDegrees[2], + translation[0], + translation[1], + translation[2]), + new Point(250, 80), + Imgproc.FONT_HERSHEY_SIMPLEX, + .6, + new Scalar(255, 255, 255), + 1); + } + + // write a frame to a file name java.jpg + // final MatOfInt writeBoardParams = new MatOfInt(Imgcodecs.IMWRITE_JPEG_QUALITY, 100); // + // debugging - pair-wise; param1, value1, ... + // Imgcodecs.imwrite("java" + frame + ".jpg", out, writeBoardParams); // debugging - save image + // in jpg file + + if (!progressInsert.empty()) { + // add to the display the board/camera overlap image + Imgproc.resize( + progressInsert, + progressInsert, + new Size(Cfg.image_width * 0.1, Cfg.image_height * 0.1), + 0, + 0, + Imgproc.INTER_CUBIC); + List temp1 = new ArrayList<>(3); // make the 1 b&w channel into 3 channels + temp1.add(progressInsert); + temp1.add(progressInsert); + temp1.add(progressInsert); + Mat temp2 = new Mat(); + Core.merge(temp1, temp2); + Imgproc.rectangle( + temp2, // outline the insert for better visibility + new Point(0, 0), + new Point(progressInsert.cols() - 1., progressInsert.rows() - 1.), + new Scalar(255., 255., 0.), + 1); + temp2.copyTo( + out.submat( + (int) (Cfg.image_height * 0.45), + (int) (Cfg.image_height * 0.45) + progressInsert.rows(), + 0, + progressInsert.cols())); + temp2.release(); + + Imgproc.putText( + out, + String.format( + "similar%5.2f/%4.2f", ugui.pose_close_to_tgt_get(), Cfg.pose_close_to_tgt_min), + new Point(0, (int) (Cfg.image_height * 0.45) + progressInsert.rows() + 20), + Imgproc.FONT_HERSHEY_SIMPLEX, + 0.6, + new Scalar(255, 255, 255), + 1); + } + + // display intrinsics convergence + for (int i = 0; i < 9; i++) { + Scalar color; + if (ugui.pconverged()[i]) { + color = new Scalar(0, 190, 0); + } else { + color = new Scalar(0, 0, 255); + } + Imgproc.rectangle( + out, + new Point((double) i * 20, Cfg.image_height * 0.4), + new Point((double) (i + 1) * 20, Cfg.image_height * 0.4 + 20), + color, + Imgproc.FILLED); + Imgproc.putText( + out, + ugui.INTRINSICS()[i], + new Point((double) i * 20, Cfg.image_height * 0.4 + 15), + Imgproc.FONT_HERSHEY_SIMPLEX, + .4, + new Scalar(255, 255, 255), + 1); + } + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* logDetectedCorners */ + /* logDetectedCorners */ + /* logDetectedCorners */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * Detected Board data in mrgingham format plus the board info + * + * @param img Camera image + * @param ugui User Guidance Class + * @throws FileNotFoundException + */ + public static void logDetectedCorners(Mat img, UserGuidance ugui) throws FileNotFoundException { + if (vnlog == null) // first time switch + { + vnlog = new PrintWriter(Cfg.cornersLog); + vnlog.println("## produced by pose guidance calibration program"); + vnlog.println("# filename x y level cid boardX boardY"); + } + + // write the captured frame to a file name + int captureCount = ugui.calib.keyframes.size(); + + String filename = String.format("img%02d.jpg", captureCount); + final MatOfInt writeBoardParams = + new MatOfInt(Imgcodecs.IMWRITE_JPEG_QUALITY, 100); // pair-wise; param1, value1, ... + Imgcodecs.imwrite(filename, img, writeBoardParams); // save camera image + + // for efficiency put Mat data in arrays + Point3[] ChessboardCorners = + ugui.tracker + .board() + .getChessboardCorners() + .toArray(); // 1 col 3 channels x, y, z in a Point3 (float) + + int[] DetectedCIDs = + new int + [ugui.tracker + .cids() + .rows()]; // get detected corners - assume captured image does have corners + ugui.tracker.cids().get(0, 0, DetectedCIDs); + + float[] DetectedCorners = + new float + [ugui.tracker.ccorners().rows() + * ugui.tracker.ccorners().cols() + * ugui.tracker.ccorners().channels()]; // below assumes x and y in a row + ugui.tracker.ccorners().get(0, 0, DetectedCorners); + + // save vnlog + for (int detectedCornerIndex = 0; + detectedCornerIndex < DetectedCIDs.length; + detectedCornerIndex++) { + int boardCornerId = DetectedCIDs[detectedCornerIndex]; // get board corner that is detected + StringBuilder logLine = new StringBuilder(); + logLine.append(filename); + logLine.append(" "); + logLine.append(DetectedCorners[detectedCornerIndex * 2]); // x + logLine.append(" "); + logLine.append(DetectedCorners[detectedCornerIndex * 2 + 1]); // y + logLine.append(" 0 "); // intended to be decimations always 0 + logLine.append(boardCornerId); + logLine.append(" "); + logLine.append(ChessboardCorners[boardCornerId].x); // x + logLine.append(" "); + logLine.append(ChessboardCorners[boardCornerId].y); // y + vnlog.println(logLine.toString()); + } + } +} +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* End Main class */ +/* End Main class */ +/* End Main class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ + +// https://github.com/mcm001/photonvision/tree/2023-10-30_pose_calib_integration +// I made this by running +// gradlew clean +// then for RPi +// gradlew shadowjar -PArchOverride=linuxarm64 +// or for Windows +// gradlew shadowjar + +// inside the photonvision project's root directory +// that spits the jar out into photon-server/build/libs +// you should be able to stop the RPi photonvision service with +// sudo service photonvision stop +// and then +// java -jar photonvision-dev-v2024.1.1-beta-3.1-5-ga99e85a8-linuxarm64.jar +// is all you should need + +// Disable spotless in VSCode extensions or Append "-x spotlessapply" to the commands you run to +// disable it diff --git a/photon-core/src/main/java/org/photonvision/calibrator/PoseGeneratorDist.java b/photon-core/src/main/java/org/photonvision/calibrator/PoseGeneratorDist.java new file mode 100644 index 0000000000..c63a485230 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/calibrator/PoseGeneratorDist.java @@ -0,0 +1,809 @@ +/* + * 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 . + */ + +// This project and file are derived in part from the "Pose Calib" project by +// @author Pavel Rojtberg +// It is subject to his license terms in the PoseCalibLICENSE file. + +package org.photonvision.calibrator; + +import java.util.ArrayList; +import java.util.List; +import java.util.function.Function; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.Core; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.core.Size; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; + +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* PoseGeneratorDist class */ +/* PoseGeneratorDist class */ +/* PoseGeneratorDist class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +public class PoseGeneratorDist { + private static final Logger logger = new Logger(PoseGeneratorDist.class, LogGroup.General); + + enum Pose { + NONE, + ORBITAL, + PLANAR_FULL_SCREEN, + FROM_BOUNDS + }; + + static Pose pose = + Pose.NONE; // probably shouldn't be static but they are mostly used in a static method and + + // there is only one instantiation + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* Gen_Bin class */ + /* Gen_Bin class */ + /* Gen_Bin class */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + // generate values in range by binary subdivision + // note that the list of values grows exponentially with each call for a value + // seems like a poor algorithm to me but it is used sparsely in this application + class Gen_Bin { + private List lst = + new ArrayList<>( + 40); // always manipulate 2 items at a time instead of making a new datatype for a pair + + // - not so safe, though + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* Gen_Bin constructor */ + /* Gen_Bin constructor */ + /* Gen_Bin constructor */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /*private*/ Gen_Bin(double s, double e) { + double t = (s + e) / 2.; + this.lst.add(s); + this.lst.add(t); + this.lst.add(t); + this.lst.add(e); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* next */ + /* next */ + /* next */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /*private*/ double next() { + double s = this.lst.get(0); // pop the top + this.lst.remove(0); + + double e = this.lst.get(0); // pop the new top + this.lst.remove(0); + + double t = (s + e) / 2.; + this.lst.add(s); + this.lst.add(t); + this.lst.add(t); + this.lst.add(e); + + return t; + } + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* unproject */ + /* unproject */ + /* unproject */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * project pixel back to a 3D coordinate at depth Z + * + * @param p + * @param K + * @param cdist + * @param Z + * @return + */ + private static Mat unproject(MatOfPoint2f p, Mat K, Mat cdist, double Z) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + // returns a translation (t) Mat + // logger.debug("p in " + p.dump()); + // logger.debug("camera matrix K " + K + "\n" + K.dump()); + // logger.debug("cdist " + cdist.dump()); + // logger.debug("Z " + Z); + + Calib3d.undistortPointsIter( + p, p, K, cdist, new Mat(), new Mat(), Cfg.undistortPointsIterCriteria); + + // logger.debug("p out " + p.dump()); + + double[] pXY = p.get(0, 0); // get X and Y channels for the point (ravel) + + Mat p3D = new Mat(1, 3, CvType.CV_64FC1); + p3D.put(0, 0, pXY[0], pXY[1], 1.); // x, y, 1 + + Core.multiply(p3D, new Scalar(Z, Z, Z), p3D); + + // logger.debug("return p3D Z scaled " + p3D.dump()); + + return p3D; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* orbital_pose */ + /* orbital_pose */ + /* orbital_pose */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * @param bbox object bounding box. note: assumes planar object with virtual Z dimension. + * @param rx rotation around x axis in rad + * @param ry rotation around y axis in rad + * @param Z distance to camera in board lengths + * @param rz + * @return rvec, tvec + */ + private static List orbital_pose( + Mat bbox, double rx, double ry, double Z, double rz) // force caller to use rz=0 if defaulting + { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + pose = Pose.ORBITAL; + // logger.debug("bbox " + bbox + "\n" + bbox.dump()); + // logger.debug("rx " + rx); + // logger.debug("ry " + ry); + // logger.debug("Z " + Z); + // logger.debug("rz " + rz); + + // compute rotation matrix from rotation vector + double[] angleZ = {0., 0., rz}; + double[] angleX = {Math.PI + rx, 0., 0.}; // flip by 180° so Z is up + double[] angleY = {0., ry, 0.}; + Mat angleZVector = new Mat(3, 1, CvType.CV_64FC1); + Mat angleXVector = new Mat(3, 1, CvType.CV_64FC1); + Mat angleYVector = new Mat(3, 1, CvType.CV_64FC1); + angleZVector.put(0, 0, angleZ); + angleXVector.put(0, 0, angleX); + angleYVector.put(0, 0, angleY); + Mat Rz = new Mat(); + Mat Rx = new Mat(); + Mat Ry = new Mat(); + + /**************************************************************************************** */ + Calib3d.Rodrigues(angleZVector, Rz); + Calib3d.Rodrigues(angleXVector, Rx); + Calib3d.Rodrigues(angleYVector, Ry); + /**************************************************************************************** */ + + // logger.debug("Rz\n" + Rz.dump()); + // logger.debug("Rx\n" + Rx.dump()); + // logger.debug("Ry\n" + Ry.dump()); + + // in Python (Ry).dot(Rx).dot(Rz) messed up nomenclature - it's often really matrix multiply Ry + // times Rx times Rz + Mat R = Mat.eye(4, 4, CvType.CV_64FC1); + Mat R3x3 = R.submat(0, 3, 0, 3); + + /**************************************************************************************** */ + Core.gemm(Ry, Rx, 1., new Mat(), 0, R3x3); + Core.gemm( + R3x3, Rz, 1., new Mat(), 0., R3x3); // rotation matrix of the input Euler Angles [radians] + /**************************************************************************************** */ + + angleZVector.release(); + angleXVector.release(); + angleYVector.release(); + Rz.release(); + Rx.release(); + Ry.release(); + + // translate board to its center + Mat Tc = Mat.eye(4, 4, CvType.CV_64FC1); + Mat Tc1x3 = Tc.submat(3, 4, 0, 3); + Mat Tc3x1 = new Mat(); + Mat translateToBoardCenter = + new Mat( + bbox.rows(), bbox.cols(), bbox.type()); // matching bbox for element by element multiply + translateToBoardCenter.put(0, 0, -0.5, -0.5, 0.); + translateToBoardCenter = bbox.mul(translateToBoardCenter); + // logger.debug("translateToBoardCenter\n" + translateToBoardCenter.dump()); + + // logger.debug("R " + R.dump()); + // logger.debug("R3x3 " + R3x3.dump()); + // logger.debug("Tc " + Tc.dump()); + // logger.debug("Tc1x3 " + Tc1x3.dump()); + + /*************************************************************************************** */ + Core.gemm(R3x3, translateToBoardCenter, 1., new Mat(), 0., Tc3x1); + Tc3x1.t().copyTo(Tc1x3); // update Tc + /*************************************************************************************** */ + // logger.debug("Tc " + Tc.dump()); + // logger.debug("Tc1x3 " + Tc1x3.dump()); + + // translate board to center of image + + Mat T = Mat.eye(4, 4, CvType.CV_64FC1); + Mat T1x3 = T.submat(3, 4, 0, 3); + /*************************************************************************************** */ + Mat translateToImageCenter = + new Mat( + bbox.rows(), bbox.cols(), bbox.type()); // matching bbox for element by element multiply + translateToImageCenter.put(0, 0, -0.5, -0.5, Z); + bbox.mul(translateToImageCenter).t().copyTo(T1x3); + /*************************************************************************************** */ + // logger.debug("translateToImageCenter " + translateToImageCenter.dump()); + // logger.debug("T1x3 " + T1x3.dump()); + // logger.debug("T " + T.dump()); + + // rotate center of board + Mat Rf = new Mat(); + + /*************************************************************************************** */ + Core.gemm(Tc.inv(), R, 1., new Mat(), 0., Rf); + Core.gemm(Rf, Tc, 1., new Mat(), 0., Rf); + Core.gemm(Rf, T, 1., new Mat(), 0., Rf); + /*************************************************************************************** */ + // logger.debug("Rf " + Rf.dump()); + + // return cv2.Rodrigues(Rf[:3, :3])[0].ravel(), Rf[3, :3] + Mat Rf3x3 = Rf.submat(0, 3, 0, 3); + Mat RfVector = new Mat(1, 3, CvType.CV_64FC1); + + /*************************************************************************************** */ + Calib3d.Rodrigues(Rf3x3, RfVector); + Core.transpose(RfVector, RfVector); + /*************************************************************************************** */ + // logger.debug("RfVector returned " + RfVector.dump()); + + Mat t = Rf.submat(3, 4, 0, 3); + Mat tVector = new Mat(); + t.copyTo(tVector); + // logger.debug("tVector returned " + tVector.dump()); + + Tc.release(); + Tc1x3.release(); + translateToBoardCenter.release(); + translateToImageCenter.release(); + T.release(); + T1x3.release(); + Rf.release(); + Rf3x3.release(); + t.release(); + + List rt = new ArrayList(2); + rt.add(RfVector); + rt.add(tVector); + + return rt; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* pose_planar_fullscreen */ + /* pose_planar_fullscreen */ + /* pose_planar_fullscreen */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + private static List pose_planar_fullscreen(Mat K, Mat cdist, Size img_size, Mat bbox) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + pose = Pose.PLANAR_FULL_SCREEN; + // don't use the principal point throughout just have X and Y no Z until it's calculated in the + // middle + // compute a new Z + // logger.debug("camera matrix K " + K + "\n" + K.dump()); + // logger.debug("cdist " + cdist.dump()); + // logger.debug("img_size " + img_size.toString()); + // logger.debug("bbox " + bbox + "\n" + bbox.dump()); + + Mat KB = new Mat(); // ignore principal point + Mat bboxZeroZ = new Mat(3, 1, CvType.CV_64FC1); + bboxZeroZ.put(0, 0, bbox.get(0, 0)[0], bbox.get(1, 0)[0], 0.); + + Core.gemm(K, bboxZeroZ, 1., new Mat(), 0., KB); + + double KBx = KB.get(0, 0)[0]; + double KBy = KB.get(1, 0)[0]; + double Z = Math.min(KBx / img_size.width, KBy / img_size.height); + double[] pB = {KBx / Z, KBy / Z}; + + Mat r = new Mat(1, 3, CvType.CV_64FC1); + r.put(0, 0, Math.PI, 0., 0.); // flip image + + // move board to center, org = bl + MatOfPoint2f p = + new MatOfPoint2f( + new Point(img_size.width / 2. - pB[0] / 2., img_size.height / 2. + pB[1] / 2.)); + + Mat t = unproject(p, K, cdist, Z); + + // logger.debug("KBnoPrinciplePoint " + KB + KB.dump()); + // logger.debug("Z, pB(x, y) " + Z + ", " + java.util.Arrays.toString(pB)); + // logger.debug("p " + p + p.dump()); + // logger.debug("returning r " + r + r.dump()); + // logger.debug("returning t " + t + t.dump()); + + bboxZeroZ.release(); + KB.release(); + p.release(); + + List rt = new ArrayList<>(2); + rt.add(r); // don't release r; this is the object that is returned + rt.add(t); // don't release t; this is the object that is returned + return rt; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* pose_from_bounds */ + /* pose_from_bounds */ + /* pose_from_bounds */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * @param src_extParm + * @param tgt_rect + * @param K + * @param cdist + * @param img_sz + * @return + */ + private pose_from_boundsReturn pose_from_bounds( + Mat src_extParm, Rect tgt_rect, Mat K, Mat cdist, Size img_sz) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + pose = Pose.FROM_BOUNDS; + // logger.debug("src_extParm " + src_extParm + "\n" + src_extParm.dump()); // full ChArUcoBoard + // size + Z + // logger.debug("tgt_rect " + tgt_rect); // guidance board posed + // logger.debug("camera matrix K " + K + "\n" + K.dump()); + // logger.debug("cdist " + cdist.dump()); + // logger.debug("img_sz " + img_sz.toString()); // camera screen image size + + double[] src_ext = new double[(int) src_extParm.total()]; + src_extParm.get(0, 0, src_ext); + + boolean rot90 = tgt_rect.height > tgt_rect.width; + + int MIN_WIDTH = + (int) + Math.floor( + img_sz.width + / 3.333); // posed guidance board must be about a third or more of the camera, + // screen size + + // logger.debug("rot90 " + rot90); + + if (rot90) { + // flip width and height + // src_ext = src_ext.clone(); // cloning not needed in this implementation since already + // hiding parameter src_extParm + double swapWH = src_ext[0]; + src_ext[0] = src_ext[1]; + src_ext[1] = swapWH; + + if (tgt_rect.height < MIN_WIDTH) { + // double scale = MIN_WIDTH / tgt_rect.width; //FIXME is this wrong in original? + // tgt_rect.width => tgt_rect.height? - rkt + double scale = MIN_WIDTH / tgt_rect.height; + tgt_rect.height = MIN_WIDTH; + tgt_rect.width *= scale; + } + } else { + if (tgt_rect.width < MIN_WIDTH) { + double scale = MIN_WIDTH / tgt_rect.width; + tgt_rect.width = MIN_WIDTH; + tgt_rect.height *= scale; + } + } + double aspect = + src_ext[0] / src_ext[1]; // w/h of the full ChArUcoBoard // [2520; 1680; 2520] => 1.5 + + // match aspect ratio of tgt to src, but keep tl + if (!rot90) { + tgt_rect.height = (int) (tgt_rect.width / aspect); // adapt height + } else { + tgt_rect.width = (int) (tgt_rect.height * aspect); // adapt width + } + // logic error here (missing check), I'm sure, so fix it - rkt + // if target too wide reduce to image size; if target too high reduce to image size + if (tgt_rect.width > img_sz.width) { + aspect = img_sz.width / tgt_rect.width; + tgt_rect.width = (int) (tgt_rect.height * aspect); // adapt width + tgt_rect.height = (int) (tgt_rect.width * aspect); // adapt height + } + if (tgt_rect.height > img_sz.height) { + aspect = img_sz.height / tgt_rect.height; + tgt_rect.width = (int) (tgt_rect.height * aspect); // adapt width + tgt_rect.height = (int) (tgt_rect.width * aspect); // adapt height + } + + Mat r = new Mat(1, 3, CvType.CV_64FC1); + r.put(0, 0, Math.PI, 0., 0.); + + // org is bl (bottom left) + if (rot90) { + Mat R = new Mat(); + + Calib3d.Rodrigues(r, R); + + // logger.debug("R " + R.dump()); + Mat rz = new Mat(1, 3, CvType.CV_64FC1); + rz.put(0, 0, 0., 0., -Math.PI / 2.); + Mat Rz = new Mat(); + + Calib3d.Rodrigues(rz, Rz); + + // logger.debug("Rz " + Rz.dump()); + + Core.gemm(R, Rz, 1., new Mat(), 0., R); // rotation matrix of the input Euler Angles + + Calib3d.Rodrigues(R, r); + + r = r.t(); // (ravel) Rodrigues out is 3x1 and (most of) the rest of program is 1x3 + + R.release(); + Rz.release(); + rz.release(); + // org is tl (top left) + } + + double Z = (K.get(0, 0)[0] * src_ext[0]) / tgt_rect.width; + // logger.debug("before clip tgt_rect " + tgt_rect); + + // clip to image region + int[] min_off = {0, 0}; + int[] max_off = { + (int) (img_sz.width - tgt_rect.width), (int) (img_sz.height - tgt_rect.height) + }; + tgt_rect.x = Math.min(max_off[0], Math.max(tgt_rect.x, min_off[0])); + tgt_rect.y = Math.min(max_off[1], Math.max(tgt_rect.y, min_off[1])); + // logger.debug("after clip tgt_rect " + tgt_rect); + + if (!rot90) { + tgt_rect.y += tgt_rect.height; + } + + MatOfPoint2f p = new MatOfPoint2f(new Point(tgt_rect.x, tgt_rect.y)); + + Mat t = unproject(p, K, cdist, Z); + + if (!rot90) { + tgt_rect.y -= tgt_rect.height; + } + + // logger.debug("returning r " + r.dump()); + // logger.debug("returning t " + t.dump()); + // logger.debug("returning tgt_rect " + tgt_rect); + + return new pose_from_boundsReturn(r, t, tgt_rect); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* PoseGeneratorDist Constructor */ + /* PoseGeneratorDist Constructor */ + /* PoseGeneratorDist Constructor */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + // two angle bins - one for the x axis [0] and one for the y axis [1] + private Gen_Bin[] gb = { + new Gen_Bin(Math.toRadians(-70.), Math.toRadians(70.)), // valid poses: r_x -> -70° .. 70° + new Gen_Bin(Math.toRadians(-70.), Math.toRadians(70.)) + }; // valid poses: r_y -> -70° .. 70° + + /** + * getter for the angle bins per axis + * + * @param axis x = 0; y = 1 + * @return next angle iteration for the selected axis + */ + private Function orbital = + (axis) -> { + // !!!! NOT a normal function - it returns a different value each time + // from the angle binning "iterator" + /* + * generate next angles + * {next x , 0} for the x axis [0] + * OR + * { 0 , next y} for the y axis [1] + */ + double[] angle = {0., 0.}; + angle[axis] = gb[axis].next(); + return angle; + }; + + private static final int SUBSAMPLE = + 20; // there is not a good conversion in general for this type of Python variable. Assuming + // value doesn't change nor instantiated more than once and changed elsewhere this + // conversion works. + private Size img_size; + // self.stats = [1, 1] # number of (intrinsic, distortion) poses -- NOT USED + private double orbitalZ = 1.6; + private static final double rz = Math.PI / 8.; + + private Mat mask; + private double sgn = 1.; + + /** + * generate poses based on min/ max distortion + * + * @param img_size + */ + PoseGeneratorDist(Size img_size) { + logger.debug("Starting ----------------------------------------"); + + this.img_size = img_size; + mask = + Mat.zeros( + new Size( + Math.floor(img_size.width / this.SUBSAMPLE), + Math.floor(img_size.height / this.SUBSAMPLE)), + CvType + .CV_8UC1); // t() transpose not needed in Java since those bindings account for w-h + // reversal + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* compute_distortion */ + /* compute_distortion */ + /* compute_distortion */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + private List compute_distortion(Mat K, Mat cdist, int subsample) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + return Distortion.sparse_undistort_map(K, img_size, cdist, K, subsample); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* get_pose */ + /* get_pose */ + /* get_pose */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* + * Determines (returns) the rotation and translation vectors to apply to the guidance board to effect + * the desired pose that was determined to collect information to converge on the correct intrinsics. + * + * @param bbox bounding box size of the calibration pattern + * @param nk number of keyframes captured so far + * @param tgt_param intrinic number of parameter that should be optimized by the pose + * @param K camera matrix current calibration estimate + * @param cdist distortion coefficients current calibration estimate + * @return rotation vector and translation vector + * @throws Exception + */ + List get_pose(Mat bbox, int nk, int tgt_param, Mat K, Mat cdist) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + // logger.debug("bbox " + bbox + "\n" + bbox.dump()); + // logger.debug("nk " + nk); + // logger.debug("tgt_param " + tgt_param); + // logger.debug("camera matrix K " + K + "\n" + K.dump()); + // logger.debug("cdist " + cdist.dump()); + + // first frame will be orbital pose from fixed angles + if (nk == 0) { + // x is camera pointing ahead up/down; y is camera pointing left/right; z is camera rotated (Z + // is axis from camera to target) + // init sequence: first keyframe 0° flat - not up or down, 45° pointing left, 22.5° rotated + // CW + + /********************************************************************************************************* */ + return orbital_pose(bbox, 0., Math.PI / 4., this.orbitalZ, Math.PI / 8. /*probably this.rz*/); + /********************************************************************************************************* */ + } + + // second frame will be full screen planar based on the K estimated from the first frame + if (nk == 1) { + // init sequence: second keyframe + + /********************************************************************************************************* */ + return pose_planar_fullscreen(K, cdist, this.img_size, bbox); + /********************************************************************************************************* */ + } + + // poses for all the frames after the first two + // pose will be based on the parameter being analyzed + if (tgt_param < 4) { + // orbital pose is used for focal length + int axis = (tgt_param + 1) % 2; // f_y -> r_x + + // r, t = orbital_pose(bbox, *next(self.orbital[axis])); + + double[] angleIteration = + this.orbital.apply( + axis); // get the next iteration for x & y pair of angles for the given axis + // logger.debug("angleIteration " + java.util.Arrays.toString(angleIteration)); + + /********************************************************************************************************* */ + List rt = + orbital_pose(bbox, angleIteration[0], angleIteration[1], this.orbitalZ, this.rz); + /********************************************************************************************************* */ + + // logger.debug("rt " + rt.get(0).dump() + rt.get(1).dump()); + + Mat t = rt.get(1); + + if (tgt_param > 1) { + // nudge the principal point in the axis in process and unproject it + // "off" is Principal Point Cx, Cy from the intrinsics camera matrix + double[] offPPoint = {K.get(0, 2)[0], K.get(1, 2)[0]}; + offPPoint[tgt_param - 2] += + ((tgt_param - 2) == 0 ? this.img_size.width : this.img_size.height) * 0.05 * this.sgn; + MatOfPoint2f off = new MatOfPoint2f(new Point(offPPoint)); + + /********************************************************************************************************* */ + Mat off3d = unproject(off, K, cdist, t.get(0, 2)[0]); + /********************************************************************************************************* */ + + off3d.put(0, 2, 0.); // zero out the Z + Core.add(t, off3d, t); // pretty big offset being nearly the principal point + this.sgn = -this.sgn; + off.release(); + off3d.release(); + } + + rt.set(1, t); // update the nudged t and return r and t + + // logger.debug("returning rt " + rt.get(0).dump() + rt.get(1).dump()); + + return rt; + } + + /********************************************************************************************************* */ + List res = compute_distortion(K, cdist, this.SUBSAMPLE); + /********************************************************************************************************* */ + + Mat dpts = res.get(0); + Mat pts = res.get(1); + + /********************************************************************************************************* */ + Rect bounds = + Distortion.loc_from_dist( + pts, dpts, this.mask, false, 1.); // ignore previously used masked off areas + /********************************************************************************************************* */ + + if (bounds == null) { + logger.error("bounds is null, pose not contributing; guessing what to do"); + return get_pose( + bbox, nk, 3, K, cdist); // best guess of what the author meant to do (drop the axis) + } + + dpts.release(); + pts.release(); + + Rect tgt_rect = + new Rect( + bounds.x * this.SUBSAMPLE, + bounds.y * this.SUBSAMPLE, + bounds.width * this.SUBSAMPLE, + bounds.height * this.SUBSAMPLE); + /********************************************************************************************************* */ + pose_from_boundsReturn res2 = pose_from_bounds(bbox, tgt_rect, K, cdist, this.img_size); + /********************************************************************************************************* */ + Mat r = res2.r; + Mat t = res2.t; + Rect nbounds = res2.tgt_rect; + + // logger.debug("returning r " + r.dump()); + // logger.debug("returning t " + t.dump()); + // logger.debug("nbounds " + nbounds); + + nbounds.x = (int) Math.ceil((double) nbounds.x / this.SUBSAMPLE); + nbounds.y = (int) Math.ceil((double) nbounds.y / this.SUBSAMPLE); + nbounds.width = (int) Math.ceil((double) nbounds.width / this.SUBSAMPLE); + nbounds.height = (int) Math.ceil((double) nbounds.height / this.SUBSAMPLE); + + // The np.ceil of the scalar x is the smallest integer i, such that i >= x + // mask off y through y+h(-1) and x through x+w(-1) + Mat.ones(nbounds.height, nbounds.width, this.mask.type()) + .copyTo( + this.mask.submat( + nbounds.y, nbounds.y + nbounds.height, nbounds.x, nbounds.x + nbounds.width)); + + // logger.debug("mask count non-zeros = " + Core.countNonZero(this.mask) + "\n" + + // ArrayUtils.brief(this.mask)); + + List rt = new ArrayList<>(2); + rt.add(r); + rt.add(t); + + return rt; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* pose_from_boundsReturn class */ + /* pose_from_boundsReturn class */ + /* pose_from_boundsReturn class */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** container to return multiple values from pose_from_bounds */ + class pose_from_boundsReturn { + Mat r; + Mat t; + Rect tgt_rect; + + pose_from_boundsReturn(Mat r, Mat t, Rect tgt_rect) { + this.r = r; + this.t = t; + this.tgt_rect = tgt_rect; + } + } +} +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* End PoseGenerator class */ +/* End PoseGenerator class */ +/* End PoseGenerator class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ + +// Gen_Bin unit test +// PoseGeneratorDist pgd = new PoseGeneratorDist(new Size(1280., 720.)); +// // Gen_Bin gb = pgd.new Gen_Bin(Math.toRadians(-70.), Math.toRadians(70.)); +// Gen_Bin gb = pgd.new Gen_Bin(0., 1.); +// for (int i = 0; i < 40; i++) +// { +// System.out.println(gb.next()); +// } +// System.exit(0); + +// OpenCV uses reference counting. +// submat adds another reference to the data memory. +// .release() does not deallocate the memory, unless the last reference was removed/decremented. diff --git a/photon-core/src/main/java/org/photonvision/calibrator/README.md b/photon-core/src/main/java/org/photonvision/calibrator/README.md new file mode 100644 index 0000000000..157b74ac94 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/calibrator/README.md @@ -0,0 +1,56 @@ +# Java Camera Calibrator +Program guides user to pose the camera to the ChArUcoBoard for most efficient and accurate calibration. + +**Run from a terminal window either the Windows jar or Linux arm64 jar (RPi, etc.)** + +**don't double click the jar file name** + +`java -jar the_right_jar_file [options]` + +Run log is in the directory with the jar file. + +User specified Runtime Options are listed with `-help`, for example, on a Windows PC: + +`java -jar calib-photonvision-dev--winx64.jar -help` + +This is a minimal set of options from the many options set in the program at compile time. + +On a computer with an internal camera the camera ids on Windows typically are: +internal and external cameras connected at boot up: external=0, internal=1 +internal camera connected at boot up and external plugged in later: internal=0, external=1 +It may be convenient to disable the internal camera for extended calibration testing to make the camera id for the external constant. + +Start the program (successfully) to make a ChArUcoBoard.png file that can be printed. Alternatively the ChArUcoBoard file can be displayed in another frame on the computer screen or on a second screen. Use of a computer flat screen display can improve the camera calibration, however, there may be restrictions compared to using a paper board. Computer displays must be flat and visible at steep (nearly parallel) angles. For the smallest guidance target some displays may not have enough resolution or refresh rate to display the guidance (although it may look fine to the human eye). + +Run the program and aim the camera at the printed board in the pose that matches the guidance board on the computer screen. It may be easier to align a fixed camera and hold and move the board. + +The top line of the guidance display has movement information that isn't very useful. Below that are the guidance rotations and translations (r{x, y, z} t{x, y, z}). The camera should be aimed such that its rotations and translations should fairly closely match the guidance. The small insert of the outline of the guidance and the current camera pose indicate how clsoe the camera is to matching the guidance. + +The first two guidance poses are always the same. + +The first pose at a fairly steep angle to the board's left (camera's right) sets the initial camera matrix. I advise getting the angles correct but not at precisely the correct distance so the images do not align yet. Then carefully move closer or further to get the precise size alignment. The capture should be automatic. “Well begun is half done.” - Aristotle. [Rotated poses are good to calibrate the camera matrix.] The guidance board is always intended to be able to be duplicated by a camera pose. An occaisonal distorted board that is bent, broken or split indicates previously captured poses weren't close enough to the guidance. Start over. + +Occaisionally the rotated first pose "jumps" to a different size. That is NOT a capture and is the program trying to use the latest estimate of the camera matrix for that first pose. That pose is captured when the second pose - the head-on pose - appears. + +The second pose is straight head-on and sets the initial distortion. Similarly, get the straight-on correct, move to align the images but not yet the matching sizes then move closer or further to match. The capture should be automatic. [Straight-on poses are good to calibrate the distortion coefficients.] + +If the guidance board and the camera image match, the program should auto-capture that information. Rarely, but if the auto capture is not happening, the user can force capture by pressing "c" and Enter on the computer terminal window that was used to start the program. The black and white insert shows what the poses are of the exact guidance board and the estimated camera view. The similar numbers are the Jaccard index between the poses and the minimum acceptable value for auto capture. + +The nine red camera intrinsic parameters turn green when the poses provide enough information. Usually after about 10 poses. That information might not be good unless you have carefully aligned to the guidance and especially autocapture yields good information. (You can point incorrectly at the target and randomly hit the "c" and "enter" several times to complete a bad calibration; it doesn't care, although it has been known to throw an "unknown exception" and keep going as if nothing bad happened.) + +The terminal (keyboard) inputs are 'c' for force capture, 'm' for mirror view if that helps you align the camera to the guidance, and 'q' to quit. Rarely if ever are those needed. + +The display of the guidance board and camera view are on a browser's port 1185 to the computer running the program. For example, 127.0.0.1:1185?action=stream. + +The camera server is standard WPILib camera server stuff so you can adust many camera parameters on the automatically assigned port - 1181 or higher. For example 127.0.0.1:1181 to see the camera parameters. + +If you run this on a system with PhotonVision running, then stop PhotonVision. (linux command is `sudo service photonvision stop`) + +Camera calibration data is written to a json file suitable for import into PhotonVision. (Note the camera name and platform are unknown so if you needed those, edit the file.) +References: + +https://arxiv.org/pdf/1907.04096.pdf + +https://www.calibdb.net/# + +https://github.com/paroj/pose_calib diff --git a/photon-core/src/main/java/org/photonvision/calibrator/UserGuidance.java b/photon-core/src/main/java/org/photonvision/calibrator/UserGuidance.java new file mode 100644 index 0000000000..860d546038 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/calibrator/UserGuidance.java @@ -0,0 +1,736 @@ +/* + * 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 . + */ + +// This project and file are derived in part from the "Pose Calib" project by +// @author Pavel Rojtberg +// It is subject to his license terms in the PoseCalibLICENSE file. + +package org.photonvision.calibrator; + +import static org.photonvision.calibrator.ArrayUtils.argmax; +import static org.photonvision.calibrator.ArrayUtils.argmin; +import static org.photonvision.calibrator.ArrayUtils.isAllTrue; + +import java.io.FileNotFoundException; +import java.io.PrintWriter; +import java.text.SimpleDateFormat; +import java.time.LocalDateTime; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Date; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import org.opencv.calib3d.Calib3d; +import org.opencv.core.Core; +import org.opencv.core.CvType; +import org.opencv.core.Mat; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; + +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* UserGuidance class */ +/* UserGuidance class */ +/* UserGuidance class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +public class UserGuidance { + private static final Logger logger = new Logger(UserGuidance.class, LogGroup.General); + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* UserGuidance constructor */ + /* UserGuidance constructor */ + /* UserGuidance constructor */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + Calibrator calib; + + private final String[] AX_NAMES = {"red", "green", "blue"}; + private final String[] INTRINSICS = {"fx", "fy", "cx", "cy", "k1", "k2", "p1", "p2", "k3"}; + private final String[] POSE = {"fx", "ry", "rz", "tx", "ty", "tz"}; + + // parameters that are optimized by the same board poses + private final int PARAM_GROUPS[][] = { + {0, 1, 2, 3}, {4, 5, 6, 7, 8} + }; // grouping and numbering the INTRINSICS + + // get geometry from tracker + ChArucoDetector tracker; + private int allpts; + private int square_len; + private int marker_len; + private int SQUARE_LEN_PIX = 12; + + private Size img_size; + private Mat overlap; + private BoardPreview board; + private Mat board_units; + private Mat board_warped = new Mat(); + private double var_terminate; + private boolean converged = false; + private boolean[] pconverged; + private double min_reperr_init = Double.POSITIVE_INFINITY; + + private int tgt_param = + -999_999_999; // None in Python which throws error if accessed; this may throw an error if + // used as a subscript + + // actual user guidance + private double pose_close_to_tgt = 0.; + private boolean pose_reached = false; + private boolean capture = false; + private boolean still = false; + private String user_info_text = "initialization"; + + private PoseGeneratorDist posegen; + private Mat tgt_r = new Mat(); + private Mat tgt_t = new Mat(); + + // getters + boolean converged() { + return converged; + } + + String user_info_text() { + return user_info_text; + } + + Mat tgt_r() { + return tgt_r; + } + + Mat tgt_t() { + return tgt_t; + } + + boolean[] pconverged() { + return pconverged; + } + + String[] INTRINSICS() { + return INTRINSICS; + } + + double pose_close_to_tgt_get() { + return pose_close_to_tgt; + } + + public UserGuidance(ChArucoDetector tracker, double var_terminate) + throws Exception // force use of var_terminate=0.1 instead of defaulting + { + logger.debug("Starting ----------------------------------------"); + + this.tracker = tracker; + this.var_terminate = var_terminate; + this.calib = new Calibrator(tracker.img_size); + this.pconverged = new boolean[this.calib.nintr()]; // initialized to false by Java + this.allpts = + (Cfg.board_x - 1) + * (Cfg.board_y - 1); // board w = 9 h = 6 => 54 squares; 8x5 => 40 interior corners + this.square_len = Cfg.square_len; + this.marker_len = Cfg.marker_len; + this.SQUARE_LEN_PIX = this.square_len; + this.img_size = tracker.img_size; + this.overlap = Mat.zeros((int) this.img_size.height, (int) this.img_size.width, CvType.CV_8UC1); + + // preview image + this.board = new BoardPreview(this.tracker.boardImage); + + // desired pose of board for first frame + // translation defined in terms of board dimensions + this.board_units = new Mat(3, 1, CvType.CV_64FC1); + this.board_units.put(0, 0, tracker.board_sz().width * this.square_len); + this.board_units.put(1, 0, tracker.board_sz().height * this.square_len); + this.board_units.put(2, 0, tracker.board_sz().width * this.square_len); + this.posegen = new PoseGeneratorDist(this.img_size); + + // set first pose + this.set_next_pose(); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* calibrate */ + /* calibrate */ + /* calibrate */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + private void calibrate() { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + if (this.calib.keyframes.size() < 2) // need at least 2 keyframes + { + return; + } + + double[] pvar_prev = + this.calib.varIntrinsics().clone(); // calib still has the previous intrinsics to save here + boolean first = this.calib.keyframes.size() == 2; + + // compute the new intrinsics in calibrate + double[] index_of_dispersion = + this.calib.calibrate(new ArrayList<>(1)); // dummy arg to avoid overloaded method + + double[] pvar = + this.calib + .varIntrinsics(); // save the new intrinsics (shorter name to match original code) to + // compare with the previous + + double[] rel_pstd = new double[pvar.length]; + + if (!first) { + double total_var_prev = Arrays.stream(pvar_prev).sum(); + double total_var = Arrays.stream(pvar).sum(); + + if (total_var > total_var_prev) { + // logger.debug("note: total var degraded"); + } + // check for convergence + for (int i = 0; i < pvar.length; i++) { + rel_pstd[i] = + 1 - Math.sqrt(pvar[i]) / Math.sqrt(pvar_prev[i]); // relative change to each std dev + } + + // logger.debug("relative stddev " + Arrays.toString(rel_pstd)); + + if (rel_pstd[this.tgt_param] < 0) { + // logger.debug(this.INTRINSICS[this.tgt_param] + " degraded"); + } + + // g0(p0 p1 p2 p3) g1(p4 p5 p6 p7 p8) + for (int[] g : this.PARAM_GROUPS) // loop through all groups (2 groups) + { + // check if tgt_parm in this group + boolean inGroup = false; // first assume not in this group + for (int p : g) // loop through whole group (4 or 5 items) + { + if (this.tgt_param == p) { + inGroup = true; // found it in this group + break; // no need to check further + } + } + + if (!inGroup) { + continue; // not in this group so move on to next group + } + + StringBuilder converged = new StringBuilder(); + + for (int p : g) { + if (rel_pstd[p] > 0 && rel_pstd[p] < this.var_terminate) { + if (!this.pconverged[p]) { + converged.append(this.INTRINSICS[p]); + this.pconverged[p] = true; + } + } + } + if (converged.length() > 0) { + // logger.debug("{" + converged + "} converged"); + } + } + } + // if an intrinsic has converged, then set it to 0 so it can't be selected (again) as the max + for (int i = 0; i < this.pconverged.length; i++) { + if (this.pconverged[i]) { + index_of_dispersion[i] = 0.; + } + } + + this.tgt_param = argmax(index_of_dispersion); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* set_next_pose */ + /* set_next_pose */ + /* set_next_pose */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + private void set_next_pose() { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + int nk = this.calib.keyframes.size(); + + List rt = + this.posegen.get_pose( + this.board_units, // rotation and translation of the guidance board + nk, + this.tgt_param, + this.calib.K(), + this.calib.cdist()); + rt.get(0).copyTo(this.tgt_r); + rt.get(1).copyTo(this.tgt_t); + + rt.get(1).release(); + rt.remove(1); + rt.get(0).release(); + rt.remove(0); + + this.board.create_maps(this.calib.K(), this.calib.cdist(), this.img_size); + // make the guidance board warped and right size + // board_warped_shape = # Height Width Channels (720, 1280, 3) + this.board_warped.release(); + + this.board_warped = this.board.project(this.tgt_r, this.tgt_t, false, Imgproc.INTER_NEAREST); + + // logger.debug("r/t and board_warped " + this.tgt_r.dump() + this.tgt_t.dump() + + // board_warped); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* pose_close_to_tgt */ + /* pose_close_to_tgt */ + /* pose_close_to_tgt */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * Jaccard similarity coefficient (Jaccard index) compares estimated image pose and target + * GuidanceBoard pose. Change from original - Returning numerical index instead of the true/false + * decision of close to target. Calling program can decide what to do with the number. This can + * put all the decisions in the same place instead of dispersed. + * + * @return Jaccard similarity coefficient of estimated image pose and desired (target) guidance + * pose + */ + private double pose_close_to_tgt() { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + // logger.debug("pose_valid " + this.tracker.pose_valid() + ", tgt_r empty " + + // this.tgt_r.empty()); + + double jaccard = 0.; + + if (!this.tracker.pose_valid()) return jaccard; + + if (this.tgt_r.empty()) return jaccard; + + byte[] board_warpedArray = + new byte + [this.board_warped.rows() * this.board_warped.cols() * this.board_warped.channels()]; + this.board_warped.get(0, 0, board_warpedArray); // efficient retrieval of complete board_warped + + byte[] overlapArray = + new byte + [this.overlap.rows() * this.overlap.cols()]; // 1 channel; java sets this to all zeros + + int indexBoard_warpedArray = 1; // start position; extracting channel 1 (of 0, 1, 2) + for (int row = 0; row < overlapArray.length; row++) { + if (board_warpedArray[indexBoard_warpedArray] != 0) { + overlapArray[row] = 1; + } + indexBoard_warpedArray += 3; // bump to next pixel; incrementing by number of channels + } + this.overlap.put(0, 0, overlapArray); + + int Aa = + Core.countNonZero(this.overlap); // number of on (1) pixels in the warped_board (from above) + + Mat tmp = + this.board.project( + this.tracker + .rvec(), // create projected shadow same way as the guidance board but using the + // estimated pose of the camera image + this.tracker.tvec(), + true, + Imgproc.INTER_NEAREST); + // debug display + Mat tempImg = new Mat(); + tmp.copyTo( + Main.progressInsert); // test 1 has the board projected (warped) from where the detector + // thinks is the camera image pose + this.overlap.copyTo(tempImg); // tempImg has the warped guidance board + + Core.multiply( + Main.progressInsert, + Cfg.progressInsertCameraGrey, + Main.progressInsert); // brighten (to near white) so it can be seen by humans + Core.multiply( + tempImg, + Cfg.progressInsertGuidanceGrey, + tempImg); // brighten (to dark gray) so it can be seen by humans + Core.add( + Main.progressInsert, tempImg, Main.progressInsert); // where they overlap is bright white + + // logger.debug("shadow_warped created r/t " + this.tracker.rvec().dump() + + // this.tracker.tvec().dump() + board_warped); + + int Ab = Core.countNonZero(tmp); // number of on (1) pixels in the warped shadow board + Core.bitwise_and(this.overlap, tmp, this.overlap); // make the overlapped pixels on (1) + int Aab = + Core.countNonZero(this.overlap); // number of on (1) pixels that overlap on the 2 boards + + // circumvents instability during initialization and large variance in depth later on + // Jaccard similarity index + jaccard = (double) Aab / (double) (Aa + Ab - Aab); + + tmp.release(); + tempImg.release(); + + // logger.debug("jaccard " + jaccard); + + return jaccard; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* update */ + /* update */ + /* update */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * @param force + * @return true if a new pose was captured + * @throws Exception + */ + public boolean update(boolean force) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + // first time need to see at least half of the interior corners or force + if ((this.calib.keyframes.isEmpty() && this.tracker.N_pts() >= this.allpts / 2)) { + // logger.debug("initial calibrate"); + // try to estimate intrinsic params from single frame + this.calib.calibrate(Arrays.asList(this.tracker.get_calib_pts())); + + if (this.calib.reperr() + < this + .min_reperr_init) // assume K is all numeric - no way it couldn't be, original checked + // for nan but it never was + { + // logger.debug("initial set_next_pose and intrinsics"); + try { + this.set_next_pose(); + } catch (Exception e) { + logger.error("asdf", e); + } // update target pose + this.tracker.set_intrinsics(this.calib); + this.min_reperr_init = this.calib.reperr(); + } + } + + this.pose_reached = force && this.tracker.N_pts() >= Cfg.minCorners; // original had > 4 + + this.pose_close_to_tgt = this.pose_close_to_tgt(); + + if (this.pose_close_to_tgt > Cfg.pose_close_to_tgt_min) { + this.pose_reached = true; + } + // we need at least 57.5 points after 2 frames # rkt - the calc yields 27 with init nintr of 9, + // not 57.5 + // and 15 points per frame from then on + int n_required = ((this.calib.nintr() + 2 * 6) * 5 + 3) / (2 * 2); // 27 + + if (this.calib.keyframes.size() >= 2) { + n_required = 6 / 2 * 5; // yup - that's a 15 rkt + } + + this.still = this.tracker.mean_flow() < Cfg.mean_flow_max; + // use all points instead to ensure we have a stable pose + this.pose_reached &= this.tracker.N_pts() >= n_required; + + this.capture = this.pose_reached && (this.still || force); + + // logger.log(Level.WARNING, + // "corners " + this.tracker.N_pts() + + // ", pose_close_to_tgt " + pose_close_to_tgt + + // ", still " + this.still + + // ", mean_flow " + this.tracker.mean_flow() + + // ", pose_reached " + this.pose_reached + + // ", force " + force); + + if (!this.capture) { + return false; + } + + // image captured (saved) to use for calibration + // check for all parameters converged + // set the next guidance board pose if not all converged + + this.calib.keyframes.add(this.tracker.get_calib_pts()); + // logger.debug("keyframe captured " + this.calib.keyframes.size()); + // update calibration with all keyframe + this.calibrate(); + + // use the updated calibration results for tracking + this.tracker.set_intrinsics(this.calib); + + logger.debug("calibration image captured"); + logger.debug("camera matrix\n" + this.calib.K().dump()); + logger.debug("camera distortion " + this.calib.cdist().dump()); + + this.converged = isAllTrue(this.pconverged); + + if (this.converged) { + this.tgt_r.release(); + this.tgt_r = new Mat(); // clear the rotation + } else { + this.set_next_pose(); + } + + this._update_user_info(); + + return true; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* _update_user_info */ + /* _update_user_info */ + /* _update_user_info */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + private void _update_user_info() { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + this.user_info_text = ""; + + if (this.calib.keyframes.size() < 2) { + this.user_info_text = "initialization"; + } else if (!this.converged) { + String action = ""; + int axis; + if (this.tgt_param < 2) { + action = "rotate"; + // do not consider r_z as it does not add any information + double[] temp = {this.calib.pose_var()[0], this.calib.pose_var()[1]}; + axis = argmin(temp); + } else { + action = "translate"; + // do not consider t_z + // FIXME above comment doesn't seem to match the code below + double[] temp = { + this.calib.pose_var()[3], this.calib.pose_var()[4], this.calib.pose_var()[5] + }; + axis = + argmin(temp) + + 3; // find min of t_x, t_y, t_z and add 3 to that index to skip the rotation + // locations + } + String param = this.INTRINSICS[this.tgt_param]; + this.user_info_text = + String.format("{%s} {%s} to minimize {%s}", action, this.POSE[axis], param); + } else { + this.user_info_text = "converged at MSE: {" + this.calib.reperr() + "}"; + } + + if (this.pose_reached && !this.still) { + this.user_info_text += "\nhold camera steady"; + } + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* draw */ + /* draw */ + /* draw */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /** + * add the guidance board to the camera image to make the new user display + * + * @param img in/out; the composite of everything Mat that will be displayed to the user + * @param mirror + * @throws Exception + */ + public void draw(Mat img, boolean mirror) + // force users to specify mirror false instead of defaulting + { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + // assumes both img and board are 3 color channels BGR + if (!this.tgt_r.empty()) { + // process complete Mats' temp buffers for efficient access + byte[] imgBuff = new byte[img.rows() * img.cols() * img.channels()]; + byte[] board_warpedBuff = + new byte + [this.board_warped.rows() * this.board_warped.cols() * this.board_warped.channels()]; + + if (imgBuff.length != board_warpedBuff.length) { + logger.error("Wtf, i guess, guh"); + return; + } + + img.get(0, 0, imgBuff); // get the Mat + this.board_warped.get(0, 0, board_warpedBuff); // get the Mat + + for (int index = 0; index < imgBuff.length; index++) { + // if there is a non-black pixel in the warped board then use it in img + if (board_warpedBuff[index] != 0) { + imgBuff[index] = board_warpedBuff[index]; + } + } + img.put(0, 0, imgBuff); // update the Mat + } + + if (this.tracker.pose_valid()) { + this.tracker.draw_axis(img); // draw axes on the detected board from the camera image + } + + if (mirror) { + Core.flip(img, img, 1); + } + } + + /** + * seed NOT USED -- NOT CONVERTED seed NOT USED -- NOT CONVERTED seed NOT USED -- NOT CONVERTED + */ + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* write */ + /* write */ + /* write */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + void write() { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + String pattern = "yyyy-MM-dd-HH-mm-ss"; + SimpleDateFormat simpleDateFormat = new SimpleDateFormat(pattern); + + // ???????????????????????????????????????????? + // FIXME use the PV way to format JSON from MAT + // ???????????????????????????????????????????? + + String calibrationDataFile = + "CameraCalibrationData_" + simpleDateFormat.format(new Date()) + ".json"; + double[] cameraMatrix = new double[9]; + this.calib.K().get(0, 0, cameraMatrix); + double[] distortionCoefficients = new double[5]; + this.calib.cdist().get(0, 0, distortionCoefficients); + + logger.debug("calibration data file " + calibrationDataFile); + + try (PrintWriter pw = new PrintWriter(calibrationDataFile)) { + pw.println("{"); + pw.println(" \"camera\": \"unknown\","); + pw.println(" \"platform\": \"unknown\","); + pw.println(" \"avg_reprojection_error\": " + this.calib.reperr() + ","); + pw.format( + " \"camera_matrix\": [%n" + " [%f, %f, %f],%n [%f, %f, %f],%n [%f, %f, %f]%n" + "],%n", + cameraMatrix[0], + cameraMatrix[1], + cameraMatrix[2], + cameraMatrix[3], + cameraMatrix[4], + cameraMatrix[5], + cameraMatrix[6], + cameraMatrix[7], + cameraMatrix[8]); + pw.format( + " \"distortion_coefficients\":%n [%f, %f, %f, %f, %f ],%n", + distortionCoefficients[0], + distortionCoefficients[1], + distortionCoefficients[2], + distortionCoefficients[3], + distortionCoefficients[4]); + pw.println(" \"distortion_model\": \"rectilinear\","); + pw.format( + " \"img_size\": [%.0f, %.0f],%n", + this.calib.img_size().width, this.calib.img_size().height); + pw.format(" \"calibration_time\": \"%s\"%n", LocalDateTime.now()); + pw.print("}"); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } + logger.debug("calibration_time: " + LocalDateTime.now()); + logger.debug("nr_of_frames: " + this.calib.keyframes.size()); + logger.debug("image_width: " + this.calib.img_size().width); + logger.debug("image_height: " + this.calib.img_size().height); + logger.debug("board_width: " + this.tracker.board_sz().width); + logger.debug("board_height: " + this.tracker.board_sz().height); + logger.debug("square_size: " + this.square_len); + logger.debug("marker_size: " + this.marker_len); + logger.debug(formatFlags(calib.flags())); + logger.debug("fisheye_model: " + 0); + logger.debug("camera_matrix:\n" + this.calib.K().dump()); + logger.debug("distortion_coefficients:\n" + this.calib.cdist().dump()); + logger.debug("avg_reprojection_error: " + this.calib.reperr()); + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* formatFlags */ + /* formatFlags */ + /* formatFlags */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + static String formatFlags(int flagsCalibration) { + // logger.debug("method entered . . . . . . . . . . . . . . . . . . . . . . . ."); + + HashMap flags = new HashMap<>(3); + flags.put(Calib3d.CALIB_FIX_PRINCIPAL_POINT, "+fix_principal_point"); + flags.put(Calib3d.CALIB_ZERO_TANGENT_DIST, "+zero_tangent_dist"); + flags.put(Calib3d.CALIB_USE_LU, "+use_lu"); + flags.put(Calib3d.CALIB_FIX_ASPECT_RATIO, "+fix aspect ratio"); + flags.put(Calib3d.CALIB_FIX_PRINCIPAL_POINT, "+fix principal point"); + flags.put(Calib3d.CALIB_ZERO_TANGENT_DIST, "+zero tangent dist"); + flags.put(Calib3d.CALIB_FIX_K1, "+fix k1"); + flags.put(Calib3d.CALIB_FIX_K2, "+fix k2"); + flags.put(Calib3d.CALIB_FIX_K3, "+fix k3"); + + StringBuilder flags_str = new StringBuilder("flags: "); + int unknownFlags = flagsCalibration; // initially assume all flags are unknown to the hashmap + + for (Map.Entry flag : flags.entrySet()) { + if ((flagsCalibration & flag.getKey()) == flag.getKey()) { + flags_str.append(flag.getValue()); + unknownFlags -= flag.getKey(); // this flag is known so un-mark unknown flags + } + } + + flags_str.append(String.format("\nflags: %08x", flagsCalibration)); + if (unknownFlags != 0) { + flags_str.append(String.format("; unknown flag usage = %08x", unknownFlags)); + } + return flags_str.toString(); + } +} +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* End UserGuidance class */ +/* End UserGuidance class */ +/* End UserGuidance class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ diff --git a/photon-core/src/main/java/org/photonvision/calibrator/keyframe.java b/photon-core/src/main/java/org/photonvision/calibrator/keyframe.java new file mode 100644 index 0000000000..41f016006f --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/calibrator/keyframe.java @@ -0,0 +1,80 @@ +/* + * 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 . + */ + +// This project and file are derived in part from the "Pose Calib" project by +// @author Pavel Rojtberg +// It is subject to his license terms in the PoseCalibLICENSE file. + +package org.photonvision.calibrator; + +import org.opencv.core.Mat; +import org.photonvision.common.logging.LogGroup; +import org.photonvision.common.logging.Logger; + +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* keyframe class */ +/* keyframe class */ +/* keyframe class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +class keyframe { + private static final Logger logger = new Logger(keyframe.class, LogGroup.General); + + private Mat p2d; // detected ccorners in camera image + private Mat + p3d; // target ChArUcoBoard object - perfect without distortion in 3d space but ours is always + + // flat on a wall so Z = 0 + + // getters + Mat p2d() { + return p2d; + } + + Mat p3d() { + return p3d; + } + + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + /* */ + /* keyframe constructor */ + /* keyframe constructor */ + /* keyframe constructor */ + /* */ + /*-------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------*/ + keyframe(Mat p2d, Mat p3d) { + this.p2d = p2d; + this.p3d = p3d; + if (this.p2d.rows() != this.p3d.rows() || this.p2d.cols() != p3d.cols()) { + logger.error("size of p2d != p3d\n" + this.p2d.dump() + "\n" + this.p3d.dump()); + } + } +} +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ +/* */ +/* End keyframe class */ +/* End keyframe class */ +/* End keyframe class */ +/* */ +/*-------------------------------------------------------------------------------------------------*/ +/*-------------------------------------------------------------------------------------------------*/ diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java index e1a3bc8aae..5620a037c7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java @@ -27,6 +27,9 @@ import org.opencv.core.Mat; import org.opencv.core.Size; import org.opencv.imgcodecs.Imgcodecs; +import org.photonvision.calibrator.Cfg; +import org.photonvision.calibrator.ChArucoDetector; +import org.photonvision.calibrator.UserGuidance; import org.photonvision.common.configuration.ConfigManager; import org.photonvision.common.dataflow.DataChangeService; import org.photonvision.common.dataflow.events.OutgoingUIEvent; @@ -58,7 +61,8 @@ public class Calibrate3dPipeline // Getter methods have been set for calibrate and takeSnapshot private boolean takeSnapshot = false; - // Output of the corners + // Detected corners + // Frame resolution, object points, board corners final List> foundCornersList; /// Output of the calibration, getter method is set for this. @@ -77,11 +81,22 @@ public Calibrate3dPipeline() { this(12); } + ChArucoDetector tracker; + UserGuidance ugui; + public Calibrate3dPipeline(int minSnapshots) { super(PROCESSING_TYPE); this.settings = new Calibration3dPipelineSettings(); this.foundCornersList = new ArrayList<>(); this.minSnapshots = minSnapshots; + + try { + tracker = new ChArucoDetector(); + ugui = new UserGuidance(tracker, Cfg.var_terminate); + } catch (Exception e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } } @Override @@ -119,6 +134,23 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se // Check if the frame has chessboard corners var outputColorCVMat = new CVMat(); inputColorMat.copyTo(outputColorCVMat.getMat()); + + if (inputColorMat.height() != Cfg.image_height || inputColorMat.width() != Cfg.image_width) { + try { + Cfg.image_width = inputColorMat.width(); + Cfg.image_height = inputColorMat.height(); + tracker = new ChArucoDetector(); + ugui = new UserGuidance(tracker, Cfg.var_terminate); + } catch (Exception e) { + } + } + + tracker.detect(inputColorMat); + ugui.draw(outputColorCVMat.getMat(), true); + // displayOverlay(out, ugui); + + ugui.update(false); // calibrate + var findBoardResult = findBoardCornersPipe.run(Pair.of(inputColorMat, outputColorCVMat.getMat())).output; diff --git a/photon-server/ChArUcoBoard.jpg b/photon-server/ChArUcoBoard.jpg new file mode 100644 index 0000000000..ae3b5c1191 Binary files /dev/null and b/photon-server/ChArUcoBoard.jpg differ diff --git a/photon-server/ChArUcoBoard.png b/photon-server/ChArUcoBoard.png new file mode 100644 index 0000000000..19a8d8de6e Binary files /dev/null and b/photon-server/ChArUcoBoard.png differ