PageRenderTime 39ms CodeModel.GetById 25ms RepoModel.GetById 0ms app.codeStats 1ms

/src/main/java/com/shootoff/camera/autocalibration/AutoCalibrationManager.java

https://gitlab.com/clyde/ShootOFF
Java | 1129 lines | 756 code | 270 blank | 103 comment | 106 complexity | 198ed0b199869932f6b79bd968153056 MD5 | raw file
  1. /*
  2. * ShootOFF - Software for Laser Dry Fire Training
  3. * Copyright (C) 2016 phrack
  4. *
  5. * This program is free software: you can redistribute it and/or modify
  6. * it under the terms of the GNU General Public License as published by
  7. * the Free Software Foundation, either version 3 of the License, or
  8. * (at your option) any later version.
  9. *
  10. * This program is distributed in the hope that it will be useful,
  11. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  13. * GNU General Public License for more details.
  14. *
  15. * You should have received a copy of the GNU General Public License
  16. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  17. */
  18. package com.shootoff.camera.autocalibration;
  19. import java.awt.image.BufferedImage;
  20. import java.io.File;
  21. import java.util.ArrayList;
  22. import java.util.List;
  23. import java.util.Optional;
  24. import javafx.geometry.BoundingBox;
  25. import javafx.geometry.Bounds;
  26. import javafx.geometry.Dimension2D;
  27. import javafx.util.Callback;
  28. import org.opencv.calib3d.Calib3d;
  29. import org.opencv.core.Core;
  30. import org.opencv.core.CvType;
  31. import org.opencv.core.Mat;
  32. import org.opencv.core.MatOfPoint;
  33. import org.opencv.core.MatOfPoint2f;
  34. import org.opencv.core.Point;
  35. import org.opencv.core.RotatedRect;
  36. import org.opencv.core.Scalar;
  37. import org.opencv.core.Size;
  38. import org.opencv.core.TermCriteria;
  39. import org.opencv.imgproc.Imgproc;
  40. import org.opencv.imgproc.Moments;
  41. import org.opencv.highgui.Highgui;
  42. import org.slf4j.Logger;
  43. import org.slf4j.LoggerFactory;
  44. //import ch.qos.logback.classic.Level;
  45. import com.shootoff.camera.Camera;
  46. import com.shootoff.camera.CameraManager;
  47. public class AutoCalibrationManager {
  48. private static final Logger logger = LoggerFactory.getLogger(AutoCalibrationManager.class);
  49. private static final int PATTERN_WIDTH = 9;
  50. private static final int PATTERN_HEIGHT = 6;
  51. private static final double PAPER_MARGIN_WIDTH = 1.048;
  52. private static final double PAPER_MARGIN_HEIGHT = 1.063;
  53. private static final Size boardSize = new Size(PATTERN_WIDTH, PATTERN_HEIGHT);
  54. private Callback<Void, Void> callback;
  55. private CameraManager cameraManager;
  56. private final boolean calculateFrameDelay;
  57. // Stores the transformation matrix
  58. private Mat perspMat = null;
  59. // Stores the bounding box we'll pass back to CameraManager
  60. private Bounds boundingBox = null;
  61. private RotatedRect boundsRect;
  62. private boolean warpInitialized = false;
  63. private boolean isCalibrated = false;
  64. // Edge is 11 pixels wide. Squares are 168 pixels wide.
  65. // 11/168 = 0.06547619047619047619047619047619
  66. // Maybe I should have made it divisible...
  67. private static final double BORDER_FACTOR = 0.065476;
  68. private long frameTimestampBeforeFrameChange = -1;
  69. private Bounds boundsResult = null;
  70. private long frameDelayResult = -1;
  71. private final TermCriteria term = new TermCriteria(TermCriteria.EPS | TermCriteria.MAX_ITER, 60, 0.0001);
  72. /* Paper Pattern */
  73. private Optional<Dimension2D> paperDimensions = Optional.empty();
  74. private int stepThreeAttempts = 0;
  75. private final static int STEP_THREE_MAX_ATTEMPTS = 2;
  76. public Optional<Dimension2D> getPaperDimensions() {
  77. return paperDimensions;
  78. }
  79. public void setPaperDimensions(Optional<Dimension2D> paperDimensions) {
  80. this.paperDimensions = paperDimensions;
  81. }
  82. public AutoCalibrationManager(final CameraManager cameraManager, final boolean calculateFrameDelay) {
  83. this.cameraManager = cameraManager;
  84. this.calculateFrameDelay = calculateFrameDelay;
  85. }
  86. public void setCallback(final Callback<Void, Void> callback) {
  87. this.callback = callback;
  88. }
  89. public Mat getPerspMat() {
  90. return perspMat;
  91. }
  92. public Bounds getBoundsResult() {
  93. if (boundsResult == null)
  94. logger.error("getBoundsResult called when boundsResult==null, isCalibrated {}", isCalibrated);
  95. return boundsResult;
  96. }
  97. public long getFrameDelayResult() {
  98. return frameDelayResult;
  99. }
  100. public void reset() {
  101. isCalibrated = false;
  102. warpInitialized = false;
  103. boundsResult = null;
  104. boundsRect = null;
  105. boundingBox = null;
  106. callback = null;
  107. perspMat = null;
  108. frameDelayResult = -1;
  109. frameTimestampBeforeFrameChange = -1;
  110. stepThreeAttempts = 0;
  111. paperDimensions = Optional.empty();
  112. }
  113. // Converts to BW mat from bufferedimage
  114. public void preProcessFrame(final BufferedImage frame, final Mat mat) {
  115. final Mat matTemp;
  116. synchronized (frame) {
  117. matTemp = Camera.bufferedImageToMat(frame);
  118. }
  119. Imgproc.cvtColor(matTemp, mat, Imgproc.COLOR_BGR2GRAY);
  120. Imgproc.equalizeHist(mat, mat);
  121. if (logger.isTraceEnabled()) {
  122. String filename = String.format("grayscale.png");
  123. File file = new File(filename);
  124. filename = file.toString();
  125. Highgui.imwrite(filename, mat);
  126. }
  127. }
  128. private boolean isStepOneCompleted() {
  129. return !(boundsResult == null);
  130. }
  131. private boolean isStepTwoCompleted() {
  132. // If frameDelayResult > -1 OR calculateFrameDelay is False
  133. // Then step two is complete
  134. return !(frameDelayResult == -1 && calculateFrameDelay);
  135. }
  136. private boolean inStepTwo() {
  137. // We're in step two if frameTimestampBeforeFrameChange > -1
  138. // AND step two is not complete
  139. return !(frameTimestampBeforeFrameChange == -1 || isStepTwoCompleted());
  140. }
  141. private boolean isStepThreeCompleted() {
  142. return (stepThreeAttempts == STEP_THREE_MAX_ATTEMPTS);
  143. }
  144. private boolean isFinished() {
  145. return (isStepOneCompleted() && isStepTwoCompleted() && isStepThreeCompleted());
  146. }
  147. // Step one: Find main pattern, paper pattern optional
  148. // Step two: Frame delay, if enabled
  149. // Step three: Paper pattern attempt IF not found in step one
  150. public void processFrame(final BufferedImage frame) {
  151. logger.trace("processFrame");
  152. if (isFinished()) {
  153. callback();
  154. return;
  155. }
  156. Mat mat = new Mat();
  157. preProcessFrame(frame, mat);
  158. if (!isStepOneCompleted()) {
  159. stepOne(mat);
  160. if (isFinished()) callback();
  161. logger.trace("stepOne {}", isStepOneCompleted());
  162. return;
  163. }
  164. if (isStepOneCompleted() && !isStepTwoCompleted()) {
  165. stepTwo(mat);
  166. if (isFinished()) callback();
  167. if (isStepTwoCompleted()) {
  168. cameraManager.setArenaBackground(null);
  169. }
  170. logger.trace("stepTwo {}", isStepTwoCompleted());
  171. return;
  172. }
  173. if (isStepOneCompleted() && isStepTwoCompleted() && !isStepThreeCompleted()) {
  174. stepThreeAttempts++;
  175. cameraManager.setArenaBackground(null);
  176. mat = undistortFrame(mat);
  177. List<MatOfPoint2f> listPatterns = findPatterns(mat, true);
  178. if (listPatterns.isEmpty()) return;
  179. findPaperPattern(mat, listPatterns, false);
  180. if (isFinished())
  181. {
  182. callback();
  183. stepThreeAttempts = STEP_THREE_MAX_ATTEMPTS;
  184. }
  185. logger.trace("stepThree {}", isStepThreeCompleted());
  186. return;
  187. }
  188. logger.trace("finished {}", isFinished());
  189. if (isFinished()) callback();
  190. }
  191. private void stepTwo(Mat mat) {
  192. if (!inStepTwo()) {
  193. logger.debug("Step two: Checking frame delay");
  194. checkForFrameChange(mat);
  195. frameTimestampBeforeFrameChange = cameraManager.getCurrentFrameTimestamp();
  196. cameraManager.setArenaBackground(null);
  197. } else {
  198. final Optional<Long> frameDelay = checkForFrameChange(mat);
  199. if (frameDelay.isPresent()) {
  200. frameDelayResult = frameDelay.get();
  201. logger.debug("Step Two: frameDelayResult {}", frameDelayResult);
  202. }
  203. }
  204. }
  205. private void callback() {
  206. if (callback != null) {
  207. callback.call(null);
  208. callback = null;
  209. }
  210. }
  211. private void stepOne(Mat mat) {
  212. List<MatOfPoint2f> listPatterns = findPatterns(mat, true);
  213. if (listPatterns.isEmpty()) return;
  214. findPaperPattern(mat, listPatterns, true);
  215. if (listPatterns.isEmpty()) return;
  216. // Technically there could still be more than one pattern
  217. // or even a pattern that is much too small
  218. // But damn if we're gonna fix every problem the user gives us
  219. Optional<Bounds> bounds = calibrateFrame(listPatterns.get(0), mat);
  220. if (bounds.isPresent()) {
  221. boundsResult = bounds.get();
  222. } else {
  223. boundsResult = null;
  224. }
  225. }
  226. private List<MatOfPoint2f> findPatterns(Mat mat, boolean findMultiple) {
  227. List<MatOfPoint2f> patternList = new ArrayList<MatOfPoint2f>();
  228. int count = 0;
  229. while (true) {
  230. Optional<MatOfPoint2f> boardCorners = findChessboard(mat);
  231. if (boardCorners.isPresent()) {
  232. patternList.add(boardCorners.get());
  233. if (!findMultiple) break;
  234. final RotatedRect rect = getPatternDimensions(boardCorners.get());
  235. blankRotatedRect(mat, rect);
  236. if (logger.isTraceEnabled()) {
  237. String filename = String.format("blanked-box-%d.png", count);
  238. File file = new File(filename);
  239. filename = file.toString();
  240. Highgui.imwrite(filename, mat);
  241. }
  242. // Shortcut to not try to find three+ patterns
  243. // We never should see more than two but maybe that'll change
  244. // in the future
  245. findMultiple = false;
  246. } else {
  247. break;
  248. }
  249. count++;
  250. }
  251. return patternList;
  252. }
  253. private Dimension2D averageDimensions(Dimension2D d2d1, Dimension2D d2d2) {
  254. return new Dimension2D((d2d1.getWidth() + d2d2.getWidth()) / 2, (d2d1.getHeight() + d2d2.getHeight()) / 2);
  255. }
  256. private double[] patternLuminosity = { -1, -1, -1 };
  257. private Optional<Long> checkForFrameChange(Mat mat) {
  258. mat = undistortFrame(mat);
  259. final double[] pixel = getFrameDelayPixel(mat);
  260. // Initialize
  261. if (patternLuminosity[0] == -1) {
  262. patternLuminosity = pixel;
  263. return Optional.empty();
  264. }
  265. final Mat tempMat = new Mat(1, 2, CvType.CV_8UC3);
  266. tempMat.put(0, 0, patternLuminosity);
  267. tempMat.put(0, 1, pixel);
  268. Imgproc.cvtColor(tempMat, tempMat, Imgproc.COLOR_BGR2HSV);
  269. final long change = cameraManager.getCurrentFrameTimestamp() - frameTimestampBeforeFrameChange;
  270. if (tempMat.get(0, 1)[2] < .9 * tempMat.get(0, 0)[2]) {
  271. return Optional.of(change);
  272. } else if (change > 250) {
  273. return Optional.of(-1L);
  274. }
  275. return Optional.empty();
  276. }
  277. private double[] getFrameDelayPixel(Mat mat) {
  278. final double squareHeight = boundsResult.getHeight() / (double) (PATTERN_HEIGHT + 1);
  279. final double squareWidth = boundsResult.getWidth() / (double) (PATTERN_WIDTH + 1);
  280. final int secondSquareCenterX = (int) (boundsResult.getMinX() + (squareWidth * 1.5));
  281. final int secondSquareCenterY = (int) (boundsResult.getMinY() + (squareHeight * .5));
  282. return mat.get(secondSquareCenterY, secondSquareCenterX);
  283. }
  284. public Optional<Bounds> calibrateFrame(MatOfPoint2f boardCorners, Mat mat) {
  285. // For debugging
  286. Mat traceMat = null;
  287. if (logger.isTraceEnabled()) {
  288. traceMat = mat.clone();
  289. }
  290. // Turn the chessboard into corners
  291. final MatOfPoint2f boardRect = calcBoardRectFromCorners(boardCorners);
  292. // Estimate the pattern corners
  293. MatOfPoint2f estimatedPatternRect = estimatePatternRect(traceMat, boardRect);
  294. // More definitively find corners using goodFeaturesToTrack
  295. final Optional<Point[]> corners = findCorners(boardRect, mat, estimatedPatternRect);
  296. if (!corners.isPresent()) return Optional.empty();
  297. // Creates sorted cornerArray for warp perspective
  298. MatOfPoint2f corners2f = sortPointsForWarpPerspective(boardRect, corners.get());
  299. if (logger.isTraceEnabled()) {
  300. String filename = String.format("calibrate-dist.png");
  301. final File file = new File(filename);
  302. filename = file.toString();
  303. Highgui.imwrite(filename, traceMat);
  304. }
  305. // Initialize the warp matrix and bounding box
  306. initializeWarpPerspective(mat, corners2f);
  307. if (boundingBox.getMinX() < 0 || boundingBox.getMinY() < 0
  308. || boundingBox.getWidth() > cameraManager.getFeedWidth()
  309. || boundingBox.getHeight() > cameraManager.getFeedHeight()) {
  310. return Optional.empty();
  311. }
  312. if (logger.isDebugEnabled()) logger.debug("bounds {} {} {} {}", boundingBox.getMinX(), boundingBox.getMinY(),
  313. boundingBox.getWidth(), boundingBox.getHeight());
  314. if (logger.isTraceEnabled()) {
  315. final Mat undistorted = warpPerspective(mat);
  316. String filename = String.format("calibrate-undist.png");
  317. File file = new File(filename);
  318. filename = file.toString();
  319. Highgui.imwrite(filename, undistorted);
  320. Mat undistortedCropped = undistorted.submat((int) boundingBox.getMinY(), (int) boundingBox.getMaxY(),
  321. (int) boundingBox.getMinX(), (int) boundingBox.getMaxX());
  322. filename = String.format("calibrate-undist-cropped.png");
  323. file = new File(filename);
  324. filename = file.toString();
  325. Highgui.imwrite(filename, undistortedCropped);
  326. }
  327. Mat warpedBoardCorners = warpCorners(boardCorners);
  328. isCalibrated = true;
  329. if (calculateFrameDelay) {
  330. final Mat undistorted = warpPerspective(mat);
  331. findColors(undistorted, warpedBoardCorners);
  332. final double squareHeight = boundingBox.getHeight() / (double) (PATTERN_HEIGHT + 1);
  333. final double squareWidth = boundingBox.getWidth() / (double) (PATTERN_WIDTH + 1);
  334. int secondSquareCenterX = (int) (boundingBox.getMinX() + (squareWidth * 1.5));
  335. int secondSquareCenterY = (int) (boundingBox.getMinY() + (squareHeight * .5));
  336. if (logger.isDebugEnabled()) logger.debug("pF getFrameDelayPixel x {} y {} p {}", secondSquareCenterX,
  337. secondSquareCenterY, undistorted.get(secondSquareCenterY, secondSquareCenterX));
  338. }
  339. return Optional.of(boundingBox);
  340. }
  341. private MatOfPoint2f sortPointsForWarpPerspective(final MatOfPoint2f boardRect, final Point[] corners) {
  342. Point[] cornerArray = new Point[4];
  343. Double[] cornerED = new Double[4];
  344. Point[] boardRectArray = boardRect.toArray();
  345. for (int i = 0; i < 4; i++)
  346. cornerED[i] = -1.0;
  347. for (Point cpt : corners) {
  348. for (int i = 0; i < 4; i++) {
  349. double tempED = euclideanDistance(cpt, boardRectArray[i]);
  350. if (cornerED[i] == -1.0 || tempED < cornerED[i]) {
  351. cornerArray[i] = cpt;
  352. cornerED[i] = tempED;
  353. }
  354. }
  355. }
  356. MatOfPoint2f corners2f = new MatOfPoint2f();
  357. corners2f.fromArray(cornerArray);
  358. return corners2f;
  359. }
  360. private Optional<Point[]> findCorners(MatOfPoint2f boardRect, Mat mat, MatOfPoint2f estimatedPatternRect) {
  361. Point[] cornerArray = new Point[4];
  362. Mat mask;
  363. final Point[] estimatedPoints = estimatedPatternRect.toArray();
  364. // Establishes a search region
  365. long region = mat.total() / 19200;
  366. Mat tempMat = null;
  367. if (logger.isTraceEnabled()) {
  368. tempMat = new Mat(mat.size(), CvType.CV_8UC3);
  369. Imgproc.cvtColor(mat, tempMat, Imgproc.COLOR_GRAY2BGR);
  370. }
  371. int i = 0;
  372. for (Point pt : estimatedPoints) {
  373. MatOfPoint tempCorners = new MatOfPoint();
  374. mask = Mat.zeros(mat.size(), CvType.CV_8UC1);
  375. Point leftpt = new Point(pt.x - region, pt.y - region);
  376. Point rightpt = new Point(pt.x + region, pt.y + region);
  377. Core.rectangle(mask, leftpt, rightpt, new Scalar(255), -1);
  378. if (logger.isTraceEnabled()) {
  379. String filename = String.format("mask-%d.png", i);
  380. File file = new File(filename);
  381. filename = file.toString();
  382. Highgui.imwrite(filename, mask);
  383. }
  384. Imgproc.goodFeaturesToTrack(mat, tempCorners, 2, .10, 0, mask, 3, true, .04);
  385. if (tempCorners.empty()) return Optional.empty();
  386. Point res = null;
  387. long dist = mat.total();
  388. for (Point p : tempCorners.toArray()) {
  389. long tempDist = (long) (Math.min(mat.width() - p.x, p.x) + Math.min(mat.height() - p.y, p.y));
  390. if (tempDist < dist) {
  391. dist = tempDist;
  392. res = p;
  393. }
  394. if (logger.isTraceEnabled()) {
  395. logger.trace("corner {} {}", p.x, p.y);
  396. Core.circle(tempMat, p, 1, new Scalar(0, 0, 255), -1);
  397. }
  398. }
  399. cornerArray[i] = res;
  400. i++;
  401. }
  402. if (logger.isTraceEnabled()) {
  403. String filename = String.format("corners.png");
  404. File file = new File(filename);
  405. filename = file.toString();
  406. Highgui.imwrite(filename, tempMat);
  407. }
  408. return Optional.of(cornerArray);
  409. }
  410. /**
  411. * Perspective pattern discovery
  412. *
  413. * Works similar to arena calibration but does not try to identify the
  414. * outline of the projection area We are only concerned with size, not
  415. * alignment or angle
  416. *
  417. */
  418. public boolean findPaperPattern(Mat mat, List<MatOfPoint2f> patternList, boolean averagePatterns) {
  419. MatOfPoint2f boardCorners = null;
  420. int index = 0;
  421. boolean found = false;
  422. for (; index < patternList.size(); index++) {
  423. boardCorners = patternList.get(index);
  424. RotatedRect rect = getPatternDimensions(boardCorners);
  425. // OpenCV gives us the checkerboard corners, not the outside
  426. // dimension
  427. // So this estimates where the outside corner would be, plus a fudge
  428. // factor for the edge of the paper
  429. // Printer margins are usually a quarter inch on each edge
  430. double rect_width = rect.size.width, rect_height = rect.size.height;
  431. double width = rect_width, height = rect_height;
  432. // Flip them if its sideways
  433. if (height > width) {
  434. width = rect_height;
  435. height = rect_width;
  436. rect_height = width;
  437. rect_width = height;
  438. }
  439. width = ((double) width * ((double) (PATTERN_WIDTH + 1) / (double) (PATTERN_WIDTH - 1)) * PAPER_MARGIN_WIDTH
  440. * 1 + (BORDER_FACTOR / PATTERN_WIDTH));
  441. height = ((double) height * ((double) (PATTERN_HEIGHT + 1) / (double) (PATTERN_HEIGHT - 1))
  442. * PAPER_MARGIN_HEIGHT * 1 + (BORDER_FACTOR / PATTERN_HEIGHT));
  443. final double PAPER_PATTERN_SIZE_THRESHOLD = .25;
  444. if (width > PAPER_PATTERN_SIZE_THRESHOLD * mat.cols()
  445. || height > PAPER_PATTERN_SIZE_THRESHOLD * mat.rows()) {
  446. continue;
  447. }
  448. if (logger.isTraceEnabled()) {
  449. logger.trace("pattern width {} height {}", rect_width, rect_height);
  450. logger.trace("paper width {} height {}", width, height);
  451. }
  452. final Dimension2D newPaperDimensions = new Dimension2D(width, height);
  453. found = true;
  454. if (!paperDimensions.isPresent()) {
  455. paperDimensions = Optional.of(newPaperDimensions);
  456. logger.debug("Found paper dimensions {}", paperDimensions.get());
  457. } else if (paperDimensions.isPresent() && averagePatterns) {
  458. paperDimensions = Optional.of(averageDimensions(paperDimensions.get(), newPaperDimensions));
  459. logger.trace("Averaged paper dimensions {}", paperDimensions.get());
  460. } else
  461. {
  462. paperDimensions = Optional.of(newPaperDimensions);
  463. logger.debug("Found paper dimensions {}", paperDimensions.get());
  464. }
  465. break;
  466. }
  467. if (found) {
  468. logger.trace("Removing paper pattern from patternList (index {})", index);
  469. patternList.remove(index);
  470. return true;
  471. }
  472. return false;
  473. }
  474. // What a stupid function, can't be the best way
  475. private void blankRotatedRect(Mat mat, final RotatedRect rect) {
  476. Mat tempMat = Mat.zeros(mat.size(), CvType.CV_8UC1);
  477. Point points[] = new Point[4];
  478. rect.points(points);
  479. for (int i = 0; i < 4; ++i) {
  480. Core.line(tempMat, points[i], points[(i + 1) % 4], new Scalar(255, 255, 255));
  481. }
  482. Mat tempMask = Mat.zeros((mat.rows() + 2), (mat.cols() + 2), CvType.CV_8UC1);
  483. Imgproc.floodFill(tempMat, tempMask, rect.center, new Scalar(255, 255, 255), null, new Scalar(0, 0, 0),
  484. new Scalar(254, 254, 254), 4);
  485. if (logger.isTraceEnabled()) {
  486. String filename = String.format("poly.png");
  487. File file = new File(filename);
  488. filename = file.toString();
  489. Highgui.imwrite(filename, tempMat);
  490. }
  491. mat.setTo(new Scalar(0, 0, 0), tempMat);
  492. }
  493. private RotatedRect getPatternDimensions(MatOfPoint2f boardCorners) {
  494. final MatOfPoint2f boardRect2f = calcBoardRectFromCorners(boardCorners);
  495. return Imgproc.minAreaRect(boardRect2f);
  496. }
  497. private void findColors(Mat frame, Mat warpedBoardCorners) {
  498. final Point rCenter = findChessBoardSquareCenter(warpedBoardCorners, 2, 3);
  499. final Point gCenter = findChessBoardSquareCenter(warpedBoardCorners, 2, 5);
  500. final Point bCenter = findChessBoardSquareCenter(warpedBoardCorners, 2, 7);
  501. if (logger.isTraceEnabled()) {
  502. logger.trace("findColors {} {} {}", rCenter, gCenter, bCenter);
  503. logger.trace("findColors r {} {} {} {}", (int) rCenter.y - 10, (int) rCenter.y + 10, (int) rCenter.x - 10,
  504. (int) rCenter.x + 10);
  505. }
  506. final Scalar rMeanColor = Core.mean(
  507. frame.submat((int) rCenter.y - 10, (int) rCenter.y + 10, (int) rCenter.x - 10, (int) rCenter.x + 10));
  508. final Scalar gMeanColor = Core.mean(
  509. frame.submat((int) gCenter.y - 10, (int) gCenter.y + 10, (int) gCenter.x - 10, (int) gCenter.x + 10));
  510. final Scalar bMeanColor = Core.mean(
  511. frame.submat((int) bCenter.y - 10, (int) bCenter.y + 10, (int) bCenter.x - 10, (int) bCenter.x + 10));
  512. if (logger.isTraceEnabled()) {
  513. String filename = String.format("rColor.png");
  514. File file = new File(filename);
  515. filename = file.toString();
  516. Highgui.imwrite(filename, frame.submat((int) rCenter.y - 10, (int) rCenter.y + 10, (int) rCenter.x - 10,
  517. (int) rCenter.x + 10));
  518. filename = String.format("gColor.png");
  519. file = new File(filename);
  520. filename = file.toString();
  521. Highgui.imwrite(filename, frame.submat((int) gCenter.y - 10, (int) gCenter.y + 10, (int) gCenter.x - 10,
  522. (int) gCenter.x + 10));
  523. filename = String.format("bColor.png");
  524. file = new File(filename);
  525. filename = file.toString();
  526. Highgui.imwrite(filename, frame.submat((int) bCenter.y - 10, (int) bCenter.y + 10, (int) bCenter.x - 10,
  527. (int) bCenter.x + 10));
  528. }
  529. if (logger.isTraceEnabled()) logger.trace("meanColor {} {} {}", rMeanColor, gMeanColor, bMeanColor);
  530. }
  531. public BufferedImage undistortFrame(BufferedImage frame) {
  532. if (isCalibrated) {
  533. final Mat mat = Camera.bufferedImageToMat(frame);
  534. frame = Camera.matToBufferedImage(warpPerspective(mat));
  535. } else {
  536. logger.warn("undistortFrame called when isCalibrated is false");
  537. }
  538. return frame;
  539. }
  540. // MUST BE IN BGR pixel format.
  541. public Mat undistortFrame(Mat mat) {
  542. if (!isCalibrated) {
  543. logger.warn("undistortFrame called when isCalibrated is false");
  544. return mat;
  545. }
  546. return warpPerspective(mat);
  547. }
  548. private MatOfPoint2f estimatePatternRect(Mat traceMat, MatOfPoint2f boardRect) {
  549. // We use this to calculate the angle
  550. final RotatedRect boardBox = Imgproc.minAreaRect(boardRect);
  551. final double boardBoxAngle = boardBox.size.height > boardBox.size.width ? 90.0 + boardBox.angle
  552. : boardBox.angle;
  553. // This is the board corners with the angle eliminated
  554. final Mat unRotMat = getRotationMatrix(massCenterMatOfPoint2f(boardRect), boardBoxAngle);
  555. final MatOfPoint2f unRotatedRect = rotateRect(unRotMat, boardRect);
  556. // This is the estimated projection area that has minimum angle (Not
  557. // rotated)
  558. final MatOfPoint2f estimatedPatternSizeRect = estimateFullPatternSize(unRotatedRect);
  559. // This is what we'll use as the transformation target and bounds given
  560. // back to the cameramanager
  561. boundsRect = Imgproc.minAreaRect(estimatedPatternSizeRect);
  562. // We now rotate the estimation back to the original angle to use for
  563. // transformation source
  564. final Mat rotMat = getRotationMatrix(massCenterMatOfPoint2f(estimatedPatternSizeRect), -boardBoxAngle);
  565. final MatOfPoint2f rotatedPatternSizeRect = rotateRect(rotMat, estimatedPatternSizeRect);
  566. if (logger.isTraceEnabled()) {
  567. logger.trace("center {} angle {} width {} height {}", boardBox.center, boardBoxAngle, boardBox.size.width,
  568. boardBox.size.height);
  569. logger.debug("boundsRect {} {} {} {}", boundsRect.boundingRect().x, boundsRect.boundingRect().y,
  570. boundsRect.boundingRect().x + boundsRect.boundingRect().width,
  571. boundsRect.boundingRect().y + boundsRect.boundingRect().height);
  572. Core.circle(traceMat, new Point(boardRect.get(0, 0)[0], boardRect.get(0, 0)[1]), 1, new Scalar(255, 0, 0),
  573. -1);
  574. Core.circle(traceMat, new Point(boardRect.get(1, 0)[0], boardRect.get(1, 0)[1]), 1, new Scalar(255, 0, 0),
  575. -1);
  576. Core.circle(traceMat, new Point(boardRect.get(2, 0)[0], boardRect.get(2, 0)[1]), 1, new Scalar(255, 0, 0),
  577. -1);
  578. Core.circle(traceMat, new Point(boardRect.get(3, 0)[0], boardRect.get(3, 0)[1]), 1, new Scalar(255, 0, 0),
  579. -1);
  580. Core.line(traceMat, new Point(unRotatedRect.get(0, 0)[0], unRotatedRect.get(0, 0)[1]),
  581. new Point(unRotatedRect.get(1, 0)[0], unRotatedRect.get(1, 0)[1]), new Scalar(0, 255, 0));
  582. Core.line(traceMat, new Point(unRotatedRect.get(1, 0)[0], unRotatedRect.get(1, 0)[1]),
  583. new Point(unRotatedRect.get(2, 0)[0], unRotatedRect.get(2, 0)[1]), new Scalar(0, 255, 0));
  584. Core.line(traceMat, new Point(unRotatedRect.get(3, 0)[0], unRotatedRect.get(3, 0)[1]),
  585. new Point(unRotatedRect.get(2, 0)[0], unRotatedRect.get(2, 0)[1]), new Scalar(0, 255, 0));
  586. Core.line(traceMat, new Point(unRotatedRect.get(3, 0)[0], unRotatedRect.get(3, 0)[1]),
  587. new Point(unRotatedRect.get(0, 0)[0], unRotatedRect.get(0, 0)[1]), new Scalar(0, 255, 0));
  588. Core.line(traceMat, new Point(estimatedPatternSizeRect.get(0, 0)[0], estimatedPatternSizeRect.get(0, 0)[1]),
  589. new Point(estimatedPatternSizeRect.get(1, 0)[0], estimatedPatternSizeRect.get(1, 0)[1]),
  590. new Scalar(255, 255, 0));
  591. Core.line(traceMat, new Point(estimatedPatternSizeRect.get(1, 0)[0], estimatedPatternSizeRect.get(1, 0)[1]),
  592. new Point(estimatedPatternSizeRect.get(2, 0)[0], estimatedPatternSizeRect.get(2, 0)[1]),
  593. new Scalar(255, 255, 0));
  594. Core.line(traceMat, new Point(estimatedPatternSizeRect.get(3, 0)[0], estimatedPatternSizeRect.get(3, 0)[1]),
  595. new Point(estimatedPatternSizeRect.get(2, 0)[0], estimatedPatternSizeRect.get(2, 0)[1]),
  596. new Scalar(255, 255, 0));
  597. Core.line(traceMat, new Point(estimatedPatternSizeRect.get(3, 0)[0], estimatedPatternSizeRect.get(3, 0)[1]),
  598. new Point(estimatedPatternSizeRect.get(0, 0)[0], estimatedPatternSizeRect.get(0, 0)[1]),
  599. new Scalar(255, 255, 0));
  600. Core.line(traceMat, new Point(rotatedPatternSizeRect.get(0, 0)[0], rotatedPatternSizeRect.get(0, 0)[1]),
  601. new Point(rotatedPatternSizeRect.get(1, 0)[0], rotatedPatternSizeRect.get(1, 0)[1]),
  602. new Scalar(255, 255, 0));
  603. Core.line(traceMat, new Point(rotatedPatternSizeRect.get(1, 0)[0], rotatedPatternSizeRect.get(1, 0)[1]),
  604. new Point(rotatedPatternSizeRect.get(2, 0)[0], rotatedPatternSizeRect.get(2, 0)[1]),
  605. new Scalar(255, 255, 0));
  606. Core.line(traceMat, new Point(rotatedPatternSizeRect.get(3, 0)[0], rotatedPatternSizeRect.get(3, 0)[1]),
  607. new Point(rotatedPatternSizeRect.get(2, 0)[0], rotatedPatternSizeRect.get(2, 0)[1]),
  608. new Scalar(255, 255, 0));
  609. Core.line(traceMat, new Point(rotatedPatternSizeRect.get(3, 0)[0], rotatedPatternSizeRect.get(3, 0)[1]),
  610. new Point(rotatedPatternSizeRect.get(0, 0)[0], rotatedPatternSizeRect.get(0, 0)[1]),
  611. new Scalar(255, 255, 0));
  612. }
  613. return rotatedPatternSizeRect;
  614. }
  615. /*
  616. * This function takes a rectangular region representing the chessboard
  617. * inner corners and estimates the corners of the full pattern image
  618. */
  619. private MatOfPoint2f estimateFullPatternSize(MatOfPoint2f rect) {
  620. // Result Mat
  621. final MatOfPoint2f result = new MatOfPoint2f();
  622. result.alloc(4);
  623. // Get the sources as points
  624. final Point topLeft = new Point(rect.get(0, 0)[0], rect.get(0, 0)[1]);
  625. final Point topRight = new Point(rect.get(1, 0)[0], rect.get(1, 0)[1]);
  626. final Point bottomRight = new Point(rect.get(2, 0)[0], rect.get(2, 0)[1]);
  627. final Point bottomLeft = new Point(rect.get(3, 0)[0], rect.get(3, 0)[1]);
  628. // We need the heights and widths to estimate the square sizes
  629. final double topWidth = Math.sqrt(Math.pow(topRight.x - topLeft.x, 2) + Math.pow(topRight.y - topLeft.y, 2));
  630. final double leftHeight = Math
  631. .sqrt(Math.pow(bottomLeft.x - topLeft.x, 2) + Math.pow(bottomLeft.y - topLeft.y, 2));
  632. final double bottomWidth = Math
  633. .sqrt(Math.pow(bottomRight.x - bottomLeft.x, 2) + Math.pow(bottomRight.y - bottomLeft.y, 2));
  634. final double rightHeight = Math
  635. .sqrt(Math.pow(bottomRight.x - topRight.x, 2) + Math.pow(bottomRight.y - topRight.y, 2));
  636. if (logger.isTraceEnabled()) {
  637. logger.trace("points {} {} {} {}", topLeft, topRight, bottomRight, bottomLeft);
  638. double angle = Math.atan((topRight.y - topLeft.y) / (topRight.x - topLeft.x)) * 180 / Math.PI;
  639. double angle2 = Math.atan((bottomRight.y - bottomLeft.y) / (bottomRight.x - bottomLeft.x)) * 180 / Math.PI;
  640. logger.trace("square size {} {} - angle {}", topWidth / (PATTERN_WIDTH - 1),
  641. leftHeight / (PATTERN_HEIGHT - 1), angle);
  642. logger.trace("square size {} {} - angle {}", bottomWidth / (PATTERN_WIDTH - 1),
  643. rightHeight / (PATTERN_HEIGHT - 1), angle2);
  644. }
  645. // Estimate the square widths, that is what we base the estimate of the
  646. // real corners on
  647. double squareTopWidth = (1 + BORDER_FACTOR) * (topWidth / (PATTERN_WIDTH - 1));
  648. double squareLeftHeight = (1 + BORDER_FACTOR) * (leftHeight / (PATTERN_HEIGHT - 1));
  649. double squareBottomWidth = (1 + BORDER_FACTOR) * (bottomWidth / (PATTERN_WIDTH - 1));
  650. double squareRightHeight = (1 + BORDER_FACTOR) * (rightHeight / (PATTERN_HEIGHT - 1));
  651. // The estimations
  652. double[] newTopLeft = { topLeft.x - squareTopWidth, topLeft.y - squareLeftHeight };
  653. double[] newBottomLeft = { bottomLeft.x - squareBottomWidth, bottomLeft.y + squareLeftHeight };
  654. double[] newTopRight = { topRight.x + squareTopWidth, topRight.y - squareRightHeight };
  655. double[] newBottomRight = { bottomRight.x + squareBottomWidth, bottomRight.y + squareRightHeight };
  656. // Populate the result
  657. result.put(0, 0, newTopLeft);
  658. result.put(1, 0, newTopRight);
  659. result.put(2, 0, newBottomRight);
  660. result.put(3, 0, newBottomLeft);
  661. return result;
  662. }
  663. // Given a rotation matrix and a quadrilateral, rotate the points
  664. private MatOfPoint2f rotateRect(Mat rotMat, MatOfPoint2f boardRect) {
  665. final MatOfPoint2f result = new MatOfPoint2f();
  666. result.alloc(4);
  667. for (int i = 0; i < 4; i++) {
  668. final Point rPoint = rotPoint(rotMat, new Point(boardRect.get(i, 0)[0], boardRect.get(i, 0)[1]));
  669. final double[] rPointD = new double[2];
  670. rPointD[0] = rPoint.x;
  671. rPointD[1] = rPoint.y;
  672. result.put(i, 0, rPointD);
  673. }
  674. return result;
  675. }
  676. private Mat getRotationMatrix(final Point center, final double rotationAngle) {
  677. return Imgproc.getRotationMatrix2D(center, rotationAngle, 1.0);
  678. }
  679. /*
  680. * The one time calculation of the transformations.
  681. *
  682. * After this is done, the transformation is just applied
  683. */
  684. private void initializeWarpPerspective(final Mat frame, final MatOfPoint2f sourceCorners) {
  685. final MatOfPoint2f destCorners = new MatOfPoint2f();
  686. destCorners.alloc(4);
  687. destCorners.put(0, 0, new double[] { boundsRect.boundingRect().x, boundsRect.boundingRect().y });
  688. destCorners.put(1, 0, new double[] { boundsRect.boundingRect().x + boundsRect.boundingRect().width,
  689. boundsRect.boundingRect().y });
  690. destCorners.put(3, 0, new double[] { boundsRect.boundingRect().x,
  691. boundsRect.boundingRect().y + boundsRect.boundingRect().height });
  692. destCorners.put(2, 0, new double[] { boundsRect.boundingRect().x + boundsRect.boundingRect().width,
  693. boundsRect.boundingRect().y + boundsRect.boundingRect().height });
  694. if (logger.isTraceEnabled()) {
  695. logger.trace("initializeWarpPerspective src corners {} {} {} {}", sourceCorners.get(0, 0),
  696. sourceCorners.get(1, 0), sourceCorners.get(2, 0), sourceCorners.get(3, 0));
  697. logger.trace("initializeWarpPerspective dest corners {} {} {} {}", destCorners.get(0, 0),
  698. destCorners.get(1, 0), destCorners.get(2, 0), destCorners.get(3, 0));
  699. }
  700. perspMat = Imgproc.getPerspectiveTransform(sourceCorners, destCorners);
  701. int width = boundsRect.boundingRect().width;
  702. int height = boundsRect.boundingRect().height;
  703. // Make them divisible by two for video recording purposes
  704. if ((width & 1) == 1) width++;
  705. if ((height & 1) == 1) height++;
  706. boundingBox = new BoundingBox(boundsRect.boundingRect().x, boundsRect.boundingRect().y, width, height);
  707. warpInitialized = true;
  708. if (logger.isTraceEnabled()) {
  709. Mat debugFrame = frame.clone();
  710. Core.circle(debugFrame, new Point(sourceCorners.get(0, 0)[0], sourceCorners.get(0, 0)[1]), 1,
  711. new Scalar(255, 0, 255), -1);
  712. Core.circle(debugFrame, new Point(sourceCorners.get(1, 0)[0], sourceCorners.get(1, 0)[1]), 1,
  713. new Scalar(255, 0, 255), -1);
  714. Core.circle(debugFrame, new Point(sourceCorners.get(2, 0)[0], sourceCorners.get(2, 0)[1]), 1,
  715. new Scalar(255, 0, 255), -1);
  716. Core.circle(debugFrame, new Point(sourceCorners.get(3, 0)[0], sourceCorners.get(3, 0)[1]), 1,
  717. new Scalar(255, 0, 255), -1);
  718. Core.circle(debugFrame, new Point(destCorners.get(0, 0)[0], destCorners.get(0, 0)[1]), 1,
  719. new Scalar(255, 0, 0), -1);
  720. Core.circle(debugFrame, new Point(destCorners.get(1, 0)[0], destCorners.get(1, 0)[1]), 1,
  721. new Scalar(255, 0, 0), -1);
  722. Core.circle(debugFrame, new Point(destCorners.get(2, 0)[0], destCorners.get(2, 0)[1]), 1,
  723. new Scalar(255, 0, 0), -1);
  724. Core.circle(debugFrame, new Point(destCorners.get(3, 0)[0], destCorners.get(3, 0)[1]), 1,
  725. new Scalar(255, 0, 0), -1);
  726. Core.line(debugFrame, new Point(boundingBox.getMinX(), boundingBox.getMinY()),
  727. new Point(boundingBox.getMaxX(), boundingBox.getMinY()), new Scalar(0, 255, 0));
  728. Core.line(debugFrame, new Point(boundingBox.getMinX(), boundingBox.getMinY()),
  729. new Point(boundingBox.getMinX(), boundingBox.getMaxY()), new Scalar(0, 255, 0));
  730. Core.line(debugFrame, new Point(boundingBox.getMaxX(), boundingBox.getMaxY()),
  731. new Point(boundingBox.getMaxX(), boundingBox.getMinY()), new Scalar(0, 255, 0));
  732. Core.line(debugFrame, new Point(boundingBox.getMaxX(), boundingBox.getMaxY()),
  733. new Point(boundingBox.getMinX(), boundingBox.getMaxY()), new Scalar(0, 255, 0));
  734. String filename = String.format("calibrate-transformation.png");
  735. File file = new File(filename);
  736. filename = file.toString();
  737. Highgui.imwrite(filename, debugFrame);
  738. }
  739. }
  740. // initializeWarpPerspective MUST BE CALLED first
  741. private Mat warpPerspective(final Mat frame) {
  742. if (warpInitialized) {
  743. final Mat mat = new Mat();
  744. Imgproc.warpPerspective(frame, mat, perspMat, frame.size(), Imgproc.INTER_LINEAR);
  745. return mat;
  746. } else {
  747. logger.warn("warpPerspective called when warpInitialized is false - {} {} - {}", perspMat, boundingBox,
  748. isCalibrated);
  749. return frame;
  750. }
  751. }
  752. // initializeWarpPerspective MUST BE CALLED first
  753. private Mat warpCorners(MatOfPoint2f imageCorners) {
  754. Mat mat = null;
  755. if (warpInitialized) {
  756. mat = new Mat();
  757. Core.transform(imageCorners, mat, perspMat);
  758. } else {
  759. logger.warn("warpCorners called when warpInitialized is false - {} {} - {}", perspMat, boundingBox,
  760. isCalibrated);
  761. }
  762. return mat;
  763. }
  764. public Optional<MatOfPoint2f> findChessboard(Mat mat) {
  765. MatOfPoint2f imageCorners = new MatOfPoint2f();
  766. boolean found = Calib3d.findChessboardCorners(mat, boardSize, imageCorners,
  767. Calib3d.CALIB_CB_ADAPTIVE_THRESH | Calib3d.CALIB_CB_NORMALIZE_IMAGE);
  768. logger.trace("found {}", found);
  769. if (found) {
  770. // optimization
  771. Imgproc.cornerSubPix(mat, imageCorners, new Size(1, 1), new Size(-1, -1), term);
  772. return Optional.of(imageCorners);
  773. }
  774. return Optional.empty();
  775. }
  776. // converts the chessboard corners into a quadrilateral
  777. private MatOfPoint2f calcBoardRectFromCorners(MatOfPoint2f corners) {
  778. MatOfPoint2f result = new MatOfPoint2f();
  779. result.alloc(4);
  780. Point topLeft = new Point(corners.get(0, 0)[0], corners.get(0, 0)[1]);
  781. Point topRight = new Point(corners.get(PATTERN_WIDTH - 1, 0)[0], corners.get(PATTERN_WIDTH - 1, 0)[1]);
  782. Point bottomRight = new Point(corners.get(PATTERN_WIDTH * PATTERN_HEIGHT - 1, 0)[0],
  783. corners.get(PATTERN_WIDTH * PATTERN_HEIGHT - 1, 0)[1]);
  784. Point bottomLeft = new Point(corners.get(PATTERN_WIDTH * (PATTERN_HEIGHT - 1), 0)[0],
  785. corners.get(PATTERN_WIDTH * (PATTERN_HEIGHT - 1), 0)[1]);
  786. Point[] unsorted = { topLeft, topRight, bottomLeft, bottomRight };
  787. Point[] sorted = sortCorners(unsorted);
  788. result.fromArray(sorted);
  789. // result.put(0, 0, topLeft.x, topLeft.y, topRight.x, topRight.y,
  790. // bottomRight.x, bottomRight.y, bottomLeft.x,
  791. // bottomLeft.y);
  792. return result;
  793. }
  794. // Given 4 corners, use the mass center to arrange the corners into correct
  795. // order
  796. // 1st-------2nd
  797. // | |
  798. // | |
  799. // | |
  800. // 4th-------3rd
  801. private Point[] sortCorners(Point[] corners) {
  802. Point[] result = new Point[4];
  803. Point center = new Point(0, 0);
  804. for (Point corner : corners) {
  805. center.x += corner.x;
  806. center.y += corner.y;
  807. }
  808. center.x *= (1.0 / corners.length);
  809. center.y *= (1.0 / corners.length);
  810. List<Point> top = new ArrayList<Point>();
  811. List<Point> bot = new ArrayList<Point>();
  812. for (int i = 0; i < corners.length; i++) {
  813. if (corners[i].y < center.y)
  814. top.add(corners[i]);
  815. else
  816. bot.add(corners[i]);
  817. }
  818. result[0] = top.get(0).x > top.get(1).x ? top.get(1) : top.get(0);
  819. result[1] = top.get(0).x > top.get(1).x ? top.get(0) : top.get(1);
  820. result[2] = bot.get(0).x > bot.get(1).x ? bot.get(0) : bot.get(1);
  821. result[3] = bot.get(0).x > bot.get(1).x ? bot.get(1) : bot.get(0);
  822. return result;
  823. }
  824. private Point findChessBoardSquareCenter(Mat corners, int row, int col) {
  825. if (row >= PATTERN_HEIGHT - 1 || col >= PATTERN_WIDTH - 1) {
  826. logger.warn("findChessBoardSquareColor invalid row or col {} {}", row, col);
  827. return null;
  828. }
  829. final Point topLeft = new Point(corners.get((row * PATTERN_WIDTH - 1) + col, 0)[0],
  830. corners.get((row * PATTERN_WIDTH - 1) + col, 0)[1]);
  831. final Point bottomRight = new Point(corners.get(((row + 1) * PATTERN_WIDTH - 1) + col + 1, 0)[0],
  832. corners.get(((row + 1) * PATTERN_WIDTH - 1) + col + 1, 0)[1]);
  833. final Point result = new Point((topLeft.x + bottomRight.x) / 2, (topLeft.y + bottomRight.y) / 2);
  834. if (logger.isTraceEnabled()) {
  835. logger.trace("findChessBoardSquareColor {}", corners.size());
  836. logger.trace("findChessBoardSquareColor {} {}", (row * PATTERN_WIDTH - 1) + col,
  837. ((row + 1) * PATTERN_WIDTH - 1) + col + 1);
  838. logger.trace("findChessBoardSquareColor {} {} {}", topLeft, bottomRight, result);
  839. }
  840. return result;
  841. }
  842. private double euclideanDistance(final Point pt1, final Point pt2) {
  843. return Math.sqrt(Math.pow(pt1.x - pt2.x, 2) + Math.pow(pt1.y - pt2.y, 2));
  844. }
  845. // Given a rotation matrix, rotates a point
  846. private Point rotPoint(final Mat rot_mat, final Point point) {
  847. final Point rp = new Point();
  848. rp.x = rot_mat.get(0, 0)[0] * point.x + rot_mat.get(0, 1)[0] * point.y + rot_mat.get(0, 2)[0];
  849. rp.y = rot_mat.get(1, 0)[0] * point.x + rot_mat.get(1, 1)[0] * point.y + rot_mat.get(1, 2)[0];
  850. return rp;
  851. }
  852. private Point massCenterMatOfPoint2f(final MatOfPoint2f map) {
  853. final Moments moments = Imgproc.moments(map);
  854. final Point centroid = new Point();
  855. centroid.x = moments.get_m10() / moments.get_m00();
  856. centroid.y = moments.get_m01() / moments.get_m00();
  857. return centroid;
  858. }
  859. }