List of usage examples for org.opencv.imgproc Imgproc cornerSubPix
public static void cornerSubPix(Mat image, Mat corners, Size winSize, Size zeroZone, TermCriteria criteria)
From source file:com.shootoff.camera.autocalibration.AutoCalibrationManager.java
License:Open Source License
public Optional<MatOfPoint2f> findChessboard(Mat mat) { //Mat grayImage = new Mat(); //Imgproc.cvtColor(mat, grayImage, Imgproc.COLOR_BGR2GRAY); Mat grayImage = mat;/*from ww w . j a va2 s . c o m*/ MatOfPoint2f imageCorners = new MatOfPoint2f(); boolean found = Calib3d.findChessboardCorners(grayImage, boardSize, imageCorners, Calib3d.CALIB_CB_NORMALIZE_IMAGE + Calib3d.CALIB_CB_ADAPTIVE_THRESH); logger.trace("found {}", found); if (found) { // optimization Imgproc.cornerSubPix(grayImage, imageCorners, new Size(1, 1), new Size(-1, -1), term); return Optional.of(imageCorners); } return Optional.empty(); }
From source file:org.usfirst.frc.team2084.CMonster2016.vision.CameraCalibration.java
License:Open Source License
/** * Draws checkerboard corners on an image. * // w w w . j a v a 2s . c o m * @param image the image to process * @param addToCalibration if true, add this image to the corner list */ public void process(Mat image, boolean addToCalibration) { boolean patternFound = Calib3d.findChessboardCorners(image, boardSize, boardCorners, Calib3d.CALIB_CB_ADAPTIVE_THRESH | Calib3d.CALIB_CB_NORMALIZE_IMAGE | Calib3d.CALIB_CB_FAST_CHECK); if (patternFound) { // Refine corner positions to be more accurate Imgproc.cvtColor(image, grayImage, Imgproc.COLOR_BGR2GRAY); Imgproc.cornerSubPix(grayImage, boardCorners, new Size(6, 6), new Size(-1, -1), new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 30, 0.1)); if (addToCalibration) { calibrationCorners.add(boardCorners); } } image.copyTo(boardImage); Calib3d.drawChessboardCorners(boardImage, boardSize, boardCorners, patternFound); if (!addToCalibration) { debugImage("Board", boardImage); } Imgproc.undistort(image, undistortImage, cameraMatrix, distCoeffs); undistortImage.copyTo(image); Imgproc.putText(image, "Error: " + error, new Point(20, 20), Core.FONT_HERSHEY_PLAIN, 1.5, new Scalar(0, 255, 0)); }
From source file:org.usfirst.frc.team2084.CMonster2016.vision.Target.java
License:Open Source License
/** * Creates a new possible target based on the specified blob and calculates * its score./*from w w w .j ava 2 s . co m*/ * * @param p the shape of the possible target */ public Target(MatOfPoint contour, Mat grayImage) { // Simplify contour to make the corner finding algorithm work better MatOfPoint2f fContour = new MatOfPoint2f(); contour.convertTo(fContour, CvType.CV_32F); Imgproc.approxPolyDP(fContour, fContour, VisionParameters.getGoalApproxPolyEpsilon(), true); fContour.convertTo(contour, CvType.CV_32S); this.contour = contour; // Check area, and don't do any calculations if it is not valid if (validArea = validateArea()) { // Find a bounding rectangle RotatedRect rect = Imgproc.minAreaRect(fContour); Point[] rectPoints = new Point[4]; rect.points(rectPoints); for (int j = 0; j < rectPoints.length; j++) { Point rectPoint = rectPoints[j]; double minDistance = Double.MAX_VALUE; Point point = null; for (int i = 0; i < contour.rows(); i++) { Point contourPoint = new Point(contour.get(i, 0)); double dist = distance(rectPoint, contourPoint); if (dist < minDistance) { minDistance = dist; point = contourPoint; } } rectPoints[j] = point; } MatOfPoint2f rectMat = new MatOfPoint2f(rectPoints); // Refine the corners to improve accuracy Imgproc.cornerSubPix(grayImage, rectMat, new Size(4, 10), new Size(-1, -1), new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 30, 0.1)); rectPoints = rectMat.toArray(); // Identify each corner SortedMap<Double, List<Point>> x = new TreeMap<>(); Arrays.stream(rectPoints).forEach((p) -> { List<Point> points; if ((points = x.get(p.x)) == null) { x.put(p.x, points = new LinkedList<>()); } points.add(p); }); int i = 0; for (Iterator<List<Point>> it = x.values().iterator(); it.hasNext();) { List<Point> s = it.next(); for (Point p : s) { switch (i) { case 0: topLeft = p; break; case 1: bottomLeft = p; break; case 2: topRight = p; break; case 3: bottomRight = p; } i++; } } // Organize corners if (topLeft.y > bottomLeft.y) { Point p = bottomLeft; bottomLeft = topLeft; topLeft = p; } if (topRight.y > bottomRight.y) { Point p = bottomRight; bottomRight = topRight; topRight = p; } // Create corners for centroid calculation corners = new MatOfPoint2f(rectPoints); // Calculate center Moments moments = Imgproc.moments(corners); center = new Point(moments.m10 / moments.m00, moments.m01 / moments.m00); // Put the points in the correct order for solvePNP rectPoints[0] = topLeft; rectPoints[1] = topRight; rectPoints[2] = bottomLeft; rectPoints[3] = bottomRight; // Recreate corners in the new order corners = new MatOfPoint2f(rectPoints); widthTop = distance(topLeft, topRight); widthBottom = distance(bottomLeft, bottomRight); width = (widthTop + widthBottom) / 2.0; heightLeft = distance(topLeft, bottomLeft); heightRight = distance(topRight, bottomRight); height = (heightLeft + heightRight) / 2.0; Mat tvec = new Mat(); // Calculate target's location Calib3d.solvePnP(OBJECT_POINTS, corners, CAMERA_MAT, DISTORTION_MAT, rotation, tvec, false, Calib3d.CV_P3P); // ======================================= // Position and Orientation Transformation // ======================================= double armAngle = VisionResults.getArmAngle(); // Flip y axis to point upward Core.multiply(tvec, SIGN_NORMALIZATION_MATRIX, tvec); // Shift origin to arm pivot point, on the robot's centerline CoordinateMath.translate(tvec, CAMERA_X_OFFSET, CAMERA_Y_OFFSET, ARM_LENGTH); // Align axes with ground CoordinateMath.rotateX(tvec, -armAngle); Core.add(rotation, new MatOfDouble(armAngle, 0, 0), rotation); // Shift origin to robot center of rotation CoordinateMath.translate(tvec, 0, ARM_PIVOT_Y_OFFSET, -ARM_PIVOT_Z_OFFSET); double xPosFeet = tvec.get(0, 0)[0]; double yPosFeet = tvec.get(1, 0)[0]; double zPosFeet = tvec.get(2, 0)[0]; // Old less effective aiming heading and distance calculation // double pixelsToFeet = TARGET_WIDTH / width; // distance = (TARGET_WIDTH * HighGoalProcessor.IMAGE_SIZE.width // / (2 * width ** Math.tan(VisionParameters.getFOVAngle() / 2))); // double xPosFeet = (center.x - (HighGoalProcessor.IMAGE_SIZE.width // / 2)) * pixelsToFeet; // double yPosFeet = -(center.y - // (HighGoalProcessor.IMAGE_SIZE.height / 2)) * pixelsToFeet; distance = Math.sqrt(xPosFeet * xPosFeet + zPosFeet * zPosFeet); position = new Point3(xPosFeet, yPosFeet, zPosFeet); xGoalAngle = Math.atan(xPosFeet / zPosFeet); yGoalAngle = Math.atan(yPosFeet / zPosFeet); validate(); score = calculateScore(); } else { valid = false; } }