List of usage examples for org.opencv.core Point Point
public Point(double x, double y)
From source file:classes.ObjectFinder.java
private void drawOutput(Mat output) { Point center;/*from w w w .j a v a 2 s. c o m*/ int fontFace = Core.FONT_HERSHEY_SIMPLEX; double fontScale = 0.7; int thickness = 1; Core.ellipse(output, trackBox, new Scalar(0, 0, 255), 2); center = new Point(trackBox.center.x, trackBox.center.y); System.out.println("center: " + center); Core.circle(output, center, 2, new Scalar(255, 0, 0), 2); Core.circle(output, massCenter, 2, new Scalar(0, 255, 0), 2); if (this.objectName != null) { Core.putText(output, getObjectName(), center, fontFace, fontScale, new Scalar(0, 0, 0), thickness, 8, false); } }
From source file:classes.TextExtractor.java
private static Point transformPoint(Point point, Mat transformationMatrix) { double newX = transformationMatrix.get(0, 0)[0] * point.x + transformationMatrix.get(0, 1)[0] * point.y + transformationMatrix.get(0, 2)[0]; double newY = transformationMatrix.get(1, 0)[0] * point.x + transformationMatrix.get(1, 1)[0] * point.y + transformationMatrix.get(1, 2)[0]; return new Point(newX, newY); }
From source file:classes.TextExtractor.java
public void extractText(Rect roi, double roiAngle) throws Exception { Point roiTopLeft = roi.tl();//from w w w . j a v a 2 s .co m double radians = Math.toRadians(roiAngle); double sin = Math.abs(Math.sin(radians)); double cos = Math.abs(Math.cos(radians)); int newWidth = (int) (image.width() * cos + image.height() * sin); int newHeight = (int) (image.width() * sin + image.height() * cos); int[] newWidthHeight = { newWidth, newHeight }; int pivotX = newWidthHeight[0] / 2; int pivotY = newWidthHeight[1] / 2; Point center = new Point(pivotX, pivotY); Size targetSize = new Size(newWidthHeight[0], newWidthHeight[1]); Mat intermediateImage = new Mat(targetSize, image.type()); int offsetX = (newWidthHeight[0] - image.width()) / 2; int offsetY = (newWidthHeight[1] - image.height()) / 2; Point paddedTopLeft = new Point(roiTopLeft.x + offsetX, roiTopLeft.y + offsetY); Mat containerImage = intermediateImage.submat(offsetY, offsetY + image.height(), offsetX, offsetX + image.width()); image.copyTo(containerImage); Mat rotationMatrix = Imgproc.getRotationMatrix2D(center, roiAngle, 1.0); Point transformedTopLeft = transformPoint(paddedTopLeft, rotationMatrix); Mat rotatedImage = new Mat(); Imgproc.warpAffine(intermediateImage, rotatedImage, rotationMatrix, targetSize, Imgproc.INTER_LINEAR, Imgproc.BORDER_CONSTANT, new Scalar(0)); ImageUtils.saveImage(rotatedImage, imageID + "_rotatedImage.png", request); double adjustedWidth = roi.size().width; double adjustedHeight = roi.size().height; if (transformedTopLeft.x + adjustedWidth > rotatedImage.width()) { adjustedWidth = rotatedImage.width() - transformedTopLeft.x; } if (transformedTopLeft.y + adjustedHeight > rotatedImage.height()) { adjustedHeight = rotatedImage.height() - transformedTopLeft.y; } Rect newROI = new Rect(transformedTopLeft, new Size(adjustedWidth, adjustedHeight)); Mat extractedROI = new Mat(rotatedImage, newROI); String fileName = ImageUtils.saveImage(extractedROI, imageID + "_ROI.png", request); extractText(fileName); }
From source file:classes.TextRecognitionPreparer.java
public static ArrayList<String> generateRecognizableImagesNames(Mat img, Scalar userPickedColor, String imageID, HttpServletRequest request) {//from w ww .jav a 2 s.c om ArrayList<String> imageNames = new ArrayList<String>(); Mat filledImage = img.clone(); Scalar newVal = new Scalar(userPickedColor.val[2], userPickedColor.val[1], userPickedColor.val[0]); Imgproc.floodFill(filledImage, new Mat(), new Point(0, 0), newVal); String file1 = imageID + "_filledImage.png"; // Highgui.imwrite(file1, filledImage); imageNames.add(ImageUtils.saveImage(filledImage, file1, request)); Mat filledGrayImage = new Mat(); Imgproc.cvtColor(filledImage, filledGrayImage, Imgproc.COLOR_BGR2GRAY); String file2 = imageID + "_filledGrayImage.png"; // Highgui.imwrite(file2, filledGrayImage); imageNames.add(ImageUtils.saveImage(filledGrayImage, file2, request)); Mat gaussianGrayImage = new Mat(); Imgproc.GaussianBlur(filledGrayImage, gaussianGrayImage, new Size(0, 0), 3); Core.addWeighted(filledGrayImage, 3.5, gaussianGrayImage, -1, 0, gaussianGrayImage); String file3 = imageID + "_sharpenedImage.png"; // Highgui.imwrite(file3, gaussianGrayImage); imageNames.add(ImageUtils.saveImage(gaussianGrayImage, file3, request)); // Mat filledBinarizedImage2 = new Mat(); // Imgproc.adaptiveThreshold(filledGrayImage, filledBinarizedImage2, 255, Imgproc.ADAPTIVE_THRESH_MEAN_C, Imgproc.THRESH_BINARY, 75, 10); // String file5 = imageID + "_filledBinarizedImage2.png"; //// Highgui.imwrite(file11, filledBinarizedImage2); // imageNames.add(ImageUtils.saveImage(filledBinarizedImage2, file5)); // // Mat filledBinarizedImage1 = new Mat(); // Imgproc.adaptiveThreshold(filledGrayImage, filledBinarizedImage1, 255, Imgproc.ADAPTIVE_THRESH_MEAN_C, Imgproc.THRESH_BINARY, 15, 4); // String file4 = imageID + "_filledBinarizedImage1.png"; //// Highgui.imwrite(file4, filledBinarizedImage1); // imageNames.add(ImageUtils.saveImage(filledBinarizedImage1, file4)); return imageNames; }
From source file:classes.TextRecognitionPreparer.java
public static ArrayList<BufferedImage> generateRecognizableBufferedImages(Mat img, Scalar backgroundColor, Scalar userPickedColor) {/*from w ww. j a va 2s. com*/ ArrayList<BufferedImage> images = new ArrayList<BufferedImage>(); Mat filledImage = img.clone(); Scalar newVal = new Scalar(userPickedColor.val[2], userPickedColor.val[1], userPickedColor.val[0]); Imgproc.floodFill(filledImage, new Mat(), new Point(0, 0), newVal); images.add(Util.mat2Img(filledImage)); Mat filledGrayImage = new Mat(); Imgproc.cvtColor(filledImage, filledGrayImage, Imgproc.COLOR_BGR2GRAY); images.add(Util.mat2Img(filledGrayImage)); Mat gaussianGrayImage = new Mat(); Imgproc.GaussianBlur(filledGrayImage, gaussianGrayImage, new Size(0, 0), 3); Core.addWeighted(filledGrayImage, 3.5, gaussianGrayImage, -1, 0, gaussianGrayImage); images.add(Util.mat2Img(gaussianGrayImage)); Mat filledBinarizedImage2 = new Mat(); Imgproc.adaptiveThreshold(filledGrayImage, filledBinarizedImage2, 255, Imgproc.ADAPTIVE_THRESH_MEAN_C, Imgproc.THRESH_BINARY, 75, 10); images.add(Util.mat2Img(filledBinarizedImage2)); Mat filledBinarizedImage1 = new Mat(); Imgproc.adaptiveThreshold(filledGrayImage, filledBinarizedImage1, 255, Imgproc.ADAPTIVE_THRESH_MEAN_C, Imgproc.THRESH_BINARY, 15, 4); images.add(Util.mat2Img(filledBinarizedImage1)); return images; }
From source file:classes.TextRecognitionPreparer.java
public static ArrayList<Mat> generateRecognizableImages(Mat img, Scalar backgroundColor, Scalar userPickedColor) {//from w w w. jav a2s . co m ArrayList<Mat> images = new ArrayList<Mat>(); Mat filledImage = img.clone(); Scalar newVal = new Scalar(userPickedColor.val[2], userPickedColor.val[1], userPickedColor.val[0]); Imgproc.floodFill(filledImage, new Mat(), new Point(0, 0), newVal); String file1 = "filledImage.png"; // Highgui.imwrite(file1, filledImage); images.add(filledImage); Mat filledGrayImage = new Mat(); Imgproc.cvtColor(filledImage, filledGrayImage, Imgproc.COLOR_BGR2GRAY); String file2 = "filledGrayImage.png"; // Highgui.imwrite(file2, filledGrayImage); images.add(filledGrayImage); Mat gaussianGrayImage = new Mat(); Imgproc.GaussianBlur(filledGrayImage, gaussianGrayImage, new Size(0, 0), 3); Core.addWeighted(filledGrayImage, 3.5, gaussianGrayImage, -1, 0, gaussianGrayImage); // Core.addWeighted(filledGrayImage, 2.5, gaussianGrayImage, -0.5, 0, gaussianGrayImage); String file3 = "sharpenedImage.png"; // Highgui.imwrite(file3, gaussianGrayImage); images.add(gaussianGrayImage); Mat filledBinarizedImage = new Mat(); Imgproc.adaptiveThreshold(filledGrayImage, filledBinarizedImage, 255, Imgproc.ADAPTIVE_THRESH_MEAN_C, Imgproc.THRESH_BINARY, 15, 4); String file4 = "filledBinarizedImage.png"; // Highgui.imwrite(file4, filledBinarizedImage); images.add(filledBinarizedImage); // BackgroundSubtractorMOG2 backgroundSubtractorMOG2 = new BackgroundSubtractorMOG2(); // Mat foregroundMask = new Mat(); // backgroundSubtractorMOG2.apply(img, foregroundMask); // Highgui.imwrite("mFGMask.png", foregroundMask); Scalar fillingColor = cluster(userPickedColor, img, 3); Mat replacedColor = replaceColor(img, backgroundColor, fillingColor); String file5 = "replacedColor.png"; // Highgui.imwrite(file5, replacedColor); images.add(replacedColor); Mat grayImage = new Mat(); Imgproc.cvtColor(replacedColor, grayImage, Imgproc.COLOR_BGR2GRAY); String file6 = "grayImage.png"; // Highgui.imwrite(file6, grayImage); images.add(grayImage); Mat binarized = new Mat(); Imgproc.adaptiveThreshold(grayImage, binarized, 255, Imgproc.ADAPTIVE_THRESH_MEAN_C, Imgproc.THRESH_BINARY, 15, 4); String file7 = "binarized.png"; // Highgui.imwrite(file7, binarized); images.add(binarized); Mat colorReplacedEqualized = equalizeIntensity(replacedColor); String file8 = "colorReplacedEqualized.png"; // Highgui.imwrite(file8, colorReplacedEqualized); images.add(colorReplacedEqualized); Mat colorReducedImage = reduceColor(replacedColor, 64); String file9 = "replacedColorColorReduced.png"; // Highgui.imwrite(file9, colorReducedImage); images.add(colorReducedImage); // Equalizing image Mat colorReducedEqualized = equalizeIntensity(colorReducedImage); String file10 = "colorReducedEqualized.png"; // Highgui.imwrite(file10, colorReducedEqualized); images.add(colorReducedEqualized); return images; }
From source file:club.towr5291.Concepts.ConceptVuforiaOpGrabImage.java
@Override public void runOpMode() throws InterruptedException { //load variables sharedPreferences = PreferenceManager.getDefaultSharedPreferences(hardwareMap.appContext); teamNumber = sharedPreferences.getString("club.towr5291.Autonomous.TeamNumber", "0000"); allianceColor = sharedPreferences.getString("club.towr5291.Autonomous.Color", "Red"); allianceStartPosition = sharedPreferences.getString("club.towr5291.Autonomous.StartPosition", "Left"); allianceParkPosition = sharedPreferences.getString("club.towr5291.Autonomous.ParkPosition", "Vortex"); delay = Integer.parseInt(sharedPreferences.getString("club.towr5291.Autonomous.Delay", "0")); numBeacons = sharedPreferences.getString("club.towr5291.Autonomous.Beacons", "One"); robotConfig = sharedPreferences.getString("club.towr5291.Autonomous.RobotConfig", "TileRunner-2x40"); debug = Integer.parseInt(sharedPreferences.getString("club.towr5291.Autonomous.Debug", null)); debug = 3;/*from ww w . jav a 2 s . c o m*/ //start the log startDate = new SimpleDateFormat("yyyy/MM/dd HH:mm:ss").format(new Date()); fileLogger = new FileLogger(runtime); fileLogger.open(); fileLogger.write("Time,SysMS,Thread,Event,Desc"); fileLogger.writeEvent("init()", "Log Started"); //BeaconAnalysisOCV beaconColour = new BeaconAnalysisOCV(); //BeaconAnalysisOCV2 beaconColour = new BeaconAnalysisOCV2(); //BeaconAnalysisOCVAnalyse beaconColour = new BeaconAnalysisOCVAnalyse(); BeaconAnalysisOCVPlayground beaconColour = new BeaconAnalysisOCVPlayground(); VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(R.id.cameraMonitorViewId); parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK; parameters.vuforiaLicenseKey = "AVATY7T/////AAAAGQJxfNYzLUgGjSx0aOEU0Q0rpcfZO2h2sY1MhUZUr+Bu6RgoUMUP/nERGmD87ybv1/lM2LBFDxcBGRHkXvxtkHel4XEUCsNHFTGWYcVkMIZqctQsIrTe13MnUvSOfQj8ig7xw3iULcwDpY+xAftW61dKTJ0IAOCxx2F0QjJWqRJBxrEUR/DfQi4LyrgnciNMXCiZ8KFyBdC63XMYkQj2joTN579+2u5f8aSCe8jkAFnBLcB1slyaU9lhnlTEMcFjwrLBcWoYIFAZluvFT0LpqZRlS1/XYf45QBSJztFKHIsj1rbCgotAE36novnAQBs74ewnWsJifokJGOYWdFJveWzn3GE9OEH23Y5l7kFDu4wc"; //parameters.cameraMonitorFeedback = VuforiaLocalizer.Parameters.CameraMonitorFeedback.AXES; VuforiaLocalizer vuforia = ClassFactory.createVuforiaLocalizer(parameters); Vuforia.setFrameFormat(PIXEL_FORMAT.RGB565, true); //enables RGB565 format for the image vuforia.setFrameQueueCapacity(1); //tells VuforiaLocalizer to only store one frame at a time //ConceptVuforiaGrabImage vuforia = new ConceptVuforiaGrabImage(parameters); Vuforia.setHint(HINT.HINT_MAX_SIMULTANEOUS_IMAGE_TARGETS, 4); VuforiaTrackables velocityVortex = vuforia.loadTrackablesFromAsset("FTC_2016-17"); VuforiaTrackable wheels = velocityVortex.get(0); wheels.setName("wheels"); // wheels target VuforiaTrackable tools = velocityVortex.get(1); tools.setName("tools"); // tools target VuforiaTrackable legos = velocityVortex.get(2); legos.setName("legos"); // legos target VuforiaTrackable gears = velocityVortex.get(3); gears.setName("gears"); // gears target /** For convenience, gather together all the trackable objects in one easily-iterable collection */ List<VuforiaTrackable> allTrackables = new ArrayList<VuforiaTrackable>(); allTrackables.addAll(velocityVortex); Vuforia.setFrameFormat(PIXEL_FORMAT.RGB565, true); /** * We use units of mm here because that's the recommended units of measurement for the * size values specified in the XML for the ImageTarget trackables in data sets. E.g.: * <ImageTarget name="stones" size="247 173"/> * You don't *have to* use mm here, but the units here and the units used in the XML * target configuration files *must* correspond for the math to work out correctly. */ float mmPerInch = 25.4f; float mmBotWidth = 18 * mmPerInch; // ... or whatever is right for your robot float mmFTCFieldWidth = (12 * 12 - 2) * mmPerInch; // the FTC field is ~11'10" center-to-center of the glass panels /** * In order for localization to work, we need to tell the system where each target we * wish to use for navigation resides on the field, and we need to specify where on the robot * the phone resides. These specifications are in the form of <em>transformation matrices.</em> * Transformation matrices are a central, important concept in the math here involved in localization. * See <a href="https://en.wikipedia.org/wiki/Transformation_matrix">Transformation Matrix</a> * for detailed information. Commonly, you'll encounter transformation matrices as instances * of the {@link OpenGLMatrix} class. * * For the most part, you don't need to understand the details of the math of how transformation * matrices work inside (as fascinating as that is, truly). Just remember these key points: * <ol> * * <li>You can put two transformations together to produce a third that combines the effect of * both of them. If, for example, you have a rotation transform R and a translation transform T, * then the combined transformation matrix RT which does the rotation first and then the translation * is given by {@code RT = T.multiplied(R)}. That is, the transforms are multiplied in the * <em>reverse</em> of the chronological order in which they applied.</li> * * <li>A common way to create useful transforms is to use methods in the {@link OpenGLMatrix} * class and the Orientation class. See, for example, {@link OpenGLMatrix#translation(float, * float, float)}, {@link OpenGLMatrix#rotation(AngleUnit, float, float, float, float)}, and * {@link Orientation#getRotationMatrix(AxesReference, AxesOrder, AngleUnit, float, float, float)}. * Related methods in {@link OpenGLMatrix}, such as {@link OpenGLMatrix#rotated(AngleUnit, * float, float, float, float)}, are syntactic shorthands for creating a new transform and * then immediately multiplying the receiver by it, which can be convenient at times.</li> * * <li>If you want to break open the black box of a transformation matrix to understand * what it's doing inside, use {@link MatrixF#getTranslation()} to fetch how much the * transform will move you in x, y, and z, and use {@link Orientation#getOrientation(MatrixF, * AxesReference, AxesOrder, AngleUnit)} to determine the rotational motion that the transform * will impart. See {@link #format(OpenGLMatrix)} below for an example.</li> * * </ol> * * This example places the "stones" image on the perimeter wall to the Left * of the Red Driver station wall. Similar to the Red Beacon Location on the Res-Q * * This example places the "chips" image on the perimeter wall to the Right * of the Blue Driver station. Similar to the Blue Beacon Location on the Res-Q * * See the doc folder of this project for a description of the field Axis conventions. * * Initially the target is conceptually lying at the origin of the field's coordinate system * (the center of the field), facing up. * * In this configuration, the target's coordinate system aligns with that of the field. * * In a real situation we'd also account for the vertical (Z) offset of the target, * but for simplicity, we ignore that here; for a real robot, you'll want to fix that. * * To place the Wheels Target on the Red Audience wall: * - First we rotate it 90 around the field's X axis to flip it upright * - Then we rotate it 90 around the field's Z access to face it away from the audience. * - Finally, we translate it back along the X axis towards the red audience wall. * */ // RED Targets // To Place GEARS Target // position is approximately - (-6feet, -1feet) OpenGLMatrix gearsTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the RED WALL. Our translation here is a negative translation in X.*/ .translation(-mmFTCFieldWidth / 2, -1 * 12 * mmPerInch, 0).multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 90, 0)); gears.setLocation(gearsTargetLocationOnField); //RobotLog.ii(TAG, "Gears Target=%s", format(gearsTargetLocationOnField)); // To Place GEARS Target // position is approximately - (-6feet, 3feet) OpenGLMatrix toolsTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the Blue Audience wall. Our translation here is a positive translation in Y.*/ .translation(-mmFTCFieldWidth / 2, 3 * 12 * mmPerInch, 0) //.translation(0, mmFTCFieldWidth/2, 0) .multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 90, 0)); tools.setLocation(toolsTargetLocationOnField); //RobotLog.ii(TAG, "Tools Target=%s", format(toolsTargetLocationOnField)); //Finsih RED Targets // BLUE Targets // To Place LEGOS Target // position is approximately - (-3feet, 6feet) OpenGLMatrix legosTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the RED WALL. Our translation here is a negative translation in X.*/ .translation(-3 * 12 * mmPerInch, mmFTCFieldWidth / 2, 0).multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 0, 0)); legos.setLocation(legosTargetLocationOnField); //RobotLog.ii(TAG, "Gears Target=%s", format(legosTargetLocationOnField)); // To Place WHEELS Target // position is approximately - (1feet, 6feet) OpenGLMatrix wheelsTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the Blue Audience wall. Our translation here is a positive translation in Y.*/ .translation(1 * 12 * mmPerInch, mmFTCFieldWidth / 2, 0).multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 0, 0)); wheels.setLocation(wheelsTargetLocationOnField); //RobotLog.ii(TAG, "Tools Target=%s", format(wheelsTargetLocationOnField)); //Finsih BLUE Targets /** * Create a transformation matrix describing where the phone is on the robot. Here, we * put the phone on the right hand side of the robot with the screen facing in (see our * choice of BACK camera above) and in landscape mode. Starting from alignment between the * robot's and phone's axes, this is a rotation of -90deg along the Y axis. * * When determining whether a rotation is positive or negative, consider yourself as looking * down the (positive) axis of rotation from the positive towards the origin. Positive rotations * are then CCW, and negative rotations CW. An example: consider looking down the positive Z * axis towards the origin. A positive rotation about Z (ie: a rotation parallel to the the X-Y * plane) is then CCW, as one would normally expect from the usual classic 2D geometry. */ OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix.translation((mmBotWidth / 2), 50, 0).multiplied(Orientation .getRotationMatrix(AxesReference.EXTRINSIC, AxesOrder.YZY, AngleUnit.DEGREES, -90, 0, 0)); //RobotLog.ii(TAG, "phone=%s", format(phoneLocationOnRobot)); /** * Let the trackable listeners we care about know where the phone is. We know that each * listener is a {@link VuforiaTrackableDefaultListener} and can so safely cast because * we have not ourselves installed a listener of a different type. */ ((VuforiaTrackableDefaultListener) gears.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); ((VuforiaTrackableDefaultListener) tools.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); ((VuforiaTrackableDefaultListener) legos.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); ((VuforiaTrackableDefaultListener) wheels.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); /** * A brief tutorial: here's how all the math is going to work: * * C = phoneLocationOnRobot maps phone coords -> robot coords * P = tracker.getPose() maps image target coords -> phone coords * L = redTargetLocationOnField maps image target coords -> field coords * * So * * C.inverted() maps robot coords -> phone coords * P.inverted() maps phone coords -> imageTarget coords * * Putting that all together, * * L x P.inverted() x C.inverted() maps robot coords to field coords. * * @see VuforiaTrackableDefaultListener#getRobotLocation() */ waitForStart(); //Mat tmp = new Mat(); velocityVortex.activate(); Image rgb = null; int loop = 0; Constants.BeaconColours Colour = Constants.BeaconColours.UNKNOWN; while (opModeIsActive()) { boolean gotBeacomDims = false; Point beaconBotRight = new Point(0, 0); Point beaconTopLeft = new Point(0, 0); Point beaconMiddle = new Point(0, 0); for (VuforiaTrackable beac : velocityVortex) { OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) beac.getListener()).getRawPose(); if (pose != null) { Matrix34F rawPose = new Matrix34F(); float[] poseData = Arrays.copyOfRange(pose.transposed().getData(), 0, 12); rawPose.setData(poseData); Vec2F upperLeft = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(-TARGET_WIDTH / 2, TARGET_HEIGHT / 2, 0)); Vec2F upperRight = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(TARGET_WIDTH / 2, TARGET_HEIGHT / 2, 0)); Vec2F lowerRight = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(TARGET_WIDTH / 2, -TARGET_HEIGHT / 2, 0)); Vec2F lowerLeft = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(-TARGET_WIDTH / 2, -TARGET_HEIGHT / 2, 0)); double dblMidPointTopx = (upperRight.getData()[0] + upperLeft.getData()[0]) / 2; double dblMidPointTopy = (upperRight.getData()[1] + upperLeft.getData()[1]) / 2; double dblMidPointBotx = (lowerRight.getData()[0] + lowerLeft.getData()[0]) / 2; double dblMidPointBoty = (lowerRight.getData()[1] + lowerLeft.getData()[1]) / 2; double width = Math.sqrt((Math.pow((upperRight.getData()[1] - upperLeft.getData()[1]), 2)) + (Math.pow((upperRight.getData()[0] - upperLeft.getData()[0]), 2))); double height = Math.sqrt((Math.pow((dblMidPointTopy - dblMidPointBoty), 2)) + (Math.pow((dblMidPointTopx - dblMidPointBotx), 2))); //width is equal to 254 mm, so width of beacon is 220mm, height of beacon is 150mm //pixels per mm width, using a known size of the target double dblWidthPixelsPermm = width / TARGET_WIDTH; double dblHeightPixelsPermm = height / TARGET_HEIGHT; //beacon base is about 25mm above top of target beaconBotRight = new Point((dblMidPointTopx + (110 * dblWidthPixelsPermm)), dblMidPointTopy - (30 * dblHeightPixelsPermm)); beaconTopLeft = new Point((dblMidPointTopx - (110 * dblWidthPixelsPermm)), dblMidPointTopy - (160 * dblHeightPixelsPermm)); beaconMiddle.x = dblMidPointTopx; beaconMiddle.y = dblMidPointTopy + (105 * dblHeightPixelsPermm); gotBeacomDims = true; if (debug >= 1) { fileLogger.writeEvent("Vuforia", "upperLeft 0 " + upperLeft.getData()[0]); fileLogger.writeEvent("Vuforia", "upperLeft 1 " + upperLeft.getData()[1]); Log.d("Vuforia", "upperLeft 0 " + upperLeft.getData()[0]); Log.d("Vuforia", "upperLeft 1 " + upperLeft.getData()[1]); fileLogger.writeEvent("Vuforia", "upperRight 0 " + upperRight.getData()[0]); fileLogger.writeEvent("Vuforia", "upperRight 1 " + upperRight.getData()[1]); Log.d("Vuforia", "upperRight 0 " + upperRight.getData()[0]); Log.d("Vuforia", "upperRight 1 " + upperRight.getData()[1]); fileLogger.writeEvent("Vuforia", "lowerLeft 0 " + lowerLeft.getData()[0]); fileLogger.writeEvent("Vuforia", "lowerLeft 1 " + lowerLeft.getData()[1]); Log.d("Vuforia", "lowerLeft 0 " + lowerLeft.getData()[0]); Log.d("Vuforia", "lowerLeft 1 " + lowerLeft.getData()[1]); fileLogger.writeEvent("Vuforia", "lowerRight 0 " + lowerRight.getData()[0]); fileLogger.writeEvent("Vuforia", "lowerRight 1 " + lowerRight.getData()[1]); Log.d("Vuforia", "lowerRight 0 " + lowerRight.getData()[0]); Log.d("Vuforia", "lowerRight 1 " + lowerRight.getData()[1]); fileLogger.writeEvent("Vuforia", "dblMidPointTopx " + dblMidPointTopx); fileLogger.writeEvent("Vuforia", "dblMidPointTopy " + dblMidPointTopy); fileLogger.writeEvent("Vuforia", "dblMidPointBotx " + dblMidPointBotx); fileLogger.writeEvent("Vuforia", "dblMidPointBoty " + dblMidPointBoty); Log.d("Vuforia", "dblMidPointTopx " + dblMidPointTopx); Log.d("Vuforia", "dblMidPointTopy " + dblMidPointTopy); Log.d("Vuforia", "dblMidPointBotx " + dblMidPointBotx); Log.d("Vuforia", "dblMidPointBoty " + dblMidPointBoty); fileLogger.writeEvent("Vuforia", "width in pixels " + width); fileLogger.writeEvent("Vuforia", "height in pixels " + height); Log.d("Vuforia", "width in pixels " + width); Log.d("Vuforia", "height in pixels " + height); } } } if (gotBeacomDims) { VuforiaLocalizer.CloseableFrame frame = vuforia.getFrameQueue().take(); //takes the frame at the head of the queue long numImages = frame.getNumImages(); for (int i = 0; i < numImages; i++) { if (frame.getImage(i).getFormat() == PIXEL_FORMAT.RGB565) { rgb = frame.getImage(i); break; } } /*rgb is now the Image object that weve used in the video*/ Log.d("OPENCV", "Height " + rgb.getHeight() + " Width " + rgb.getWidth()); Bitmap bm = Bitmap.createBitmap(rgb.getWidth(), rgb.getHeight(), Bitmap.Config.RGB_565); bm.copyPixelsFromBuffer(rgb.getPixels()); Mat tmp = new Mat(rgb.getWidth(), rgb.getHeight(), CvType.CV_8UC4); Utils.bitmapToMat(bm, tmp); if (beaconTopLeft.x < 0) beaconTopLeft.x = 0; if (beaconTopLeft.y < 0) beaconTopLeft.y = 0; if (beaconBotRight.x > rgb.getWidth()) beaconBotRight.x = rgb.getWidth(); if (beaconBotRight.y > rgb.getHeight()) beaconBotRight.y = rgb.getHeight(); frame.close(); //Constants.BeaconColours Colour = beaconColour.beaconAnalysisOCV(tmp, loop); //Constants.BeaconColours Colour = beaconColour.beaconAnalysisOCV2(tmp, loop, debug); Colour = beaconColour.BeaconAnalysisOCVPlayground(debug, tmp, loop, beaconTopLeft, beaconBotRight, beaconMiddle); loop++; Log.d("OPENCV", "Returned " + Colour); } for (VuforiaTrackable trackable : allTrackables) { /** * getUpdatedRobotLocation() will return null if no new information is available since * the last time that call was made, or if the trackable is not currently visible. * getRobotLocation() will return null if the trackable is not currently visible. */ telemetry.addData(trackable.getName(), ((VuforiaTrackableDefaultListener) trackable.getListener()).isVisible() ? "Visible" : "Not Visible"); // OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener) trackable.getListener()) .getUpdatedRobotLocation(); if (robotLocationTransform != null) { lastLocation = robotLocationTransform; } } /** * Provide feedback as to where the robot was last located (if we know). */ if (lastLocation != null) { // Then you can extract the positions and angles using the getTranslation and getOrientation methods. VectorF trans = lastLocation.getTranslation(); Orientation rot = Orientation.getOrientation(lastLocation, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES); // Robot position is defined by the standard Matrix translation (x and y) robotX = trans.get(0); robotY = trans.get(1); // Robot bearing (in Cartesian system) position is defined by the standard Matrix z rotation robotBearing = rot.thirdAngle; if (robotBearing < 0) { robotBearing = 360 + robotBearing; } telemetry.addData("Pos X ", robotX); telemetry.addData("Pos Y ", robotY); telemetry.addData("Bear ", robotBearing); // RobotLog.vv(TAG, "robot=%s", format(lastLocation)); telemetry.addData("Pos ", format(lastLocation)); telemetry.addData("Text ", "*** Vision Data***"); //telemetry.addData("Red ", "Red : " + redpoint); //telemetry.addData("Blue ", "Blue: " + bluepoint); //telemetry.addData("Dir ", "Direction: " + directionOfBeacon); } else { telemetry.addData("Pos ", "Unknown"); } switch (Colour) { case BEACON_BLUE_RED: telemetry.addData("Beacon ", "Blue Red"); break; case BEACON_RED_BLUE: telemetry.addData("Beacon ", "Red Blue"); break; case BEACON_BLUE_LEFT: telemetry.addData("Beacon ", "Blue Left"); break; case BEACON_RED_LEFT: telemetry.addData("Beacon ", "Red Left"); break; case BEACON_BLUE_RIGHT: telemetry.addData("Beacon ", "Blue Right"); break; case BEACON_RED_RIGHT: telemetry.addData("Beacon ", "Red Right"); break; case UNKNOWN: telemetry.addData("Beacon ", "Unknown"); break; } telemetry.update(); } //stop the log if (fileLogger != null) { fileLogger.writeEvent("stop()", "Stopped"); fileLogger.close(); fileLogger = null; } }
From source file:club.towr5291.opmodes.AutoDriveTeam5291.java
License:Open Source License
@Override public void runOpMode() throws InterruptedException { final boolean LedOn = false; final boolean LedOff = true; //load menu settings and setup robot and debug level sharedPreferences = PreferenceManager.getDefaultSharedPreferences(hardwareMap.appContext); teamNumber = sharedPreferences.getString("club.towr5291.Autonomous.TeamNumber", "0000"); allianceColor = sharedPreferences.getString("club.towr5291.Autonomous.Color", "Red"); allianceStartPosition = sharedPreferences.getString("club.towr5291.Autonomous.StartPosition", "Left"); allianceParkPosition = sharedPreferences.getString("club.towr5291.Autonomous.ParkPosition", "Vortex"); delay = Integer.parseInt(sharedPreferences.getString("club.towr5291.Autonomous.Delay", "0")); numBeacons = sharedPreferences.getString("club.towr5291.Autonomous.Beacons", "One"); robotConfig = sharedPreferences.getString("club.towr5291.Autonomous.RobotConfig", "TileRunner-2x40"); debug = Integer.parseInt(sharedPreferences.getString("club.towr5291.Autonomous.Debug", "1")); telemetry.addData("Init1 ", "Starting!"); telemetry.update();/* w w w.j a va 2 s.c o m*/ // // get a reference to a Modern Robotics DIM, and IO channels. dim = hardwareMap.get(DeviceInterfaceModule.class, "dim"); // Use generic form of device mapping dim.setDigitalChannelMode(GREEN1_LED_CHANNEL, DigitalChannelController.Mode.OUTPUT); // Set the direction of each channel dim.setDigitalChannelMode(RED1_LED_CHANNEL, DigitalChannelController.Mode.OUTPUT); // Set the direction of each channel dim.setDigitalChannelMode(BLUE1_LED_CHANNEL, DigitalChannelController.Mode.OUTPUT); // Set the direction of each channel dim.setDigitalChannelMode(GREEN2_LED_CHANNEL, DigitalChannelController.Mode.OUTPUT); // Set the direction of each channel dim.setDigitalChannelMode(RED2_LED_CHANNEL, DigitalChannelController.Mode.OUTPUT); // Set the direction of each channel dim.setDigitalChannelMode(BLUE2_LED_CHANNEL, DigitalChannelController.Mode.OUTPUT); // Set the direction of each channel LedState(LedOn, LedOn, LedOn, LedOn, LedOn, LedOn); //load variables LibraryStateSegAuto processingSteps = new LibraryStateSegAuto(0, 0, "", false, false, 0, 0, 0, 0, 0, 0, 0); sixValues[] pathValues = new sixValues[1000]; A0Star a0Star = new A0Star(); String fieldOutput; HashMap<String, LibraryStateSegAuto> autonomousStepsAStar = new HashMap<>(); if (debug >= 1) { fileLogger = new FileLogger(runtime); fileLogger.open(); fileLogger.write("Time,SysMS,Thread,Event,Desc"); fileLogger.writeEvent(TAG, "Log Started"); Log.d(TAG, "Log Started"); runtime.reset(); telemetry.addData("FileLogger: ", runtime.toString()); telemetry.addData("FileLogger Op Out File: ", fileLogger.getFilename()); } //load all the vuforia stuff VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(R.id.cameraMonitorViewId); parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK; switch (teamNumber) { case "5291": parameters.vuforiaLicenseKey = "AVATY7T/////AAAAGQJxfNYzLUgGjSx0aOEU0Q0rpcfZO2h2sY1MhUZUr+Bu6RgoUMUP/nERGmD87ybv1/lM2LBFDxcBGRHkXvxtkHel4XEUCsNHFTGWYcVkMIZqctQsIrTe13MnUvSOfQj8ig7xw3iULcwDpY+xAftW61dKTJ0IAOCxx2F0QjJWqRJBxrEUR/DfQi4LyrgnciNMXCiZ8KFyBdC63XMYkQj2joTN579+2u5f8aSCe8jkAFnBLcB1slyaU9lhnlTEMcFjwrLBcWoYIFAZluvFT0LpqZRlS1/XYf45QBSJztFKHIsj1rbCgotAE36novnAQBs74ewnWsJifokJGOYWdFJveWzn3GE9OEH23Y5l7kFDu4wc"; break; case "11230": parameters.vuforiaLicenseKey = "Not Provided"; break; case "11231": parameters.vuforiaLicenseKey = "Aai2GEX/////AAAAGaIIK9GK/E5ZsiRZ/jrJzdg7wYZCIFQ7uzKqQrMx/0Hh212zumzIy4raGwDY6Mf6jABMShH2etZC/BcjIowIHeAG5ShG5lvZIZEplTO+1zK1nFSiGFTPV59iGVqH8KjLbQdgUbsCBqp4f3tI8BWYqAS27wYIPfTK697SuxdQnpEZAOhHpgz+S2VoShgGr+EElzYMBFEaj6kdA/Lq5OwQp31JPet7NWYph6nN+TNHJAxnQBkthYmQg687WlRZhYrvNJepnoEwsDO3NSyeGlFquwuQwgdoGjzq2qn527I9tvM/XVZt7KR1KyWCn3PIS/LFvADSuyoQ2lsiOFtM9C+KCuNWiqQmj7dPPlpvVeUycoDH"; break; } VuforiaLocalizer vuforia = ClassFactory.createVuforiaLocalizer(parameters); Vuforia.setFrameFormat(PIXEL_FORMAT.RGB565, true); //enables RGB565 format for the image vuforia.setFrameQueueCapacity(1); //tells VuforiaLocalizer to only store one frame at a time Vuforia.setHint(HINT.HINT_MAX_SIMULTANEOUS_IMAGE_TARGETS, 4); VuforiaTrackables velocityVortex = vuforia.loadTrackablesFromAsset("FTC_2016-17"); VuforiaTrackable wheels = velocityVortex.get(0); wheels.setName("wheels"); // wheels target VuforiaTrackable tools = velocityVortex.get(1); tools.setName("tools"); // tools target VuforiaTrackable legos = velocityVortex.get(2); legos.setName("legos"); // legos target VuforiaTrackable gears = velocityVortex.get(3); gears.setName("gears"); // gears target /** For convenience, gather together all the trackable objects in one easily-iterable collection */ List<VuforiaTrackable> allTrackables = new ArrayList<VuforiaTrackable>(); allTrackables.addAll(velocityVortex); Vuforia.setFrameFormat(PIXEL_FORMAT.RGB565, true); /** * We use units of mm here because that's the recommended units of measurement for the * size values specified in the XML for the ImageTarget trackables in data sets. E.g.: * <ImageTarget name="stones" size="247 173"/> * You don't *have to* use mm here, but the units here and the units used in the XML * target configuration files *must* correspond for the math to work out correctly. */ float mmPerInch = 25.4f; float mmBotWidth = 18 * mmPerInch; // ... or whatever is right for your robot float mmFTCFieldWidth = (12 * 12 - 2) * mmPerInch; // the FTC field is ~11'10" center-to-center of the glass panels /** * In order for localization to work, we need to tell the system where each target we * wish to use for navigation resides on the field, and we need to specify where on the robot * the phone resides. These specifications are in the form of <em>transformation matrices.</em> * Transformation matrices are a central, important concept in the math here involved in localization. * See <a href="https://en.wikipedia.org/wiki/Transformation_matrix">Transformation Matrix</a> * for detailed information. Commonly, you'll encounter transformation matrices as instances * of the {@link OpenGLMatrix} class. * * For the most part, you don't need to understand the details of the math of how transformation * matrices work inside (as fascinating as that is, truly). Just remember these key points: * <ol> * * <li>You can put two transformations together to produce a third that combines the effect of * both of them. If, for example, you have a rotation transform R and a translation transform T, * then the combined transformation matrix RT which does the rotation first and then the translation * is given by {@code RT = T.multiplied(R)}. That is, the transforms are multiplied in the * <em>reverse</em> of the chronological order in which they applied.</li> * * <li>A common way to create useful transforms is to use methods in the {@link OpenGLMatrix} * class and the Orientation class. See, for example, {@link OpenGLMatrix#translation(float, * float, float)}, {@link OpenGLMatrix#rotation(AngleUnit, float, float, float, float)}, and * {@link Orientation#getRotationMatrix(AxesReference, AxesOrder, AngleUnit, float, float, float)}. * Related methods in {@link OpenGLMatrix}, such as {@link OpenGLMatrix#rotated(AngleUnit, * float, float, float, float)}, are syntactic shorthands for creating a new transform and * then immediately multiplying the receiver by it, which can be convenient at times.</li> * * <li>If you want to break open the black box of a transformation matrix to understand * what it's doing inside, use {@link MatrixF#getTranslation()} to fetch how much the * transform will move you in x, y, and z, and use {@link Orientation#getOrientation(MatrixF, * AxesReference, AxesOrder, AngleUnit)} to determine the rotational motion that the transform * will impart. See {@link #format(OpenGLMatrix)} below for an example.</li> * * </ol> * * This example places the "stones" image on the perimeter wall to the Left * of the Red Driver station wall. Similar to the Red Beacon Location on the Res-Q * * This example places the "chips" image on the perimeter wall to the Right * of the Blue Driver station. Similar to the Blue Beacon Location on the Res-Q * * See the doc folder of this project for a description of the field Axis conventions. * * Initially the target is conceptually lying at the origin of the field's coordinate system * (the center of the field), facing up. * * In this configuration, the target's coordinate system aligns with that of the field. * * In a real situation we'd also account for the vertical (Z) offset of the target, * but for simplicity, we ignore that here; for a real robot, you'll want to fix that. * * To place the Wheels Target on the Red Audience wall: * - First we rotate it 90 around the field's X axis to flip it upright * - Then we rotate it 90 around the field's Z access to face it away from the audience. * - Finally, we translate it back along the X axis towards the red audience wall. * */ // RED Targets // To Place GEARS Target // position is approximately - (-6feet, -1feet) OpenGLMatrix gearsTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the RED WALL. Our translation here is a negative translation in X.*/ .translation(-mmFTCFieldWidth / 2, -1 * 12 * mmPerInch, 0).multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 90, 0)); gears.setLocation(gearsTargetLocationOnField); // To Place GEARS Target // position is approximately - (-6feet, 3feet) OpenGLMatrix toolsTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the Blue Audience wall. Our translation here is a positive translation in Y.*/ .translation(-mmFTCFieldWidth / 2, 3 * 12 * mmPerInch, 0) //.translation(0, mmFTCFieldWidth/2, 0) .multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 90, 0)); tools.setLocation(toolsTargetLocationOnField); //Finish RED Targets // BLUE Targets // To Place LEGOS Target // position is approximately - (-3feet, 6feet) OpenGLMatrix legosTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the RED WALL. Our translation here is a negative translation in X.*/ .translation(-3 * 12 * mmPerInch, mmFTCFieldWidth / 2, 0).multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 0, 0)); legos.setLocation(legosTargetLocationOnField); // To Place WHEELS Target // position is approximately - (1feet, 6feet) OpenGLMatrix wheelsTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the Blue Audience wall. Our translation here is a positive translation in Y.*/ .translation(1 * 12 * mmPerInch, mmFTCFieldWidth / 2, 0).multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 0, 0)); wheels.setLocation(wheelsTargetLocationOnField); //Finish BLUE Targets /** * Create a transformation matrix describing where the phone is on the robot. Here, we * put the phone on the right hand side of the robot with the screen facing in (see our * choice of BACK camera above) and in landscape mode. Starting from alignment between the * robot's and phone's axes, this is a rotation of -90deg along the Y axis. * * When determining whether a rotation is positive or negative, consider yourself as looking * down the (positive) axis of rotation from the positive towards the origin. Positive rotations * are then CCW, and negative rotations CW. An example: consider looking down the positive Z * axis towards the origin. A positive rotation about Z (ie: a rotation parallel to the the X-Y * plane) is then CCW, as one would normally expect from the usual classic 2D geometry. */ OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix.translation((mmBotWidth / 2), 0, 300) .multiplied(Orientation.getRotationMatrix(AxesReference.EXTRINSIC, AxesOrder.YZY, AngleUnit.DEGREES, -90, 0, 0)); //RobotLog.ii(TAG, "phone=%s", format(phoneLocationOnRobot)); /** * Let the trackable listeners we care about know where the phone is. We know that each * listener is a {@link VuforiaTrackableDefaultListener} and can so safely cast because * we have not ourselves installed a listener of a different type. */ ((VuforiaTrackableDefaultListener) gears.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); ((VuforiaTrackableDefaultListener) tools.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); ((VuforiaTrackableDefaultListener) legos.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); ((VuforiaTrackableDefaultListener) wheels.getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); /** * A brief tutorial: here's how all the math is going to work: * * C = phoneLocationOnRobot maps phone coords -> robot coords * P = tracker.getPose() maps image target coords -> phone coords * L = redTargetLocationOnField maps image target coords -> field coords * * So * * C.inverted() maps robot coords -> phone coords * P.inverted() maps phone coords -> imageTarget coords * * Putting that all together, * * L x P.inverted() x C.inverted() maps robot coords to field coords. * * @see VuforiaTrackableDefaultListener#getRobotLocation() */ telemetry.addData("Init2 ", "Vuforia Options Loaded!"); telemetry.update(); //to add more config options edit strings.xml and AutonomousConfiguration.java switch (robotConfig) { case "TileRunner-2x40": //Velocity Vortex Competition Base REVERSE_DIRECTION = 1; // Reverse the direction without significant code changes, (using motor FORWARD REVERSE will affect the driver station as we use same robotconfig file COUNTS_PER_MOTOR_REV = 1120; // eg: TETRIX = 1440 pulses, NeveRest 20 = 560 pulses, NeveRest 40 = 1120, NeveRest 60 = 1680 pulses DRIVE_GEAR_REDUCTION = 0.7; // This is < 1.0 if geared UP, Tilerunner is geared up WHEEL_DIAMETER_INCHES = 4.0; // For figuring circumference WHEEL_ACTUAL_FUDGE = 1; // Fine tuning amount COUNTS_PER_INCH = ((COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (WHEEL_DIAMETER_INCHES * 3.1415)) * WHEEL_ACTUAL_FUDGE * REVERSE_DIRECTION; ROBOT_TRACK = 16.5; // distance between centerline of rear wheels robot will pivot on rear wheel of omni on front, 16.5 track is 103.67 inches full circle WHEEL_TURN_FUDGE = 1.0; // Fine tuning amount COUNTS_PER_DEGREE = (((2 * 3.1415 * ROBOT_TRACK) * COUNTS_PER_INCH) / 360) * WHEEL_TURN_FUDGE; loadPowerTableTileRunner(); //load the power table break; case "5291 Tank Tread-2x40 Custom": //for tank tread base REVERSE_DIRECTION = 1; COUNTS_PER_MOTOR_REV = 1120; // eg: TETRIX = 1440 pulses, NeveRest 20 = 560 pulses, NeveRest 40 = 1120, NeveRest 60 = 1680 pulses DRIVE_GEAR_REDUCTION = 1.0; // Tank Tread is 1:1 ration WHEEL_DIAMETER_INCHES = 3.75; // For figuring circumference WHEEL_ACTUAL_FUDGE = 1; // Fine tuning amount COUNTS_PER_INCH = ((COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (WHEEL_DIAMETER_INCHES * 3.1415)) * WHEEL_ACTUAL_FUDGE * REVERSE_DIRECTION; ROBOT_TRACK = 18; // distance between centerline of rear wheels robot will pivot on rear wheel of omni on front, 16.5 track is 103.67 inches full circle WHEEL_TURN_FUDGE = 1.12; // Fine tuning amount COUNTS_PER_DEGREE = (((2 * 3.1415 * ROBOT_TRACK) * COUNTS_PER_INCH) / 360) * WHEEL_TURN_FUDGE; loadPowerTableTankTread(); //load the power table break; case "11231 2016 Custom": //2016 - 11231 Drivetrain COUNTS_PER_MOTOR_REV = 1120; // eg: TETRIX = 1440 pulses, NeveRest 20 = 560 pulses, NeveRest 40 = 1120, NeveRest 60 = 1680 pulses DRIVE_GEAR_REDUCTION = .667; // (.665) UP INCREASES THE DISTANCE This is < 1.0 if geared UP, Tilerunner is geared up WHEEL_DIAMETER_INCHES = 4.0; // For figuring circumference WHEEL_ACTUAL_FUDGE = 1; // Fine tuning amount COUNTS_PER_INCH = ((COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (WHEEL_DIAMETER_INCHES * 3.1415926535)) * WHEEL_ACTUAL_FUDGE; ROBOT_TRACK = 18; // distance between centerline of rear wheels robot will pivot on rear wheel of omni on front, 16.5 track is 103.67 inches full circle COUNTS_PER_DEGREE = ((2 * 3.1415926535 * ROBOT_TRACK) * COUNTS_PER_INCH) / 360; //loadPowerTableTileRunner(); //load the power table break; default: //default for competition TileRunner-2x40 REVERSE_DIRECTION = 1; COUNTS_PER_MOTOR_REV = 1120; // eg: TETRIX = 1440 pulses, NeveRest 20 = 560 pulses, NeveRest 40 = 1120, NeveRest 60 = 1680 pulses DRIVE_GEAR_REDUCTION = 1.28; // This is < 1.0 if geared UP, Tilerunner is geared up WHEEL_DIAMETER_INCHES = 4.0; // For figuring circumference WHEEL_ACTUAL_FUDGE = 1; // Fine tuning amount COUNTS_PER_INCH = ((COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (WHEEL_DIAMETER_INCHES * 3.1415)) * WHEEL_ACTUAL_FUDGE * REVERSE_DIRECTION; ROBOT_TRACK = 16.5; // distance between centerline of rear wheels robot will pivot on rear wheel of omni on front, 16.5 track is 103.67 inches full circle COUNTS_PER_DEGREE = ((2 * 3.1415 * ROBOT_TRACK) * COUNTS_PER_INCH) / 360; loadPowerTableTileRunner(); //load the power table break; } if (debug >= 1) { fileLogger.writeEvent(TAG, "Team # " + teamNumber); fileLogger.writeEvent(TAG, "Alliance Colour " + allianceColor); fileLogger.writeEvent(TAG, "Alliance Start Pos " + allianceStartPosition); fileLogger.writeEvent(TAG, "Alliance Park Pos " + allianceParkPosition); fileLogger.writeEvent(TAG, "Alliance Delay " + delay); fileLogger.writeEvent(TAG, "Alliance Beacons " + numBeacons); fileLogger.writeEvent(TAG, "Robot Config " + robotConfig); Log.d(TAG, "Team # " + teamNumber); Log.d(TAG, "Alliance Colour " + allianceColor); Log.d(TAG, "Alliance Start Pos " + allianceStartPosition); Log.d(TAG, "Alliance Park Pos " + allianceParkPosition); Log.d(TAG, "Alliance Delay " + delay); Log.d(TAG, "Alliance Beacons " + numBeacons); Log.d(TAG, "Robot Config " + robotConfig); } telemetry.addData("Init3 ", "Loading Steps"); telemetry.update(); //load the sequence based on alliance colour and team switch (teamNumber) { case "5291": switch (allianceColor) { case "Red": LedState(LedOff, LedOn, LedOff, LedOff, LedOn, LedOff); switch (allianceStartPosition) { case "Left": autonomousSteps = autonomousStepsFile.ReadStepFile("5291RedLeft.csv", allianceParkPosition, numBeacons); break; case "Right": autonomousSteps = autonomousStepsFile.ReadStepFile("5291RedRight.csv", allianceParkPosition, numBeacons); break; } break; case "Blue": LedState(LedOff, LedOff, LedOn, LedOff, LedOff, LedOn); switch (allianceStartPosition) { case "Left": autonomousSteps = autonomousStepsFile.ReadStepFile("5291BlueLeft.csv", allianceParkPosition, numBeacons); break; case "Right": autonomousSteps = autonomousStepsFile.ReadStepFile("5291BlueRight.csv", allianceParkPosition, numBeacons); break; } break; case "Test": autonomousSteps = autonomousStepsFile.ReadStepFile("5291Test.csv", allianceParkPosition, numBeacons); break; } break; case "11230": switch (allianceColor) { case "Red": switch (allianceStartPosition) { case "Left": autonomousSteps = autonomousStepsFile.ReadStepFile("11230RedLeft.csv", allianceParkPosition, numBeacons); break; case "Right": autonomousSteps = autonomousStepsFile.ReadStepFile("11230RedRight.csv", allianceParkPosition, numBeacons); break; } break; case "Blue": switch (allianceStartPosition) { case "Left": autonomousSteps = autonomousStepsFile.ReadStepFile("5291BlueRight.csv", allianceParkPosition, numBeacons); break; case "Right": autonomousSteps = autonomousStepsFile.ReadStepFile("5291BlueRight.csv", allianceParkPosition, numBeacons); break; } break; case "Test": autonomousSteps = autonomousStepsFile.ReadStepFile("11230Test.csv", allianceParkPosition, numBeacons); break; } break; case "11231": switch (allianceColor) { case "Red": switch (allianceStartPosition) { case "Left": autonomousSteps = autonomousStepsFile.ReadStepFile("11231RedLeft.csv", allianceParkPosition, numBeacons); break; case "Right": autonomousSteps = autonomousStepsFile.ReadStepFile("11231RedRight.csv", allianceParkPosition, numBeacons); break; } break; case "Blue": switch (allianceStartPosition) { case "Left": autonomousSteps = autonomousStepsFile.ReadStepFile("11231BleLeft.csv", allianceParkPosition, numBeacons); break; case "Right": autonomousSteps = autonomousStepsFile.ReadStepFile("11231BlueRight.csv", allianceParkPosition, numBeacons); break; } break; case "Test": autonomousSteps = autonomousStepsFile.ReadStepFile("11231Test.csv", allianceParkPosition, numBeacons); break; } break; } //need to load initial step of a delay based on user input autonomousStepsFile.insertSteps(delay + 1, "DEL" + (delay * 1000), false, false, 0, 0, 0, 0, 0, 0, 0, 1); // Set up the parameters with which we will use our IMU. Note that integration // algorithm here just reports accelerations to the logcat log; it doesn't actually // provide positional information. BNO055IMU.Parameters parametersAdafruitImu = new BNO055IMU.Parameters(); parametersAdafruitImu.angleUnit = BNO055IMU.AngleUnit.DEGREES; parametersAdafruitImu.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; parametersAdafruitImu.calibrationDataFile = "AdafruitIMUCalibration.json"; // see the calibration sample opmode parametersAdafruitImu.loggingEnabled = true; parametersAdafruitImu.loggingTag = "IMU"; parametersAdafruitImu.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator(); //don't crash the program if the GRYO is faulty, just bypass it try { // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU", // and named "imu". imu = hardwareMap.get(BNO055IMU.class, "imu"); imu.initialize(parametersAdafruitImu); // get a reference to a Modern Robotics GyroSensor object. gyro = (ModernRoboticsI2cGyro) hardwareMap.gyroSensor.get("gyro"); // calibrate the gyro, this takes a few seconds gyro.calibrate(); telemetry.addData("Init4 ", "Calibrating Gyro"); telemetry.update(); } catch (Exception e) { if (debug >= 1) { fileLogger.writeEvent(TAG, "Gyro Error " + e.getMessage()); Log.d(TAG, "Gyro Error " + e.getMessage()); } gyroError = true; } telemetry.addData("Init5 ", "Line Sensors"); telemetry.update(); LineSensor1 = hardwareMap.get(AnalogInput.class, "linesensor1"); LineSensor2 = hardwareMap.get(AnalogInput.class, "linesensor2"); LineSensor3 = hardwareMap.get(AnalogInput.class, "linesensor3"); LineSensor4 = hardwareMap.get(AnalogInput.class, "linesensor4"); LineSensor5 = hardwareMap.get(AnalogInput.class, "linesensor5"); /* * Initialize the drive system variables. * The init() method of the hardware class does all the work here */ telemetry.addData("Init6 ", "Base and Arm Motors"); telemetry.update(); robotDrive.init(hardwareMap); armDrive.init(hardwareMap); telemetry.addData("Init7 ", "Range Sensors"); telemetry.update(); RANGE1 = hardwareMap.i2cDevice.get("range1"); RANGE1Reader = new I2cDeviceSynchImpl(RANGE1, RANGE1ADDRESS, false); RANGE1Reader.engage(); RANGE2 = hardwareMap.i2cDevice.get("range2"); RANGE2Reader = new I2cDeviceSynchImpl(RANGE2, RANGE2ADDRESS, false); RANGE2Reader.engage(); // get a reference to our ColorSensor object. try { telemetry.addData("Init8 ", "Colour Sensor"); telemetry.update(); colorSensor = hardwareMap.colorSensor.get("sensorcolor"); } catch (Exception e) { if (debug >= 1) { fileLogger.writeEvent(TAG, "colour Error " + e.getMessage()); Log.d(TAG, "colour Error " + e.getMessage()); } colourError = true; } telemetry.addData("Init9 ", "Servos"); telemetry.update(); //config the servos servoBeaconRight = hardwareMap.servo.get("servobeaconright"); servoBeaconLeft = hardwareMap.servo.get("servobeaconleft"); servoBeaconRight.setDirection(Servo.Direction.REVERSE); servoLifterRight = hardwareMap.servo.get("servoliftright"); servoLifterLeft = hardwareMap.servo.get("servoliftleft"); servoLifterRight.setDirection(Servo.Direction.REVERSE); //lock the arms up moveServo(servoLifterRight, 135, SERVOLIFTRIGHT_MIN_RANGE, SERVOLIFTRIGHT_MAX_RANGE); moveServo(servoLifterLeft, 135, SERVOLIFTLEFT_MIN_RANGE, SERVOLIFTLEFT_MAX_RANGE); // Move the beacon pushers to home moveServo(servoBeaconRight, SERVOBEACONRIGHT_HOME, SERVOBEACONRIGHT_MIN_RANGE, SERVOBEACONRIGHT_MAX_RANGE); moveServo(servoBeaconLeft, SERVOBEACONLEFT_HOME, SERVOBEACONLEFT_MIN_RANGE, SERVOBEACONLEFT_MAX_RANGE); // Send telemetry message to signify robot waiting; telemetry.addData("Status", "Resetting Encoders"); // robotDrive.leftMotor1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robotDrive.leftMotor2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robotDrive.rightMotor1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robotDrive.rightMotor2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); robotDrive.leftMotor1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); robotDrive.leftMotor2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); robotDrive.rightMotor1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); robotDrive.rightMotor2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); mintCurStStep = stepState.STATE_INIT; mintCurStTankTurn = stepState.STATE_COMPLETE; mintCurStDrive = stepState.STATE_COMPLETE; mintCurStDriveHeading = stepState.STATE_COMPLETE; mintCurStPivotTurn = stepState.STATE_COMPLETE; mintCurStRadiusTurn = stepState.STATE_COMPLETE; mintCurStDelay = stepState.STATE_COMPLETE; mintCurStVuforiaLoc5291 = stepState.STATE_COMPLETE; mintCurStVuforiaMove5291 = stepState.STATE_COMPLETE; mintCurStVuforiaTurn5291 = stepState.STATE_COMPLETE; mintCurStAttackBeacon5291 = stepState.STATE_COMPLETE; mintCurStBeaconColour5291 = stepState.STATE_COMPLETE; mintCurStShootParticle5291 = stepState.STATE_COMPLETE; mintCurStSweeper5291 = stepState.STATE_COMPLETE; mintCurStEyelids5291 = stepState.STATE_COMPLETE; mintCurStTankTurnGyroHeading = stepState.STATE_COMPLETE; mintCurStLineFind5291 = stepState.STATE_COMPLETE; mintCurStGyroTurnEncoder5291 = stepState.STATE_COMPLETE; mint5291LEDStatus = LEDState.STATE_TEAM; mblnNextStepLastPos = false; if (!gyroError) { while (!isStopRequested() && gyro.isCalibrating()) { sleep(50); idle(); } telemetry.addData("Init10 ", "Calibrated Gyro"); telemetry.update(); } if (debug >= 1) { fileLogger.writeEvent(TAG, "Init Complete"); Log.d(TAG, "Init Complete"); } //set up variable for our capturedimage Image rgb = null; //activate vuforia velocityVortex.activate(); //show options on the driver station phone telemetry.addData("Init11 ", "Complete"); telemetry.addData("Team # ", teamNumber); telemetry.addData("Alliance ", allianceColor); telemetry.addData("Start Pos ", allianceStartPosition); telemetry.addData("Park Pos ", allianceParkPosition); telemetry.addData("Start Del ", delay); telemetry.addData("# Beacons ", numBeacons); telemetry.addData("Robot ", robotConfig); telemetry.update(); // Wait for the game to start (driver presses PLAY) waitForStart(); if (debug >= 1) { fileLogger.writeEvent(TAG, "Value of Gyro Before Reset " + gyro.getIntegratedZValue()); Log.d(TAG, "Value of Gyro Before Reset " + gyro.getIntegratedZValue()); } imu.startAccelerationIntegration(new Position(), new Velocity(), 1000); gyro.resetZAxisIntegrator(); //the main loop. this is where the action happens while (opModeIsActive()) { if (!mblnDisableVisionProcessing) { //start capturing frames for analysis if (mblnReadyToCapture) { boolean gotBeacomDims = false; boolean beacFound = false; Point beaconBotRight = new Point(0, 0); Point beaconTopLeft = new Point(0, 0); Point beaconMiddle = new Point(0, 0); if (mStateTime.milliseconds() < 1000) { gotBeacomDims = true; beacFound = false; } if (!gotBeacomDims) { for (VuforiaTrackable beac : velocityVortex) { OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) beac.getListener()).getRawPose(); if (pose != null) { Matrix34F rawPose = new Matrix34F(); float[] poseData = Arrays.copyOfRange(pose.transposed().getData(), 0, 12); rawPose.setData(poseData); Vec2F upperLeft = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(-TARGET_WIDTH / 2, TARGET_HEIGHT / 2, 0)); Vec2F upperRight = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(TARGET_WIDTH / 2, TARGET_HEIGHT / 2, 0)); Vec2F lowerRight = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(TARGET_WIDTH / 2, -TARGET_HEIGHT / 2, 0)); Vec2F lowerLeft = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(-TARGET_WIDTH / 2, -TARGET_HEIGHT / 2, 0)); double dblMidPointTopx = (upperRight.getData()[0] + upperLeft.getData()[0]) / 2; double dblMidPointTopy = (upperRight.getData()[1] + upperLeft.getData()[1]) / 2; double dblMidPointBotx = (lowerRight.getData()[0] + lowerLeft.getData()[0]) / 2; double dblMidPointBoty = (lowerRight.getData()[1] + lowerLeft.getData()[1]) / 2; double width = Math.sqrt( (Math.pow((upperRight.getData()[1] - upperLeft.getData()[1]), 2)) + (Math .pow((upperRight.getData()[0] - upperLeft.getData()[0]), 2))); double height = Math.sqrt((Math.pow((dblMidPointTopy - dblMidPointBoty), 2)) + (Math.pow((dblMidPointTopx - dblMidPointBotx), 2))); //width is equal to 254 mm, so width of beacon is 220mm, height of beacon is 150mm //pixels per mm width, using a known size of the target double dblWidthPixelsPermm = width / TARGET_WIDTH; double dblHeightPixelsPermm = height / TARGET_HEIGHT; //beacon base is about 25mm above top of target beaconBotRight = new Point((dblMidPointTopx + (110 * dblWidthPixelsPermm)), dblMidPointTopy - (30 * dblHeightPixelsPermm)); beaconTopLeft = new Point((dblMidPointTopx - (110 * dblWidthPixelsPermm)), dblMidPointTopy - (160 * dblHeightPixelsPermm)); beaconMiddle.x = dblMidPointTopx; beaconMiddle.y = dblMidPointTopy + (110 * dblHeightPixelsPermm); gotBeacomDims = true; beacFound = true; if (debug >= 1) { fileLogger.writeEvent("Vuforia", "upperLeft 0 " + upperLeft.getData()[0]); fileLogger.writeEvent("Vuforia", "upperLeft 1 " + upperLeft.getData()[1]); Log.d("Vuforia", "upperLeft 0 " + upperLeft.getData()[0]); Log.d("Vuforia", "upperLeft 1 " + upperLeft.getData()[1]); fileLogger.writeEvent("Vuforia", "upperRight 0 " + upperRight.getData()[0]); fileLogger.writeEvent("Vuforia", "upperRight 1 " + upperRight.getData()[1]); Log.d("Vuforia", "upperRight 0 " + upperRight.getData()[0]); Log.d("Vuforia", "upperRight 1 " + upperRight.getData()[1]); fileLogger.writeEvent("Vuforia", "lowerLeft 0 " + lowerLeft.getData()[0]); fileLogger.writeEvent("Vuforia", "lowerLeft 1 " + lowerLeft.getData()[1]); Log.d("Vuforia", "lowerLeft 0 " + lowerLeft.getData()[0]); Log.d("Vuforia", "lowerLeft 1 " + lowerLeft.getData()[1]); fileLogger.writeEvent("Vuforia", "lowerRight 0 " + lowerRight.getData()[0]); fileLogger.writeEvent("Vuforia", "lowerRight 1 " + lowerRight.getData()[1]); Log.d("Vuforia", "lowerRight 0 " + lowerRight.getData()[0]); Log.d("Vuforia", "lowerRight 1 " + lowerRight.getData()[1]); fileLogger.writeEvent("Vuforia", "dblMidPointTopx " + dblMidPointTopx); fileLogger.writeEvent("Vuforia", "dblMidPointTopy " + dblMidPointTopy); fileLogger.writeEvent("Vuforia", "dblMidPointBotx " + dblMidPointBotx); fileLogger.writeEvent("Vuforia", "dblMidPointBoty " + dblMidPointBoty); Log.d("Vuforia", "dblMidPointTopx " + dblMidPointTopx); Log.d("Vuforia", "dblMidPointTopy " + dblMidPointTopy); Log.d("Vuforia", "dblMidPointBotx " + dblMidPointBotx); Log.d("Vuforia", "dblMidPointBoty " + dblMidPointBoty); fileLogger.writeEvent("Vuforia", "width in pixels " + width); fileLogger.writeEvent("Vuforia", "height in pixels " + height); Log.d("Vuforia", "width in pixels " + width); Log.d("Vuforia", "height in pixels " + height); } } } } if (gotBeacomDims) { VuforiaLocalizer.CloseableFrame frame = vuforia.getFrameQueue().take(); //takes the frame at the head of the queue long numImages = frame.getNumImages(); for (int i = 0; i < numImages; i++) { if (frame.getImage(i).getFormat() == PIXEL_FORMAT.RGB565) { rgb = frame.getImage(i); break; } } /*rgb is now the Image object that weve used in the video*/ Bitmap bm = Bitmap.createBitmap(rgb.getWidth(), rgb.getHeight(), Bitmap.Config.RGB_565); bm.copyPixelsFromBuffer(rgb.getPixels()); //put the image into a MAT for OpenCV Mat tmp = new Mat(rgb.getWidth(), rgb.getHeight(), CvType.CV_8UC4); Utils.bitmapToMat(bm, tmp); if (beaconTopLeft.x < 0) beaconTopLeft.x = 0; if (beaconTopLeft.y < 0) beaconTopLeft.y = 0; if (beaconBotRight.x > rgb.getWidth()) beaconBotRight.x = rgb.getWidth(); if (beaconBotRight.y > rgb.getHeight()) beaconBotRight.y = rgb.getHeight(); //close the frame, prevents memory leaks and crashing frame.close(); //analyse the beacons //Constants.BeaconColours Colour = beaconColour.beaconAnalysisOCV(tmp, loop)); //mColour = beaconColour.beaconAnalysisOCV(tmp, mintCaptureLoop); mColour = beaconColour.beaconAnalysisOCV2(debug, tmp, mintCaptureLoop, beaconTopLeft, beaconBotRight, beaconMiddle, beacFound); if (debug >= 1) { fileLogger.writeEvent("OPENCV", "Returned " + mColour); Log.d("OPENCV", "Returned " + mColour); } telemetry.addData("Beacon ", mColour); mintCaptureLoop++; } } //use vuforia to get locations informatio for (VuforiaTrackable trackable : allTrackables) { /** * getUpdatedRobotLocation() will return null if no new information is available since * the last time that call was made, or if the trackable is not currently visible. * getRobotLocation() will return null if the trackable is not currently visible. */ telemetry.addData(trackable.getName(), ((VuforiaTrackableDefaultListener) trackable.getListener()).isVisible() ? "Visible" : "Not Visible"); // OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener) trackable .getListener()).getUpdatedRobotLocation(); if (robotLocationTransform != null) { lastLocation = robotLocationTransform; } } /** * Provide feedback as to where the robot was last located (if we know). */ if (lastLocation != null) { // Then you can extract the positions and angles using the getTranslation and getOrientation methods. VectorF trans = lastLocation.getTranslation(); Orientation rot = Orientation.getOrientation(lastLocation, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES); // Robot position is defined by the standard Matrix translation (x and y) localisedRobotX = trans.get(0); localisedRobotY = trans.get(1); // Robot bearing (in Cartesian system) position is defined by the standard Matrix z rotation localisedRobotBearing = rot.thirdAngle; if (localisedRobotBearing < 0) { localisedRobotBearing = 360 + localisedRobotBearing; } telemetry.addData("Pos X ", localisedRobotX); telemetry.addData("Pos Y ", localisedRobotY); telemetry.addData("Bear ", localisedRobotBearing); telemetry.addData("Pos ", format(lastLocation)); localiseRobotPos = true; } else { localiseRobotPos = false; telemetry.addData("Pos ", "Unknown"); } } switch (mintCurStStep) { case STATE_INIT: if (debug >= 1) { fileLogger.writeEvent(TAG, "mintCurStStep:- " + mintCurStStep + " mintCurStStep " + mintCurStStep); fileLogger.writeEvent(TAG, "About to check if step exists " + mintCurrentStep); Log.d(TAG, "mintCurStStep:- " + mintCurStStep + " mintCurStStep " + mintCurStStep); Log.d(TAG, "About to check if step exists " + mintCurrentStep); } // get step from hashmap, send it to the initStep for decoding if (autonomousSteps.containsKey(String.valueOf(mintCurrentStep))) { if (debug >= 1) { fileLogger.writeEvent(TAG, "Step Exists TRUE " + mintCurrentStep + " about to get the values from the step"); Log.d(TAG, "Step Exists TRUE " + mintCurrentStep + " about to get the values from the step"); } //processingSteps = autonomousSteps.get(String.valueOf(mintCurrentStep)); //if (debug >= 1) //{ // fileLogger.writeEvent(TAG, "Got the values for step " + mintCurrentStep + " about to decode"); // Log.d(TAG, "Got the values for step " + mintCurrentStep + " about to decode"); //} //decode the step from hashmap //initStep(processingSteps); initStep(); } else { mintCurStStep = stepState.STATE_FINISHED; } break; case STATE_START: break; case STATE_RUNNING: loadParallelSteps(); for (String stKey : mintActiveStepsCopy.keySet()) { if (debug >= 1) { fileLogger.writeEvent("STATE_RUNNING", "Looping through Parallel steps, found " + stKey); Log.d("STATE_RUNNING", "Looping through Parallel steps, found " + stKey); } mintStepNumber = mintActiveStepsCopy.get(stKey); loadActiveStep(mintStepNumber); if (debug >= 1) { fileLogger.writeEvent("STATE_RUNNING", "About to run " + mstrRobotCommand.substring(0, 3)); Log.d("STATE_RUNNING", "About to run " + mstrRobotCommand.substring(0, 3)); } processSteps(mstrRobotCommand.substring(0, 3)); } if ((mintCurStDelay == stepState.STATE_COMPLETE) && (mintCurStBeaconColour5291 == stepState.STATE_COMPLETE) && (mintCurStAttackBeacon5291 == stepState.STATE_COMPLETE) && (mintCurStVuforiaTurn5291 == stepState.STATE_COMPLETE) && (mintCurStVuforiaLoc5291 == stepState.STATE_COMPLETE) && (mintCurStVuforiaMove5291 == stepState.STATE_COMPLETE) && (mintCurStDrive == stepState.STATE_COMPLETE) && (mintCurStDriveHeading == stepState.STATE_COMPLETE) && (mintCurStPivotTurn == stepState.STATE_COMPLETE) && (mintCurStTankTurn == stepState.STATE_COMPLETE) && (mintCurStShootParticle5291 == stepState.STATE_COMPLETE) && (mintCurStSweeper5291 == stepState.STATE_COMPLETE) && (mintCurStEyelids5291 == stepState.STATE_COMPLETE) && (mintCurStLineFind5291 == stepState.STATE_COMPLETE) && (mintCurStGyroTurnEncoder5291 == stepState.STATE_COMPLETE) && (mintCurStTankTurnGyroHeading == stepState.STATE_COMPLETE) && (mintCurStRadiusTurn == stepState.STATE_COMPLETE)) { mintCurStStep = stepState.STATE_COMPLETE; } //make sure we load the current step to determine if parallel, if the steps are run out of order and a previous step was parallel //things get all messed up and a step that isn't parallel can be assumed to be parallel loadActiveStep(mintCurrentStep); if (mblnParallel) { //mark this step as complete and do next one, the current step should continue to run. Not all steps are compatible with being run in parallel // like drive steps, turns etc // Drive forward and shoot // Drive forward and detect beacon // are examples of when parallel steps should be run // errors will occur if other combinations are run // only go to next step if current step equals the one being processed for parallelism. for (String stKey : mintActiveStepsCopy.keySet()) { mintStepNumber = mintActiveStepsCopy.get(stKey); if (mintCurrentStep == mintStepNumber) mintCurStStep = stepState.STATE_COMPLETE; } } break; case STATE_PAUSE: break; case STATE_COMPLETE: if (debug >= 1) { fileLogger.writeEvent(TAG, "Step Complete - Current Step:- " + mintCurrentStep); Log.d(TAG, "Step Complete - Current Step:- " + mintCurrentStep); } // Transition to a new state and next step. mintCurrentStep++; mintCurStStep = stepState.STATE_INIT; break; case STATE_TIMEOUT: setDriveMotorPower(0); // Transition to a new state. mintCurStStep = stepState.STATE_FINISHED; break; case STATE_ERROR: telemetry.addData("STATE", "ERROR WAITING TO FINISH " + mintCurrentStep); break; case STATE_FINISHED: setDriveMotorPower(0); //deactivate vuforia velocityVortex.deactivate(); telemetry.addData("STATE", "FINISHED " + mintCurrentStep); if (debug >= 1) { if (fileLogger != null) { fileLogger.writeEvent(TAG, "Step FINISHED - FINISHED"); fileLogger.writeEvent(TAG, "Stopped"); Log.d(TAG, "FileLogger Stopped"); fileLogger.close(); fileLogger = null; } } break; case STATE_ASTAR_PRE_INIT: mintCurrentStepAStar = 1; //init the Step for AStar //get start point //get end point int startX = (int) processingSteps.getmRobotParm1(); int startY = (int) processingSteps.getmRobotParm2(); int startZ = (int) processingSteps.getmRobotParm3(); int endX = (int) processingSteps.getmRobotParm4(); int endY = (int) processingSteps.getmRobotParm5(); int endDir = (int) processingSteps.getmRobotParm6(); //before using the path in the command lets check if we can localise if (lastLocation != null) { //lets get locations for AStar, direction is most important //x and y position for Vuforia are in mm, AStar in Inches //counter clockwise rotation (x,y) = (-x, y) //origin is center of field //Astar is top right so need to add in 6 feet to each value startX = (int) (localisedRobotX / 25.4) + 72; startY = (int) (localisedRobotY / 25.4) + 72; //need to rotate the axis -90 degrees startZ = (int) localisedRobotBearing; if ((startZ > 357) && (startZ < 3)) startZ = 90; else if ((startZ > 267) && (startZ < 273)) startZ = 0; else if ((startZ > 177) && (startZ < 183)) startZ = 270; else if ((startZ > 87) && (startZ < 93)) startZ = 180; if (debug >= 1) { fileLogger.writeEvent(TAG, "AStar Init - Localised Values"); fileLogger.writeEvent(TAG, "AStar Init - localisedRobotX: " + localisedRobotX); fileLogger.writeEvent(TAG, "AStar Init - localisedRobotY: " + localisedRobotY); fileLogger.writeEvent(TAG, "AStar Init - localisedRobotBearing: " + localisedRobotBearing); fileLogger.writeEvent(TAG, "AStar Init - startX: " + startX); fileLogger.writeEvent(TAG, "AStar Init - startY: " + startY); fileLogger.writeEvent(TAG, "AStar Init - startZ: " + startZ); Log.d(TAG, "AStar Init - Localised Values"); Log.d(TAG, "AStar Init - localisedRobotX: " + localisedRobotX); Log.d(TAG, "AStar Init - localisedRobotY: " + localisedRobotY); Log.d(TAG, "AStar Init - localisedRobotBearing: " + localisedRobotBearing); Log.d(TAG, "AStar Init - startX: " + startX); Log.d(TAG, "AStar Init - startY: " + startY); Log.d(TAG, "AStar Init - startZ: " + startZ); } } //process path pathValues = getPathValues.findPathAStar(startX, startY, startZ, endX, endY, endDir); //for enhanced if (debug >= 1) { fileLogger.writeEvent(TAG, "AStar Path - length: " + pathValues.length); Log.d(TAG, "AStar Path - length: " + pathValues.length); } String[][] mapComplete = new String[A0Star.FIELDWIDTH][A0Star.FIELDWIDTH]; //write path to logfile to verify path for (int y = 0; y < a0Star.fieldLength; y++) { for (int x = 0; x < a0Star.fieldWidth; x++) { switch (allianceColor) { case "Red": if (a0Star.walkableRed[y][x]) { mapComplete[y][x] = "1"; if ((x == startX) && (y == startY)) mapComplete[y][x] = "S"; else if ((x == endX) && (y == endY)) mapComplete[y][x] = "E"; } else { mapComplete[y][x] = "0"; } break; case "Blue": if (a0Star.walkableBlue[y][x]) { mapComplete[y][x] = "1"; if ((x == startX) && (y == startY)) mapComplete[y][x] = "S"; else if ((x == endX) && (y == endY)) mapComplete[y][x] = "E"; } else { if ((x == startX) && (y == startY)) { mapComplete[y][x] = "1"; } else { mapComplete[y][x] = "0"; } } break; default: if (a0Star.walkable[y][x]) { mapComplete[y][x] = "1"; if ((x == startX) && (y == startY)) mapComplete[y][x] = "S"; else if ((x == endX) && (y == endY)) mapComplete[y][x] = "E"; } break; } } } //plot out path.. for (int i = 0; i < pathValues.length; i++) { if (debug >= 1) { fileLogger.writeEvent(TAG, "Path " + pathValues[i].val1 + " " + pathValues[i].val2 + " " + pathValues[i].val3 + " Dir:= " + pathValues[i].val4); Log.d(TAG, "Path " + pathValues[i].val1 + " " + pathValues[i].val2 + " " + pathValues[i].val3 + " Dir:= " + pathValues[i].val4); } if (((int) pathValues[i].val1 == 0) && ((int) pathValues[i].val3 == 0) && ((int) pathValues[i].val2 == 0) && ((int) pathValues[i].val4 == 0)) break; mapComplete[(int) pathValues[i].val3][(int) pathValues[i].val2] = "P"; if ((pathValues[i].val2 == startX) && (pathValues[i].val3 == startY)) { mapComplete[(int) pathValues[i].val3][(int) pathValues[i].val2] = "S"; } } mapComplete[endY][endX] = "E"; fieldOutput = ""; for (int y = 0; y < a0Star.fieldLength; y++) { for (int x = 0; x < a0Star.fieldWidth; x++) { fieldOutput = "" + fieldOutput + mapComplete[y][x]; } if (debug >= 2) { fileLogger.writeEvent(TAG, fieldOutput); Log.d(TAG, fieldOutput); } fieldOutput = ""; } //load path in Hashmap boolean dirChanged; boolean processingAStarSteps = true; int startSegment = 1; int numberOfMoves; int key = 0; int lastDirection = 0; int lasti = 0; String strAngleChange = "RT00"; while (processingAStarSteps) { numberOfMoves = 0; for (int i = startSegment; i < pathValues.length; i++) { numberOfMoves++; if (debug >= 2) { fileLogger.writeEvent(TAG, "Path " + pathValues[i].val1 + " " + pathValues[i].val2 + " " + pathValues[i].val3 + " Dir:= " + pathValues[i].val4); Log.d(TAG, "Path " + pathValues[i].val1 + " " + pathValues[i].val2 + " " + pathValues[i].val3 + " Dir:= " + pathValues[i].val4); } if (((int) pathValues[i].val1 == 0) && ((int) pathValues[i].val2 == 0) && ((int) pathValues[i].val3 == 0)) { if (debug >= 2) { fileLogger.writeEvent(TAG, "End Detected"); Log.d(TAG, "End Detected"); } //end of the sequence, lastDirection = (int) pathValues[i - 1].val4; processingAStarSteps = false; lasti = i; } //need to check if the first step is in a different direction that the start if (i == 1) { if (startZ != pathValues[i].val4) { //need to turn strAngleChange = getAngle(startZ, (int) pathValues[i].val4); if (debug >= 2) { fileLogger.writeEvent(TAG, "First Step Need to turn Robot " + strAngleChange + " Path " + pathValues[i].val1 + " " + pathValues[i].val2 + " " + pathValues[i].val3 + " Dir:= " + pathValues[i].val4); fileLogger.writeEvent(TAG, "Adding Command (" + key + ", 10, " + strAngleChange + ", 0, 0, 0, 0, 0, 0, 1, false) "); Log.d(TAG, "First Step Need to turn Robot " + strAngleChange + " Path " + pathValues[i].val1 + " " + pathValues[i].val2 + " " + pathValues[i].val3 + " Dir:= " + pathValues[i].val4); Log.d(TAG, "Adding Command (" + key + ", 10, " + strAngleChange + ", 0, 0, 0, 0, 0, 0, 1, false) "); } autonomousStepsAStar.put(String.valueOf(key), new LibraryStateSegAuto(key, 10, strAngleChange, false, false, 0, 0, 0, 0, 0, 0, 1)); key++; dirChanged = true; } else { dirChanged = false; //no change in direction } } else { //work out the sequence not the first step if (pathValues[i - 1].val4 != pathValues[i].val4) { //need to turn strAngleChange = getAngle((int) pathValues[i - 1].val4, (int) pathValues[i].val4); dirChanged = true; } else { dirChanged = false; //no change in direction } } if ((dirChanged) || (!processingAStarSteps)) { //found end of segment int AStarPathAngle; if (i == 1) { AStarPathAngle = startZ; } else { AStarPathAngle = (int) pathValues[i - 1].val4; } switch (AStarPathAngle) { case 0: case 90: case 180: case 270: if (debug >= 2) { fileLogger.writeEvent(TAG, "Heading on a Straight line " + (numberOfMoves) + " Path"); fileLogger.writeEvent(TAG, "Adding Command (" + key + ", 10, " + "FW" + (numberOfMoves) + ", false, false, 0, 0, 0, 0, 0, 0, 0.8, false) "); Log.d(TAG, "Heading on a Straight line " + (numberOfMoves) + " Path"); Log.d(TAG, "Adding Command (" + key + ", 10, " + "FW" + (numberOfMoves) + ", false, false, 0, 0, 0, 0, 0, 0, 0.8, false) "); } autonomousStepsAStar.put(String.valueOf(key), new LibraryStateSegAuto(key, 10, "FW" + numberOfMoves, false, false, 0, 0, 0, 0, 0, 0, 0.8)); numberOfMoves = 0; key++; break; case 45: case 135: case 225: case 315: if (debug >= 2) { fileLogger.writeEvent(TAG, "Heading on a Straight line " + (int) ((numberOfMoves) * 1.4142) + " Path"); fileLogger.writeEvent(TAG, "Adding Command (" + key + ", 10, " + "FW" + (int) ((numberOfMoves) * 1.4142) + ", false, false, 0, 0, 0, 0, 0, 0, .8, false) "); Log.d(TAG, "Heading on a Straight line " + (int) ((numberOfMoves) * 1.4142) + " Path"); Log.d(TAG, "Adding Command (" + key + ", 10, " + "FW" + (int) ((numberOfMoves) * 1.4142) + ", false, false, 0, 0, 0, 0, 0, 0, .8, false) "); } autonomousStepsAStar.put(String.valueOf(key), new LibraryStateSegAuto(key, 10, "FW" + (int) (numberOfMoves * 1.4142), false, false, 0, 0, 0, 0, 0, 0, 1)); numberOfMoves = 0; key++; break; } if (debug >= 2) { fileLogger.writeEvent(TAG, "Need to turn Robot " + strAngleChange + " Path " + pathValues[i].val1 + " " + pathValues[i].val2 + " " + pathValues[i].val3 + " Dir:= " + pathValues[i].val4); fileLogger.writeEvent(TAG, "Adding Command (" + key + ", 10, " + strAngleChange + ", 0, 0, 0, 0, 0, 0, 1, false) "); Log.d(TAG, "Need to turn Robot " + strAngleChange + " Path " + pathValues[i].val1 + " " + pathValues[i].val2 + " " + pathValues[i].val3 + " Dir:= " + pathValues[i].val4); Log.d(TAG, "Adding Command (" + key + ", 10, " + strAngleChange + ", 0, 0, 0, 0, 0, 0, 1, false) "); } autonomousStepsAStar.put(String.valueOf(key), new LibraryStateSegAuto(key, 10, strAngleChange, false, false, 0, 0, 0, 0, 0, 0, 0.4)); key++; } if (!processingAStarSteps) break; } //need to work out the direction we are facing and the required direction if ((lastDirection != endDir) && (!processingAStarSteps)) { if (debug >= 2) { fileLogger.writeEvent(TAG, "Sraight Moves Robot End Of Sequence - Need to Trun Robot"); Log.d(TAG, "Sraight Moves Robot End Of Sequence - Need to Trun Robot"); } strAngleChange = getAngle((int) pathValues[lasti - 1].val4, endDir); fileLogger.writeEvent(TAG, "Adding Command (" + key + ", 10, " + strAngleChange + ", 0, 0, 0, 0, 0, 0, 1, false) "); Log.d(TAG, "Adding Command (" + key + ", 10, " + strAngleChange + ", 0, 0, 0, 0, 0, 0, 1, false) "); autonomousStepsAStar.put(String.valueOf(key), new LibraryStateSegAuto(key, 10, strAngleChange, false, false, 0, 0, 0, 0, 0, 0, 0.4)); key++; } } mintCurStStep = stepState.STATE_ASTAR_INIT; break; case STATE_ASTAR_INIT: { if (debug >= 1) { fileLogger.writeEvent(TAG, "About to check if step exists " + mintCurrentStepAStar); Log.d(TAG, "About to check if step exists " + mintCurrentStepAStar); } // get step from hashmap, send it to the initStep for decoding if (autonomousStepsAStar.containsKey(String.valueOf(mintCurrentStepAStar))) { if (debug >= 1) { fileLogger.writeEvent(TAG, "Step Exists TRUE " + mintCurrentStepAStar + " about to get the values from the step"); Log.d(TAG, "Step Exists TRUE " + mintCurrentStepAStar + " about to get the values from the step"); } processingSteps = autonomousStepsAStar.get(String.valueOf(mintCurrentStepAStar)); //read the step from the hashmap autonomousStepsAStar.remove(String.valueOf(mintCurrentStepAStar)); //remove the step from the hashmap if (debug >= 1) { fileLogger.writeEvent(TAG, "Got the values for step " + mintCurrentStepAStar + " about to decode and removed them"); Log.d(TAG, "Got the values for step " + mintCurrentStepAStar + " about to decode and removed them"); } //decode the step from hashmap initAStarStep(processingSteps); } else { //if no steps left in hashmap then complete mintCurStStep = stepState.STATE_ASTAR_COMPLETE; } } break; case STATE_ASTAR_RUNNING: { //move robot according AStar hashmap TankTurnStep(); PivotTurnStep(); RadiusTurnStep(); DriveStepHeading(); if ((mintCurStDriveHeading == stepState.STATE_COMPLETE) && (mintCurStPivotTurn == stepState.STATE_COMPLETE) && (mintCurStTankTurn == stepState.STATE_COMPLETE) && (mintCurStRadiusTurn == stepState.STATE_COMPLETE)) { //increment ASTar Steps Counter mintCurrentStepAStar++; mintCurStStep = stepState.STATE_ASTAR_INIT; } } break; case STATE_ASTAR_ERROR: { //do something on error } break; case STATE_ASTAR_COMPLETE: { //empty hashmap ready for next AStar processing. //clear AStar step counter ready for next AStar process mintCurrentStepAStar = 0; //when complete, keep processing normal step if (debug >= 1) { fileLogger.writeEvent(TAG, "A* Path Completed:- " + mintCurrentStep); Log.d(TAG, "A* Path Completed:- " + mintCurrentStep); } // Transition to a new state and next step. mintCurrentStep++; mintCurStStep = stepState.STATE_INIT; } break; } //process LED status //ERROR - FLASH RED 3 TIMES switch (mint5291LEDStatus) { case STATE_TEAM: //FLASH Alliance Colour switch (allianceColor) { case "Red": LedState(LedOff, LedOn, LedOff, LedOff, LedOn, LedOff); break; case "Blue": LedState(LedOff, LedOff, LedOn, LedOff, LedOff, LedOn); break; default: LedState(LedOn, LedOn, LedOn, LedOn, LedOn, LedOn); break; } case STATE_ERROR: //Flash RED 3 times Rapidly if ((!mblnLEDON) && (mStateTime.milliseconds() > (mdblLastOff + 250))) { mdblLastOn = mStateTime.milliseconds(); mblnLEDON = true; LedState(LedOff, LedOn, LedOff, LedOff, LedOn, LedOff); } else if ((mblnLEDON) && (mStateTime.milliseconds() > (mdblLastOn + 750))) { mdblLastOff = mStateTime.milliseconds(); mblnLEDON = false; LedState(LedOff, LedOff, LedOff, LedOff, LedOff, LedOff); } break; case STATE_SUCCESS: //Flash GREEN 3 times Rapidly if ((!mblnLEDON) && (mStateTime.milliseconds() > (mdblLastOff + 250))) { mdblLastOn = mStateTime.milliseconds(); mblnLEDON = true; LedState(LedOn, LedOff, LedOff, LedOn, LedOff, LedOff); } else if ((mblnLEDON) && (mStateTime.milliseconds() > (mdblLastOn + 250))) { mdblLastOff = mStateTime.milliseconds(); mblnLEDON = false; LedState(LedOff, LedOff, LedOff, LedOff, LedOff, LedOff); mintCounts++; } if (mintCounts >= 5) { mintCounts = 0; mint5291LEDStatus = LEDState.STATE_TEAM; } break; case STATE_BEACON: // if ((!mblnLEDON) && (mStateTime.milliseconds() > (mdblLastOff + 500))) { mdblLastOn = mStateTime.milliseconds(); mblnLEDON = true; if (mColour == Constants.BeaconColours.BEACON_BLUE_RED) { //means red is to the right LedState(LedOff, LedOff, LedOn, LedOff, LedOff, LedOff); } else if (mColour == Constants.BeaconColours.BEACON_RED_BLUE) { LedState(LedOff, LedOn, LedOff, LedOff, LedOff, LedOff); } else if (mColour == Constants.BeaconColours.BEACON_BLUE) { LedState(LedOn, LedOff, LedOff, LedOff, LedOff, LedOff); } else if (mColour == Constants.BeaconColours.BEACON_BLUE) { LedState(LedOff, LedOn, LedOff, LedOff, LedOff, LedOff); } } else if ((mblnLEDON) && (mStateTime.milliseconds() > (mdblLastOn + 500))) { mdblLastOff = mStateTime.milliseconds(); mblnLEDON = false; if (mColour == Constants.BeaconColours.BEACON_BLUE_RED) { //means red is to the right LedState(LedOff, LedOff, LedOff, LedOff, LedOn, LedOff); } else if (mColour == Constants.BeaconColours.BEACON_RED_BLUE) { LedState(LedOff, LedOff, LedOff, LedOff, LedOff, LedOn); } else if (mColour == Constants.BeaconColours.BEACON_BLUE) { LedState(LedOff, LedOff, LedOff, LedOn, LedOff, LedOff); } else if (mColour == Constants.BeaconColours.BEACON_BLUE) { LedState(LedOff, LedOff, LedOff, LedOff, LedOn, LedOff); } mintCounts++; } if (mintCounts >= 10) { mintCounts = 0; mint5291LEDStatus = LEDState.STATE_TEAM; } break; case STATE_FINISHED: //Solid Green LedState(LedOn, LedOff, LedOff, LedOn, LedOff, LedOff); break; } telemetry.update(); } //opmode not active anymore }
From source file:cmib_4_4.Countour.java
public static void main(String args[]) { System.loadLibrary(Core.NATIVE_LIBRARY_NAME); Mat image = Highgui.imread("input1.jpg", Highgui.CV_LOAD_IMAGE_GRAYSCALE); Mat image1 = Highgui.imread("input1.jpg", Highgui.CV_LOAD_IMAGE_GRAYSCALE); Mat image4 = Highgui.imread("input1.jpg"); Imgproc.threshold(image1, image1, 0, 255, THRESH_OTSU); Imgproc.Canny(image1, image1, Imgproc.THRESH_BINARY_INV + Imgproc.THRESH_OTSU, Imgproc.THRESH_BINARY_INV + Imgproc.THRESH_OTSU); Mat image2 = Mat.zeros(image.rows() + 2, image.cols() + 2, CV_8U); List<MatOfPoint> contours = new ArrayList<MatOfPoint>(); Imgproc.findContours(image1, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE); for (int i = 0; i < contours.size(); i++) { if (Imgproc.contourArea(contours.get(i)) > 100) { Rect rect = Imgproc.boundingRect(contours.get(i)); Imgproc.floodFill(image1, image2, new Point(150, 150), new Scalar(255)); Rect rectCrop = new Rect(rect.x, rect.y, rect.width, rect.height); Mat image_roi_rgb = new Mat(image4, rectCrop); Highgui.imwrite("crop2.jpg", image_roi_rgb); if (rect.height > 28) { Core.rectangle(image, new Point(rect.x, rect.y), new Point(rect.x + rect.width, rect.y + rect.height), new Scalar(0, 0, 255)); }// w ww .ja v a2s.com } } Highgui.imwrite("falciparum2.jpg", image); }
From source file:com.astrocytes.core.operationsengine.CoreOperations.java
License:Open Source License
/** * Applies morphological erosion.//from ww w . ja v a2 s .c o m * * @param src - source image. * @param radius - radius of structure element. * @return eroded image. */ public static Mat erode(Mat src, int radius) { Mat dest = new Mat(); int kernelSize = radius * 2 + 1; Mat kernel = getStructuringElement(Imgproc.CV_SHAPE_ELLIPSE, new Size(kernelSize, kernelSize), new Point(radius, radius)); Imgproc.erode(src, dest, kernel); return dest; }