/src/main/java/com/shootoff/camera/autocalibration/AutoCalibrationManager.java
Java | 1129 lines | 756 code | 270 blank | 103 comment | 106 complexity | 198ed0b199869932f6b79bd968153056 MD5 | raw file
- /*
- * ShootOFF - Software for Laser Dry Fire Training
- * Copyright (C) 2016 phrack
- *
- * 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 <http://www.gnu.org/licenses/>.
- */
- package com.shootoff.camera.autocalibration;
- import java.awt.image.BufferedImage;
- import java.io.File;
- import java.util.ArrayList;
- import java.util.List;
- import java.util.Optional;
- import javafx.geometry.BoundingBox;
- import javafx.geometry.Bounds;
- import javafx.geometry.Dimension2D;
- import javafx.util.Callback;
- import org.opencv.calib3d.Calib3d;
- import org.opencv.core.Core;
- import org.opencv.core.CvType;
- import org.opencv.core.Mat;
- import org.opencv.core.MatOfPoint;
- import org.opencv.core.MatOfPoint2f;
- import org.opencv.core.Point;
- import org.opencv.core.RotatedRect;
- import org.opencv.core.Scalar;
- import org.opencv.core.Size;
- import org.opencv.core.TermCriteria;
- import org.opencv.imgproc.Imgproc;
- import org.opencv.imgproc.Moments;
- import org.opencv.highgui.Highgui;
- import org.slf4j.Logger;
- import org.slf4j.LoggerFactory;
- //import ch.qos.logback.classic.Level;
- import com.shootoff.camera.Camera;
- import com.shootoff.camera.CameraManager;
- public class AutoCalibrationManager {
- private static final Logger logger = LoggerFactory.getLogger(AutoCalibrationManager.class);
- private static final int PATTERN_WIDTH = 9;
- private static final int PATTERN_HEIGHT = 6;
- private static final double PAPER_MARGIN_WIDTH = 1.048;
- private static final double PAPER_MARGIN_HEIGHT = 1.063;
- private static final Size boardSize = new Size(PATTERN_WIDTH, PATTERN_HEIGHT);
- private Callback<Void, Void> callback;
- private CameraManager cameraManager;
- private final boolean calculateFrameDelay;
- // Stores the transformation matrix
- private Mat perspMat = null;
- // Stores the bounding box we'll pass back to CameraManager
- private Bounds boundingBox = null;
- private RotatedRect boundsRect;
- private boolean warpInitialized = false;
- private boolean isCalibrated = false;
- // Edge is 11 pixels wide. Squares are 168 pixels wide.
- // 11/168 = 0.06547619047619047619047619047619
- // Maybe I should have made it divisible...
- private static final double BORDER_FACTOR = 0.065476;
- private long frameTimestampBeforeFrameChange = -1;
- private Bounds boundsResult = null;
- private long frameDelayResult = -1;
- private final TermCriteria term = new TermCriteria(TermCriteria.EPS | TermCriteria.MAX_ITER, 60, 0.0001);
- /* Paper Pattern */
- private Optional<Dimension2D> paperDimensions = Optional.empty();
- private int stepThreeAttempts = 0;
- private final static int STEP_THREE_MAX_ATTEMPTS = 2;
- public Optional<Dimension2D> getPaperDimensions() {
- return paperDimensions;
- }
- public void setPaperDimensions(Optional<Dimension2D> paperDimensions) {
- this.paperDimensions = paperDimensions;
- }
- public AutoCalibrationManager(final CameraManager cameraManager, final boolean calculateFrameDelay) {
- this.cameraManager = cameraManager;
- this.calculateFrameDelay = calculateFrameDelay;
- }
- public void setCallback(final Callback<Void, Void> callback) {
- this.callback = callback;
- }
- public Mat getPerspMat() {
- return perspMat;
- }
- public Bounds getBoundsResult() {
- if (boundsResult == null)
- logger.error("getBoundsResult called when boundsResult==null, isCalibrated {}", isCalibrated);
- return boundsResult;
- }
- public long getFrameDelayResult() {
- return frameDelayResult;
- }
- public void reset() {
- isCalibrated = false;
- warpInitialized = false;
- boundsResult = null;
- boundsRect = null;
- boundingBox = null;
- callback = null;
- perspMat = null;
- frameDelayResult = -1;
- frameTimestampBeforeFrameChange = -1;
- stepThreeAttempts = 0;
- paperDimensions = Optional.empty();
- }
- // Converts to BW mat from bufferedimage
- public void preProcessFrame(final BufferedImage frame, final Mat mat) {
- final Mat matTemp;
- synchronized (frame) {
- matTemp = Camera.bufferedImageToMat(frame);
- }
- Imgproc.cvtColor(matTemp, mat, Imgproc.COLOR_BGR2GRAY);
- Imgproc.equalizeHist(mat, mat);
- if (logger.isTraceEnabled()) {
- String filename = String.format("grayscale.png");
- File file = new File(filename);
- filename = file.toString();
- Highgui.imwrite(filename, mat);
- }
- }
- private boolean isStepOneCompleted() {
- return !(boundsResult == null);
- }
- private boolean isStepTwoCompleted() {
- // If frameDelayResult > -1 OR calculateFrameDelay is False
- // Then step two is complete
- return !(frameDelayResult == -1 && calculateFrameDelay);
- }
- private boolean inStepTwo() {
- // We're in step two if frameTimestampBeforeFrameChange > -1
- // AND step two is not complete
- return !(frameTimestampBeforeFrameChange == -1 || isStepTwoCompleted());
- }
- private boolean isStepThreeCompleted() {
- return (stepThreeAttempts == STEP_THREE_MAX_ATTEMPTS);
- }
- private boolean isFinished() {
- return (isStepOneCompleted() && isStepTwoCompleted() && isStepThreeCompleted());
- }
- // Step one: Find main pattern, paper pattern optional
- // Step two: Frame delay, if enabled
- // Step three: Paper pattern attempt IF not found in step one
- public void processFrame(final BufferedImage frame) {
- logger.trace("processFrame");
-
- if (isFinished()) {
- callback();
- return;
- }
- Mat mat = new Mat();
- preProcessFrame(frame, mat);
- if (!isStepOneCompleted()) {
- stepOne(mat);
- if (isFinished()) callback();
- logger.trace("stepOne {}", isStepOneCompleted());
- return;
- }
- if (isStepOneCompleted() && !isStepTwoCompleted()) {
- stepTwo(mat);
- if (isFinished()) callback();
- if (isStepTwoCompleted()) {
- cameraManager.setArenaBackground(null);
- }
- logger.trace("stepTwo {}", isStepTwoCompleted());
- return;
- }
- if (isStepOneCompleted() && isStepTwoCompleted() && !isStepThreeCompleted()) {
- stepThreeAttempts++;
- cameraManager.setArenaBackground(null);
-
- mat = undistortFrame(mat);
- List<MatOfPoint2f> listPatterns = findPatterns(mat, true);
- if (listPatterns.isEmpty()) return;
- findPaperPattern(mat, listPatterns, false);
- if (isFinished())
- {
- callback();
- stepThreeAttempts = STEP_THREE_MAX_ATTEMPTS;
- }
- logger.trace("stepThree {}", isStepThreeCompleted());
- return;
- }
- logger.trace("finished {}", isFinished());
- if (isFinished()) callback();
- }
- private void stepTwo(Mat mat) {
- if (!inStepTwo()) {
- logger.debug("Step two: Checking frame delay");
- checkForFrameChange(mat);
- frameTimestampBeforeFrameChange = cameraManager.getCurrentFrameTimestamp();
- cameraManager.setArenaBackground(null);
- } else {
- final Optional<Long> frameDelay = checkForFrameChange(mat);
- if (frameDelay.isPresent()) {
- frameDelayResult = frameDelay.get();
- logger.debug("Step Two: frameDelayResult {}", frameDelayResult);
- }
- }
- }
- private void callback() {
- if (callback != null) {
- callback.call(null);
- callback = null;
- }
- }
- private void stepOne(Mat mat) {
- List<MatOfPoint2f> listPatterns = findPatterns(mat, true);
- if (listPatterns.isEmpty()) return;
- findPaperPattern(mat, listPatterns, true);
- if (listPatterns.isEmpty()) return;
- // Technically there could still be more than one pattern
- // or even a pattern that is much too small
- // But damn if we're gonna fix every problem the user gives us
- Optional<Bounds> bounds = calibrateFrame(listPatterns.get(0), mat);
- if (bounds.isPresent()) {
- boundsResult = bounds.get();
- } else {
- boundsResult = null;
- }
- }
- private List<MatOfPoint2f> findPatterns(Mat mat, boolean findMultiple) {
- List<MatOfPoint2f> patternList = new ArrayList<MatOfPoint2f>();
- int count = 0;
- while (true) {
- Optional<MatOfPoint2f> boardCorners = findChessboard(mat);
- if (boardCorners.isPresent()) {
- patternList.add(boardCorners.get());
- if (!findMultiple) break;
- final RotatedRect rect = getPatternDimensions(boardCorners.get());
- blankRotatedRect(mat, rect);
- if (logger.isTraceEnabled()) {
- String filename = String.format("blanked-box-%d.png", count);
- File file = new File(filename);
- filename = file.toString();
- Highgui.imwrite(filename, mat);
- }
- // Shortcut to not try to find three+ patterns
- // We never should see more than two but maybe that'll change
- // in the future
- findMultiple = false;
- } else {
- break;
- }
- count++;
- }
- return patternList;
- }
- private Dimension2D averageDimensions(Dimension2D d2d1, Dimension2D d2d2) {
- return new Dimension2D((d2d1.getWidth() + d2d2.getWidth()) / 2, (d2d1.getHeight() + d2d2.getHeight()) / 2);
- }
- private double[] patternLuminosity = { -1, -1, -1 };
- private Optional<Long> checkForFrameChange(Mat mat) {
- mat = undistortFrame(mat);
- final double[] pixel = getFrameDelayPixel(mat);
- // Initialize
- if (patternLuminosity[0] == -1) {
- patternLuminosity = pixel;
- return Optional.empty();
- }
- final Mat tempMat = new Mat(1, 2, CvType.CV_8UC3);
- tempMat.put(0, 0, patternLuminosity);
- tempMat.put(0, 1, pixel);
- Imgproc.cvtColor(tempMat, tempMat, Imgproc.COLOR_BGR2HSV);
- final long change = cameraManager.getCurrentFrameTimestamp() - frameTimestampBeforeFrameChange;
- if (tempMat.get(0, 1)[2] < .9 * tempMat.get(0, 0)[2]) {
- return Optional.of(change);
- } else if (change > 250) {
- return Optional.of(-1L);
- }
- return Optional.empty();
- }
- private double[] getFrameDelayPixel(Mat mat) {
- final double squareHeight = boundsResult.getHeight() / (double) (PATTERN_HEIGHT + 1);
- final double squareWidth = boundsResult.getWidth() / (double) (PATTERN_WIDTH + 1);
- final int secondSquareCenterX = (int) (boundsResult.getMinX() + (squareWidth * 1.5));
- final int secondSquareCenterY = (int) (boundsResult.getMinY() + (squareHeight * .5));
- return mat.get(secondSquareCenterY, secondSquareCenterX);
- }
- public Optional<Bounds> calibrateFrame(MatOfPoint2f boardCorners, Mat mat) {
- // For debugging
- Mat traceMat = null;
- if (logger.isTraceEnabled()) {
- traceMat = mat.clone();
- }
- // Turn the chessboard into corners
- final MatOfPoint2f boardRect = calcBoardRectFromCorners(boardCorners);
- // Estimate the pattern corners
- MatOfPoint2f estimatedPatternRect = estimatePatternRect(traceMat, boardRect);
- // More definitively find corners using goodFeaturesToTrack
- final Optional<Point[]> corners = findCorners(boardRect, mat, estimatedPatternRect);
- if (!corners.isPresent()) return Optional.empty();
- // Creates sorted cornerArray for warp perspective
- MatOfPoint2f corners2f = sortPointsForWarpPerspective(boardRect, corners.get());
- if (logger.isTraceEnabled()) {
- String filename = String.format("calibrate-dist.png");
- final File file = new File(filename);
- filename = file.toString();
- Highgui.imwrite(filename, traceMat);
- }
- // Initialize the warp matrix and bounding box
- initializeWarpPerspective(mat, corners2f);
- if (boundingBox.getMinX() < 0 || boundingBox.getMinY() < 0
- || boundingBox.getWidth() > cameraManager.getFeedWidth()
- || boundingBox.getHeight() > cameraManager.getFeedHeight()) {
- return Optional.empty();
- }
- if (logger.isDebugEnabled()) logger.debug("bounds {} {} {} {}", boundingBox.getMinX(), boundingBox.getMinY(),
- boundingBox.getWidth(), boundingBox.getHeight());
- if (logger.isTraceEnabled()) {
- final Mat undistorted = warpPerspective(mat);
- String filename = String.format("calibrate-undist.png");
- File file = new File(filename);
- filename = file.toString();
- Highgui.imwrite(filename, undistorted);
- Mat undistortedCropped = undistorted.submat((int) boundingBox.getMinY(), (int) boundingBox.getMaxY(),
- (int) boundingBox.getMinX(), (int) boundingBox.getMaxX());
- filename = String.format("calibrate-undist-cropped.png");
- file = new File(filename);
- filename = file.toString();
- Highgui.imwrite(filename, undistortedCropped);
- }
- Mat warpedBoardCorners = warpCorners(boardCorners);
- isCalibrated = true;
- if (calculateFrameDelay) {
- final Mat undistorted = warpPerspective(mat);
- findColors(undistorted, warpedBoardCorners);
- final double squareHeight = boundingBox.getHeight() / (double) (PATTERN_HEIGHT + 1);
- final double squareWidth = boundingBox.getWidth() / (double) (PATTERN_WIDTH + 1);
- int secondSquareCenterX = (int) (boundingBox.getMinX() + (squareWidth * 1.5));
- int secondSquareCenterY = (int) (boundingBox.getMinY() + (squareHeight * .5));
- if (logger.isDebugEnabled()) logger.debug("pF getFrameDelayPixel x {} y {} p {}", secondSquareCenterX,
- secondSquareCenterY, undistorted.get(secondSquareCenterY, secondSquareCenterX));
- }
- return Optional.of(boundingBox);
- }
- private MatOfPoint2f sortPointsForWarpPerspective(final MatOfPoint2f boardRect, final Point[] corners) {
- Point[] cornerArray = new Point[4];
- Double[] cornerED = new Double[4];
- Point[] boardRectArray = boardRect.toArray();
- for (int i = 0; i < 4; i++)
- cornerED[i] = -1.0;
- for (Point cpt : corners) {
- for (int i = 0; i < 4; i++) {
- double tempED = euclideanDistance(cpt, boardRectArray[i]);
- if (cornerED[i] == -1.0 || tempED < cornerED[i]) {
- cornerArray[i] = cpt;
- cornerED[i] = tempED;
- }
- }
- }
- MatOfPoint2f corners2f = new MatOfPoint2f();
- corners2f.fromArray(cornerArray);
- return corners2f;
- }
- private Optional<Point[]> findCorners(MatOfPoint2f boardRect, Mat mat, MatOfPoint2f estimatedPatternRect) {
- Point[] cornerArray = new Point[4];
- Mat mask;
- final Point[] estimatedPoints = estimatedPatternRect.toArray();
- // Establishes a search region
- long region = mat.total() / 19200;
- Mat tempMat = null;
- if (logger.isTraceEnabled()) {
- tempMat = new Mat(mat.size(), CvType.CV_8UC3);
- Imgproc.cvtColor(mat, tempMat, Imgproc.COLOR_GRAY2BGR);
- }
- int i = 0;
- for (Point pt : estimatedPoints) {
- MatOfPoint tempCorners = new MatOfPoint();
- mask = Mat.zeros(mat.size(), CvType.CV_8UC1);
- Point leftpt = new Point(pt.x - region, pt.y - region);
- Point rightpt = new Point(pt.x + region, pt.y + region);
- Core.rectangle(mask, leftpt, rightpt, new Scalar(255), -1);
- if (logger.isTraceEnabled()) {
- String filename = String.format("mask-%d.png", i);
- File file = new File(filename);
- filename = file.toString();
- Highgui.imwrite(filename, mask);
- }
- Imgproc.goodFeaturesToTrack(mat, tempCorners, 2, .10, 0, mask, 3, true, .04);
- if (tempCorners.empty()) return Optional.empty();
- Point res = null;
- long dist = mat.total();
- for (Point p : tempCorners.toArray()) {
- long tempDist = (long) (Math.min(mat.width() - p.x, p.x) + Math.min(mat.height() - p.y, p.y));
- if (tempDist < dist) {
- dist = tempDist;
- res = p;
- }
- if (logger.isTraceEnabled()) {
- logger.trace("corner {} {}", p.x, p.y);
- Core.circle(tempMat, p, 1, new Scalar(0, 0, 255), -1);
- }
- }
- cornerArray[i] = res;
- i++;
- }
- if (logger.isTraceEnabled()) {
- String filename = String.format("corners.png");
- File file = new File(filename);
- filename = file.toString();
- Highgui.imwrite(filename, tempMat);
- }
- return Optional.of(cornerArray);
- }
- /**
- * Perspective pattern discovery
- *
- * Works similar to arena calibration but does not try to identify the
- * outline of the projection area We are only concerned with size, not
- * alignment or angle
- *
- */
- public boolean findPaperPattern(Mat mat, List<MatOfPoint2f> patternList, boolean averagePatterns) {
- MatOfPoint2f boardCorners = null;
- int index = 0;
- boolean found = false;
- for (; index < patternList.size(); index++) {
- boardCorners = patternList.get(index);
- RotatedRect rect = getPatternDimensions(boardCorners);
- // OpenCV gives us the checkerboard corners, not the outside
- // dimension
- // So this estimates where the outside corner would be, plus a fudge
- // factor for the edge of the paper
- // Printer margins are usually a quarter inch on each edge
- double rect_width = rect.size.width, rect_height = rect.size.height;
- double width = rect_width, height = rect_height;
- // Flip them if its sideways
- if (height > width) {
- width = rect_height;
- height = rect_width;
- rect_height = width;
- rect_width = height;
- }
- width = ((double) width * ((double) (PATTERN_WIDTH + 1) / (double) (PATTERN_WIDTH - 1)) * PAPER_MARGIN_WIDTH
- * 1 + (BORDER_FACTOR / PATTERN_WIDTH));
- height = ((double) height * ((double) (PATTERN_HEIGHT + 1) / (double) (PATTERN_HEIGHT - 1))
- * PAPER_MARGIN_HEIGHT * 1 + (BORDER_FACTOR / PATTERN_HEIGHT));
- final double PAPER_PATTERN_SIZE_THRESHOLD = .25;
- if (width > PAPER_PATTERN_SIZE_THRESHOLD * mat.cols()
- || height > PAPER_PATTERN_SIZE_THRESHOLD * mat.rows()) {
- continue;
- }
- if (logger.isTraceEnabled()) {
- logger.trace("pattern width {} height {}", rect_width, rect_height);
- logger.trace("paper width {} height {}", width, height);
- }
- final Dimension2D newPaperDimensions = new Dimension2D(width, height);
- found = true;
- if (!paperDimensions.isPresent()) {
- paperDimensions = Optional.of(newPaperDimensions);
- logger.debug("Found paper dimensions {}", paperDimensions.get());
- } else if (paperDimensions.isPresent() && averagePatterns) {
- paperDimensions = Optional.of(averageDimensions(paperDimensions.get(), newPaperDimensions));
- logger.trace("Averaged paper dimensions {}", paperDimensions.get());
- } else
- {
- paperDimensions = Optional.of(newPaperDimensions);
- logger.debug("Found paper dimensions {}", paperDimensions.get());
- }
- break;
- }
- if (found) {
- logger.trace("Removing paper pattern from patternList (index {})", index);
- patternList.remove(index);
- return true;
- }
- return false;
- }
- // What a stupid function, can't be the best way
- private void blankRotatedRect(Mat mat, final RotatedRect rect) {
- Mat tempMat = Mat.zeros(mat.size(), CvType.CV_8UC1);
- Point points[] = new Point[4];
- rect.points(points);
- for (int i = 0; i < 4; ++i) {
- Core.line(tempMat, points[i], points[(i + 1) % 4], new Scalar(255, 255, 255));
- }
- Mat tempMask = Mat.zeros((mat.rows() + 2), (mat.cols() + 2), CvType.CV_8UC1);
- Imgproc.floodFill(tempMat, tempMask, rect.center, new Scalar(255, 255, 255), null, new Scalar(0, 0, 0),
- new Scalar(254, 254, 254), 4);
- if (logger.isTraceEnabled()) {
- String filename = String.format("poly.png");
- File file = new File(filename);
- filename = file.toString();
- Highgui.imwrite(filename, tempMat);
- }
- mat.setTo(new Scalar(0, 0, 0), tempMat);
- }
- private RotatedRect getPatternDimensions(MatOfPoint2f boardCorners) {
- final MatOfPoint2f boardRect2f = calcBoardRectFromCorners(boardCorners);
- return Imgproc.minAreaRect(boardRect2f);
- }
- private void findColors(Mat frame, Mat warpedBoardCorners) {
- final Point rCenter = findChessBoardSquareCenter(warpedBoardCorners, 2, 3);
- final Point gCenter = findChessBoardSquareCenter(warpedBoardCorners, 2, 5);
- final Point bCenter = findChessBoardSquareCenter(warpedBoardCorners, 2, 7);
- if (logger.isTraceEnabled()) {
- logger.trace("findColors {} {} {}", rCenter, gCenter, bCenter);
- logger.trace("findColors r {} {} {} {}", (int) rCenter.y - 10, (int) rCenter.y + 10, (int) rCenter.x - 10,
- (int) rCenter.x + 10);
- }
- final Scalar rMeanColor = Core.mean(
- frame.submat((int) rCenter.y - 10, (int) rCenter.y + 10, (int) rCenter.x - 10, (int) rCenter.x + 10));
- final Scalar gMeanColor = Core.mean(
- frame.submat((int) gCenter.y - 10, (int) gCenter.y + 10, (int) gCenter.x - 10, (int) gCenter.x + 10));
- final Scalar bMeanColor = Core.mean(
- frame.submat((int) bCenter.y - 10, (int) bCenter.y + 10, (int) bCenter.x - 10, (int) bCenter.x + 10));
- if (logger.isTraceEnabled()) {
- String filename = String.format("rColor.png");
- File file = new File(filename);
- filename = file.toString();
- Highgui.imwrite(filename, frame.submat((int) rCenter.y - 10, (int) rCenter.y + 10, (int) rCenter.x - 10,
- (int) rCenter.x + 10));
- filename = String.format("gColor.png");
- file = new File(filename);
- filename = file.toString();
- Highgui.imwrite(filename, frame.submat((int) gCenter.y - 10, (int) gCenter.y + 10, (int) gCenter.x - 10,
- (int) gCenter.x + 10));
- filename = String.format("bColor.png");
- file = new File(filename);
- filename = file.toString();
- Highgui.imwrite(filename, frame.submat((int) bCenter.y - 10, (int) bCenter.y + 10, (int) bCenter.x - 10,
- (int) bCenter.x + 10));
- }
- if (logger.isTraceEnabled()) logger.trace("meanColor {} {} {}", rMeanColor, gMeanColor, bMeanColor);
- }
- public BufferedImage undistortFrame(BufferedImage frame) {
- if (isCalibrated) {
- final Mat mat = Camera.bufferedImageToMat(frame);
- frame = Camera.matToBufferedImage(warpPerspective(mat));
- } else {
- logger.warn("undistortFrame called when isCalibrated is false");
- }
- return frame;
- }
- // MUST BE IN BGR pixel format.
- public Mat undistortFrame(Mat mat) {
- if (!isCalibrated) {
- logger.warn("undistortFrame called when isCalibrated is false");
- return mat;
- }
- return warpPerspective(mat);
- }
- private MatOfPoint2f estimatePatternRect(Mat traceMat, MatOfPoint2f boardRect) {
- // We use this to calculate the angle
- final RotatedRect boardBox = Imgproc.minAreaRect(boardRect);
- final double boardBoxAngle = boardBox.size.height > boardBox.size.width ? 90.0 + boardBox.angle
- : boardBox.angle;
- // This is the board corners with the angle eliminated
- final Mat unRotMat = getRotationMatrix(massCenterMatOfPoint2f(boardRect), boardBoxAngle);
- final MatOfPoint2f unRotatedRect = rotateRect(unRotMat, boardRect);
- // This is the estimated projection area that has minimum angle (Not
- // rotated)
- final MatOfPoint2f estimatedPatternSizeRect = estimateFullPatternSize(unRotatedRect);
- // This is what we'll use as the transformation target and bounds given
- // back to the cameramanager
- boundsRect = Imgproc.minAreaRect(estimatedPatternSizeRect);
- // We now rotate the estimation back to the original angle to use for
- // transformation source
- final Mat rotMat = getRotationMatrix(massCenterMatOfPoint2f(estimatedPatternSizeRect), -boardBoxAngle);
- final MatOfPoint2f rotatedPatternSizeRect = rotateRect(rotMat, estimatedPatternSizeRect);
- if (logger.isTraceEnabled()) {
- logger.trace("center {} angle {} width {} height {}", boardBox.center, boardBoxAngle, boardBox.size.width,
- boardBox.size.height);
- logger.debug("boundsRect {} {} {} {}", boundsRect.boundingRect().x, boundsRect.boundingRect().y,
- boundsRect.boundingRect().x + boundsRect.boundingRect().width,
- boundsRect.boundingRect().y + boundsRect.boundingRect().height);
- Core.circle(traceMat, new Point(boardRect.get(0, 0)[0], boardRect.get(0, 0)[1]), 1, new Scalar(255, 0, 0),
- -1);
- Core.circle(traceMat, new Point(boardRect.get(1, 0)[0], boardRect.get(1, 0)[1]), 1, new Scalar(255, 0, 0),
- -1);
- Core.circle(traceMat, new Point(boardRect.get(2, 0)[0], boardRect.get(2, 0)[1]), 1, new Scalar(255, 0, 0),
- -1);
- Core.circle(traceMat, new Point(boardRect.get(3, 0)[0], boardRect.get(3, 0)[1]), 1, new Scalar(255, 0, 0),
- -1);
- Core.line(traceMat, new Point(unRotatedRect.get(0, 0)[0], unRotatedRect.get(0, 0)[1]),
- new Point(unRotatedRect.get(1, 0)[0], unRotatedRect.get(1, 0)[1]), new Scalar(0, 255, 0));
- Core.line(traceMat, new Point(unRotatedRect.get(1, 0)[0], unRotatedRect.get(1, 0)[1]),
- new Point(unRotatedRect.get(2, 0)[0], unRotatedRect.get(2, 0)[1]), new Scalar(0, 255, 0));
- Core.line(traceMat, new Point(unRotatedRect.get(3, 0)[0], unRotatedRect.get(3, 0)[1]),
- new Point(unRotatedRect.get(2, 0)[0], unRotatedRect.get(2, 0)[1]), new Scalar(0, 255, 0));
- Core.line(traceMat, new Point(unRotatedRect.get(3, 0)[0], unRotatedRect.get(3, 0)[1]),
- new Point(unRotatedRect.get(0, 0)[0], unRotatedRect.get(0, 0)[1]), new Scalar(0, 255, 0));
- Core.line(traceMat, new Point(estimatedPatternSizeRect.get(0, 0)[0], estimatedPatternSizeRect.get(0, 0)[1]),
- new Point(estimatedPatternSizeRect.get(1, 0)[0], estimatedPatternSizeRect.get(1, 0)[1]),
- new Scalar(255, 255, 0));
- Core.line(traceMat, new Point(estimatedPatternSizeRect.get(1, 0)[0], estimatedPatternSizeRect.get(1, 0)[1]),
- new Point(estimatedPatternSizeRect.get(2, 0)[0], estimatedPatternSizeRect.get(2, 0)[1]),
- new Scalar(255, 255, 0));
- Core.line(traceMat, new Point(estimatedPatternSizeRect.get(3, 0)[0], estimatedPatternSizeRect.get(3, 0)[1]),
- new Point(estimatedPatternSizeRect.get(2, 0)[0], estimatedPatternSizeRect.get(2, 0)[1]),
- new Scalar(255, 255, 0));
- Core.line(traceMat, new Point(estimatedPatternSizeRect.get(3, 0)[0], estimatedPatternSizeRect.get(3, 0)[1]),
- new Point(estimatedPatternSizeRect.get(0, 0)[0], estimatedPatternSizeRect.get(0, 0)[1]),
- new Scalar(255, 255, 0));
- Core.line(traceMat, new Point(rotatedPatternSizeRect.get(0, 0)[0], rotatedPatternSizeRect.get(0, 0)[1]),
- new Point(rotatedPatternSizeRect.get(1, 0)[0], rotatedPatternSizeRect.get(1, 0)[1]),
- new Scalar(255, 255, 0));
- Core.line(traceMat, new Point(rotatedPatternSizeRect.get(1, 0)[0], rotatedPatternSizeRect.get(1, 0)[1]),
- new Point(rotatedPatternSizeRect.get(2, 0)[0], rotatedPatternSizeRect.get(2, 0)[1]),
- new Scalar(255, 255, 0));
- Core.line(traceMat, new Point(rotatedPatternSizeRect.get(3, 0)[0], rotatedPatternSizeRect.get(3, 0)[1]),
- new Point(rotatedPatternSizeRect.get(2, 0)[0], rotatedPatternSizeRect.get(2, 0)[1]),
- new Scalar(255, 255, 0));
- Core.line(traceMat, new Point(rotatedPatternSizeRect.get(3, 0)[0], rotatedPatternSizeRect.get(3, 0)[1]),
- new Point(rotatedPatternSizeRect.get(0, 0)[0], rotatedPatternSizeRect.get(0, 0)[1]),
- new Scalar(255, 255, 0));
- }
- return rotatedPatternSizeRect;
- }
- /*
- * This function takes a rectangular region representing the chessboard
- * inner corners and estimates the corners of the full pattern image
- */
- private MatOfPoint2f estimateFullPatternSize(MatOfPoint2f rect) {
- // Result Mat
- final MatOfPoint2f result = new MatOfPoint2f();
- result.alloc(4);
- // Get the sources as points
- final Point topLeft = new Point(rect.get(0, 0)[0], rect.get(0, 0)[1]);
- final Point topRight = new Point(rect.get(1, 0)[0], rect.get(1, 0)[1]);
- final Point bottomRight = new Point(rect.get(2, 0)[0], rect.get(2, 0)[1]);
- final Point bottomLeft = new Point(rect.get(3, 0)[0], rect.get(3, 0)[1]);
- // We need the heights and widths to estimate the square sizes
- final double topWidth = Math.sqrt(Math.pow(topRight.x - topLeft.x, 2) + Math.pow(topRight.y - topLeft.y, 2));
- final double leftHeight = Math
- .sqrt(Math.pow(bottomLeft.x - topLeft.x, 2) + Math.pow(bottomLeft.y - topLeft.y, 2));
- final double bottomWidth = Math
- .sqrt(Math.pow(bottomRight.x - bottomLeft.x, 2) + Math.pow(bottomRight.y - bottomLeft.y, 2));
- final double rightHeight = Math
- .sqrt(Math.pow(bottomRight.x - topRight.x, 2) + Math.pow(bottomRight.y - topRight.y, 2));
- if (logger.isTraceEnabled()) {
- logger.trace("points {} {} {} {}", topLeft, topRight, bottomRight, bottomLeft);
- double angle = Math.atan((topRight.y - topLeft.y) / (topRight.x - topLeft.x)) * 180 / Math.PI;
- double angle2 = Math.atan((bottomRight.y - bottomLeft.y) / (bottomRight.x - bottomLeft.x)) * 180 / Math.PI;
- logger.trace("square size {} {} - angle {}", topWidth / (PATTERN_WIDTH - 1),
- leftHeight / (PATTERN_HEIGHT - 1), angle);
- logger.trace("square size {} {} - angle {}", bottomWidth / (PATTERN_WIDTH - 1),
- rightHeight / (PATTERN_HEIGHT - 1), angle2);
- }
- // Estimate the square widths, that is what we base the estimate of the
- // real corners on
- double squareTopWidth = (1 + BORDER_FACTOR) * (topWidth / (PATTERN_WIDTH - 1));
- double squareLeftHeight = (1 + BORDER_FACTOR) * (leftHeight / (PATTERN_HEIGHT - 1));
- double squareBottomWidth = (1 + BORDER_FACTOR) * (bottomWidth / (PATTERN_WIDTH - 1));
- double squareRightHeight = (1 + BORDER_FACTOR) * (rightHeight / (PATTERN_HEIGHT - 1));
- // The estimations
- double[] newTopLeft = { topLeft.x - squareTopWidth, topLeft.y - squareLeftHeight };
- double[] newBottomLeft = { bottomLeft.x - squareBottomWidth, bottomLeft.y + squareLeftHeight };
- double[] newTopRight = { topRight.x + squareTopWidth, topRight.y - squareRightHeight };
- double[] newBottomRight = { bottomRight.x + squareBottomWidth, bottomRight.y + squareRightHeight };
- // Populate the result
- result.put(0, 0, newTopLeft);
- result.put(1, 0, newTopRight);
- result.put(2, 0, newBottomRight);
- result.put(3, 0, newBottomLeft);
- return result;
- }
- // Given a rotation matrix and a quadrilateral, rotate the points
- private MatOfPoint2f rotateRect(Mat rotMat, MatOfPoint2f boardRect) {
- final MatOfPoint2f result = new MatOfPoint2f();
- result.alloc(4);
- for (int i = 0; i < 4; i++) {
- final Point rPoint = rotPoint(rotMat, new Point(boardRect.get(i, 0)[0], boardRect.get(i, 0)[1]));
- final double[] rPointD = new double[2];
- rPointD[0] = rPoint.x;
- rPointD[1] = rPoint.y;
- result.put(i, 0, rPointD);
- }
- return result;
- }
- private Mat getRotationMatrix(final Point center, final double rotationAngle) {
- return Imgproc.getRotationMatrix2D(center, rotationAngle, 1.0);
- }
- /*
- * The one time calculation of the transformations.
- *
- * After this is done, the transformation is just applied
- */
- private void initializeWarpPerspective(final Mat frame, final MatOfPoint2f sourceCorners) {
- final MatOfPoint2f destCorners = new MatOfPoint2f();
- destCorners.alloc(4);
- destCorners.put(0, 0, new double[] { boundsRect.boundingRect().x, boundsRect.boundingRect().y });
- destCorners.put(1, 0, new double[] { boundsRect.boundingRect().x + boundsRect.boundingRect().width,
- boundsRect.boundingRect().y });
- destCorners.put(3, 0, new double[] { boundsRect.boundingRect().x,
- boundsRect.boundingRect().y + boundsRect.boundingRect().height });
- destCorners.put(2, 0, new double[] { boundsRect.boundingRect().x + boundsRect.boundingRect().width,
- boundsRect.boundingRect().y + boundsRect.boundingRect().height });
- if (logger.isTraceEnabled()) {
- logger.trace("initializeWarpPerspective src corners {} {} {} {}", sourceCorners.get(0, 0),
- sourceCorners.get(1, 0), sourceCorners.get(2, 0), sourceCorners.get(3, 0));
- logger.trace("initializeWarpPerspective dest corners {} {} {} {}", destCorners.get(0, 0),
- destCorners.get(1, 0), destCorners.get(2, 0), destCorners.get(3, 0));
- }
- perspMat = Imgproc.getPerspectiveTransform(sourceCorners, destCorners);
- int width = boundsRect.boundingRect().width;
- int height = boundsRect.boundingRect().height;
- // Make them divisible by two for video recording purposes
- if ((width & 1) == 1) width++;
- if ((height & 1) == 1) height++;
- boundingBox = new BoundingBox(boundsRect.boundingRect().x, boundsRect.boundingRect().y, width, height);
- warpInitialized = true;
- if (logger.isTraceEnabled()) {
- Mat debugFrame = frame.clone();
- Core.circle(debugFrame, new Point(sourceCorners.get(0, 0)[0], sourceCorners.get(0, 0)[1]), 1,
- new Scalar(255, 0, 255), -1);
- Core.circle(debugFrame, new Point(sourceCorners.get(1, 0)[0], sourceCorners.get(1, 0)[1]), 1,
- new Scalar(255, 0, 255), -1);
- Core.circle(debugFrame, new Point(sourceCorners.get(2, 0)[0], sourceCorners.get(2, 0)[1]), 1,
- new Scalar(255, 0, 255), -1);
- Core.circle(debugFrame, new Point(sourceCorners.get(3, 0)[0], sourceCorners.get(3, 0)[1]), 1,
- new Scalar(255, 0, 255), -1);
- Core.circle(debugFrame, new Point(destCorners.get(0, 0)[0], destCorners.get(0, 0)[1]), 1,
- new Scalar(255, 0, 0), -1);
- Core.circle(debugFrame, new Point(destCorners.get(1, 0)[0], destCorners.get(1, 0)[1]), 1,
- new Scalar(255, 0, 0), -1);
- Core.circle(debugFrame, new Point(destCorners.get(2, 0)[0], destCorners.get(2, 0)[1]), 1,
- new Scalar(255, 0, 0), -1);
- Core.circle(debugFrame, new Point(destCorners.get(3, 0)[0], destCorners.get(3, 0)[1]), 1,
- new Scalar(255, 0, 0), -1);
- Core.line(debugFrame, new Point(boundingBox.getMinX(), boundingBox.getMinY()),
- new Point(boundingBox.getMaxX(), boundingBox.getMinY()), new Scalar(0, 255, 0));
- Core.line(debugFrame, new Point(boundingBox.getMinX(), boundingBox.getMinY()),
- new Point(boundingBox.getMinX(), boundingBox.getMaxY()), new Scalar(0, 255, 0));
- Core.line(debugFrame, new Point(boundingBox.getMaxX(), boundingBox.getMaxY()),
- new Point(boundingBox.getMaxX(), boundingBox.getMinY()), new Scalar(0, 255, 0));
- Core.line(debugFrame, new Point(boundingBox.getMaxX(), boundingBox.getMaxY()),
- new Point(boundingBox.getMinX(), boundingBox.getMaxY()), new Scalar(0, 255, 0));
- String filename = String.format("calibrate-transformation.png");
- File file = new File(filename);
- filename = file.toString();
- Highgui.imwrite(filename, debugFrame);
- }
- }
- // initializeWarpPerspective MUST BE CALLED first
- private Mat warpPerspective(final Mat frame) {
- if (warpInitialized) {
- final Mat mat = new Mat();
- Imgproc.warpPerspective(frame, mat, perspMat, frame.size(), Imgproc.INTER_LINEAR);
- return mat;
- } else {
- logger.warn("warpPerspective called when warpInitialized is false - {} {} - {}", perspMat, boundingBox,
- isCalibrated);
- return frame;
- }
- }
- // initializeWarpPerspective MUST BE CALLED first
- private Mat warpCorners(MatOfPoint2f imageCorners) {
- Mat mat = null;
- if (warpInitialized) {
- mat = new Mat();
- Core.transform(imageCorners, mat, perspMat);
- } else {
- logger.warn("warpCorners called when warpInitialized is false - {} {} - {}", perspMat, boundingBox,
- isCalibrated);
- }
- return mat;
- }
- public Optional<MatOfPoint2f> findChessboard(Mat mat) {
- MatOfPoint2f imageCorners = new MatOfPoint2f();
- boolean found = Calib3d.findChessboardCorners(mat, boardSize, imageCorners,
- Calib3d.CALIB_CB_ADAPTIVE_THRESH | Calib3d.CALIB_CB_NORMALIZE_IMAGE);
- logger.trace("found {}", found);
- if (found) {
- // optimization
- Imgproc.cornerSubPix(mat, imageCorners, new Size(1, 1), new Size(-1, -1), term);
- return Optional.of(imageCorners);
- }
- return Optional.empty();
- }
- // converts the chessboard corners into a quadrilateral
- private MatOfPoint2f calcBoardRectFromCorners(MatOfPoint2f corners) {
- MatOfPoint2f result = new MatOfPoint2f();
- result.alloc(4);
- Point topLeft = new Point(corners.get(0, 0)[0], corners.get(0, 0)[1]);
- Point topRight = new Point(corners.get(PATTERN_WIDTH - 1, 0)[0], corners.get(PATTERN_WIDTH - 1, 0)[1]);
- Point bottomRight = new Point(corners.get(PATTERN_WIDTH * PATTERN_HEIGHT - 1, 0)[0],
- corners.get(PATTERN_WIDTH * PATTERN_HEIGHT - 1, 0)[1]);
- Point bottomLeft = new Point(corners.get(PATTERN_WIDTH * (PATTERN_HEIGHT - 1), 0)[0],
- corners.get(PATTERN_WIDTH * (PATTERN_HEIGHT - 1), 0)[1]);
- Point[] unsorted = { topLeft, topRight, bottomLeft, bottomRight };
- Point[] sorted = sortCorners(unsorted);
- result.fromArray(sorted);
- // result.put(0, 0, topLeft.x, topLeft.y, topRight.x, topRight.y,
- // bottomRight.x, bottomRight.y, bottomLeft.x,
- // bottomLeft.y);
- return result;
- }
- // Given 4 corners, use the mass center to arrange the corners into correct
- // order
- // 1st-------2nd
- // | |
- // | |
- // | |
- // 4th-------3rd
- private Point[] sortCorners(Point[] corners) {
- Point[] result = new Point[4];
- Point center = new Point(0, 0);
- for (Point corner : corners) {
- center.x += corner.x;
- center.y += corner.y;
- }
- center.x *= (1.0 / corners.length);
- center.y *= (1.0 / corners.length);
- List<Point> top = new ArrayList<Point>();
- List<Point> bot = new ArrayList<Point>();
- for (int i = 0; i < corners.length; i++) {
- if (corners[i].y < center.y)
- top.add(corners[i]);
- else
- bot.add(corners[i]);
- }
- result[0] = top.get(0).x > top.get(1).x ? top.get(1) : top.get(0);
- result[1] = top.get(0).x > top.get(1).x ? top.get(0) : top.get(1);
- result[2] = bot.get(0).x > bot.get(1).x ? bot.get(0) : bot.get(1);
- result[3] = bot.get(0).x > bot.get(1).x ? bot.get(1) : bot.get(0);
- return result;
- }
- private Point findChessBoardSquareCenter(Mat corners, int row, int col) {
- if (row >= PATTERN_HEIGHT - 1 || col >= PATTERN_WIDTH - 1) {
- logger.warn("findChessBoardSquareColor invalid row or col {} {}", row, col);
- return null;
- }
- final Point topLeft = new Point(corners.get((row * PATTERN_WIDTH - 1) + col, 0)[0],
- corners.get((row * PATTERN_WIDTH - 1) + col, 0)[1]);
- final Point bottomRight = new Point(corners.get(((row + 1) * PATTERN_WIDTH - 1) + col + 1, 0)[0],
- corners.get(((row + 1) * PATTERN_WIDTH - 1) + col + 1, 0)[1]);
- final Point result = new Point((topLeft.x + bottomRight.x) / 2, (topLeft.y + bottomRight.y) / 2);
- if (logger.isTraceEnabled()) {
- logger.trace("findChessBoardSquareColor {}", corners.size());
- logger.trace("findChessBoardSquareColor {} {}", (row * PATTERN_WIDTH - 1) + col,
- ((row + 1) * PATTERN_WIDTH - 1) + col + 1);
- logger.trace("findChessBoardSquareColor {} {} {}", topLeft, bottomRight, result);
- }
- return result;
- }
- private double euclideanDistance(final Point pt1, final Point pt2) {
- return Math.sqrt(Math.pow(pt1.x - pt2.x, 2) + Math.pow(pt1.y - pt2.y, 2));
- }
- // Given a rotation matrix, rotates a point
- private Point rotPoint(final Mat rot_mat, final Point point) {
- final Point rp = new Point();
- rp.x = rot_mat.get(0, 0)[0] * point.x + rot_mat.get(0, 1)[0] * point.y + rot_mat.get(0, 2)[0];
- rp.y = rot_mat.get(1, 0)[0] * point.x + rot_mat.get(1, 1)[0] * point.y + rot_mat.get(1, 2)[0];
- return rp;
- }
- private Point massCenterMatOfPoint2f(final MatOfPoint2f map) {
- final Moments moments = Imgproc.moments(map);
- final Point centroid = new Point();
- centroid.x = moments.get_m10() / moments.get_m00();
- centroid.y = moments.get_m01() / moments.get_m00();
- return centroid;
- }
- }