|
| 1 | +/* |
| 2 | + * Copyright (c) 2024 Phil Malone |
| 3 | + * |
| 4 | + * Permission is hereby granted, free of charge, to any person obtaining a copy |
| 5 | + * of this software and associated documentation files (the "Software"), to deal |
| 6 | + * in the Software without restriction, including without limitation the rights |
| 7 | + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
| 8 | + * copies of the Software, and to permit persons to whom the Software is |
| 9 | + * furnished to do so, subject to the following conditions: |
| 10 | + * |
| 11 | + * The above copyright notice and this permission notice shall be included in all |
| 12 | + * copies or substantial portions of the Software. |
| 13 | + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
| 14 | + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
| 15 | + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
| 16 | + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
| 17 | + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
| 18 | + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE |
| 19 | + * SOFTWARE. |
| 20 | + */ |
| 21 | + |
| 22 | +package org.firstinspires.ftc.robotcontroller.external.samples; |
| 23 | + |
| 24 | +import android.graphics.Color; |
| 25 | +import android.util.Size; |
| 26 | + |
| 27 | +import com.qualcomm.robotcore.eventloop.opmode.Disabled; |
| 28 | +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; |
| 29 | +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; |
| 30 | + |
| 31 | +import org.firstinspires.ftc.robotcore.external.Telemetry; |
| 32 | +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; |
| 33 | +import org.firstinspires.ftc.vision.VisionPortal; |
| 34 | +import org.firstinspires.ftc.vision.opencv.Circle; |
| 35 | +import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; |
| 36 | +import org.firstinspires.ftc.vision.opencv.ColorRange; |
| 37 | +import org.firstinspires.ftc.vision.opencv.ImageRegion; |
| 38 | + |
| 39 | +import java.util.List; |
| 40 | + |
| 41 | +/* |
| 42 | + * This OpMode illustrates how to use a video source (camera) to locate specifically colored regions. |
| 43 | + * This sample is targeted towards circular blobs. To see rectangles, look at ConceptVisionColorLocator_Rectangle |
| 44 | + * |
| 45 | + * Unlike a "color sensor" which determines the color of nearby object, this "color locator" |
| 46 | + * will search the Region Of Interest (ROI) in a camera image, and find any "blobs" of color that |
| 47 | + * match the requested color range. These blobs can be further filtered and sorted to find the one |
| 48 | + * most likely to be the item the user is looking for. |
| 49 | + * |
| 50 | + * To perform this function, a VisionPortal runs a ColorBlobLocatorProcessor process. |
| 51 | + * The ColorBlobLocatorProcessor (CBLP) process is created first, and then the VisionPortal is built. |
| 52 | + * The (CBLP) analyses the ROI and locates pixels that match the ColorRange to form a "mask". |
| 53 | + * The matching pixels are then collected into contiguous "blobs" of pixels. |
| 54 | + * The outer boundaries of these blobs are called its "contour". For each blob, the process then |
| 55 | + * creates the smallest possible circle that will fully encase the contour. The user can then call |
| 56 | + * getBlobs() to retrieve the list of Blobs, where each contains the contour and the circle. |
| 57 | + * Note: The default sort order for Blobs is ContourArea, in descending order, so the biggest |
| 58 | + * contours are listed first. |
| 59 | + * |
| 60 | + * A colored enclosing circle is drawn on the camera preview to show the location of each Blob |
| 61 | + * The original Blob contour can also be added to the preview. |
| 62 | + * This is helpful when configuring the ColorBlobLocatorProcessor parameters. |
| 63 | + * |
| 64 | + * Tip: Connect an HDMI monitor to the Control Hub to view the Color Location process in real-time. |
| 65 | + * Or use a screen copy utility like ScrCpy.exe to view the video remotely. |
| 66 | + * |
| 67 | + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. |
| 68 | + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list |
| 69 | + */ |
| 70 | + |
| 71 | +@Disabled |
| 72 | +@TeleOp(name = "Concept: Vision Color-Locator (Circle)", group = "Concept") |
| 73 | +public class ConceptVisionColorLocator_Circle extends LinearOpMode { |
| 74 | + @Override |
| 75 | + public void runOpMode() { |
| 76 | + /* Build a "Color Locator" vision processor based on the ColorBlobLocatorProcessor class. |
| 77 | + * - Specify the color range you are looking for. Use a predefined color, or create your own |
| 78 | + * |
| 79 | + * .setTargetColorRange(ColorRange.BLUE) // use a predefined color match |
| 80 | + * Available colors are: RED, BLUE, YELLOW, GREEN, ARTIFACT_GREEN, ARTIFACT_PURPLE |
| 81 | + * .setTargetColorRange(new ColorRange(ColorSpace.YCrCb, // or define your own color match |
| 82 | + * new Scalar( 32, 176, 0), |
| 83 | + * new Scalar(255, 255, 132))) |
| 84 | + * |
| 85 | + * - Focus the color locator by defining a RegionOfInterest (ROI) which you want to search. |
| 86 | + * This can be the entire frame, or a sub-region defined using: |
| 87 | + * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system. |
| 88 | + * Use one form of the ImageRegion class to define the ROI. |
| 89 | + * ImageRegion.entireFrame() |
| 90 | + * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixels at upper left corner |
| 91 | + * ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5) 50% width/height in center |
| 92 | + * |
| 93 | + * - Define which contours are included. |
| 94 | + * You can get ALL the contours, ignore contours that are completely inside another contour. |
| 95 | + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.ALL_FLATTENED_HIERARCHY) |
| 96 | + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) |
| 97 | + * EXTERNAL_ONLY helps to avoid bright reflection spots from breaking up solid colors. |
| 98 | + * |
| 99 | + * - Turn the displays of contours ON or OFF. |
| 100 | + * Turning these on helps debugging but takes up valuable CPU time. |
| 101 | + * .setDrawContours(true) Draws an outline of each contour. |
| 102 | + * .setEnclosingCircleColor(int color) Draws a circle around each contour. 0 to disable. |
| 103 | + * .setBoxFitColor(int color) Draws a rectangle around each contour. 0 to disable. ON by default. |
| 104 | + * |
| 105 | + * |
| 106 | + * - include any pre-processing of the image or mask before looking for Blobs. |
| 107 | + * There are some extra processing you can include to improve the formation of blobs. |
| 108 | + * Using these features requires an understanding of how they may effect the final |
| 109 | + * blobs. The "pixels" argument sets the NxN kernel size. |
| 110 | + * .setBlurSize(int pixels) |
| 111 | + * Blurring an image helps to provide a smooth color transition between objects, |
| 112 | + * and smoother contours. The higher the number, the more blurred the image becomes. |
| 113 | + * Note: Even "pixels" values will be incremented to satisfy the "odd number" requirement. |
| 114 | + * Blurring too much may hide smaller features. A size of 5 is good for a 320x240 image. |
| 115 | + * |
| 116 | + * .setErodeSize(int pixels) |
| 117 | + * Erosion removes floating pixels and thin lines so that only substantive objects remain. |
| 118 | + * Erosion can grow holes inside regions, and also shrink objects. |
| 119 | + * "pixels" in the range of 2-4 are suitable for low res images. |
| 120 | + * |
| 121 | + * .setDilateSize(int pixels) |
| 122 | + * Dilation makes objects and lines more visible by filling in small holes, and making |
| 123 | + * filled shapes appear larger. Dilation is useful for joining broken parts of an |
| 124 | + * object, such as when removing noise from an image. |
| 125 | + * "pixels" in the range of 2-4 are suitable for low res images. |
| 126 | + * |
| 127 | + * .setMorphOperationType(MorphOperationType morphOperationType) |
| 128 | + * This defines the order in which the Erode/Dilate actions are performed. |
| 129 | + * OPENING: Will Erode and then Dilate which will make small noise blobs go away |
| 130 | + * CLOSING: Will Dilate and then Erode which will tend to fill in any small holes in blob edges. |
| 131 | + */ |
| 132 | + ColorBlobLocatorProcessor colorLocator = new ColorBlobLocatorProcessor.Builder() |
| 133 | + .setTargetColorRange(ColorRange.ARTIFACT_PURPLE) // Use a predefined color match |
| 134 | + .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) |
| 135 | + .setRoi(ImageRegion.asUnityCenterCoordinates(-0.75, 0.75, 0.75, -0.75)) |
| 136 | + .setDrawContours(true) // Show contours on the Stream Preview |
| 137 | + .setBoxFitColor(0) // Disable the drawing of rectangles |
| 138 | + .setCircleFitColor(Color.rgb(255, 255, 0)) // Draw a circle |
| 139 | + .setBlurSize(5) // Smooth the transitions between different colors in image |
| 140 | + |
| 141 | + // the following options have been added to fill in perimeter holes. |
| 142 | + .setDilateSize(15) // Expand blobs to fill any divots on the edges |
| 143 | + .setErodeSize(15) // Shrink blobs back to original size |
| 144 | + .setMorphOperationType(ColorBlobLocatorProcessor.MorphOperationType.CLOSING) |
| 145 | + |
| 146 | + .build(); |
| 147 | + /* |
| 148 | + * Build a vision portal to run the Color Locator process. |
| 149 | + * |
| 150 | + * - Add the colorLocator process created above. |
| 151 | + * - Set the desired video resolution. |
| 152 | + * Since a high resolution will not improve this process, choose a lower resolution |
| 153 | + * that is supported by your camera. This will improve overall performance and reduce |
| 154 | + * latency. |
| 155 | + * - Choose your video source. This may be |
| 156 | + * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam |
| 157 | + * or |
| 158 | + * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera |
| 159 | + */ |
| 160 | + VisionPortal portal = new VisionPortal.Builder() |
| 161 | + .addProcessor(colorLocator) |
| 162 | + .setCameraResolution(new Size(320, 240)) |
| 163 | + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) |
| 164 | + .build(); |
| 165 | + |
| 166 | + telemetry.setMsTransmissionInterval(100); // Speed up telemetry updates for debugging. |
| 167 | + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); |
| 168 | + |
| 169 | + // WARNING: To view the stream preview on the Driver Station, this code runs in INIT mode. |
| 170 | + while (opModeIsActive() || opModeInInit()) { |
| 171 | + telemetry.addData("preview on/off", "... Camera Stream\n"); |
| 172 | + |
| 173 | + // Read the current list |
| 174 | + List<ColorBlobLocatorProcessor.Blob> blobs = colorLocator.getBlobs(); |
| 175 | + |
| 176 | + /* |
| 177 | + * The list of Blobs can be filtered to remove unwanted Blobs. |
| 178 | + * Note: All contours will be still displayed on the Stream Preview, but only those |
| 179 | + * that satisfy the filter conditions will remain in the current list of |
| 180 | + * "blobs". Multiple filters may be used. |
| 181 | + * |
| 182 | + * To perform a filter |
| 183 | + * ColorBlobLocatorProcessor.Util.filterByCriteria(criteria, minValue, maxValue, blobs); |
| 184 | + * |
| 185 | + * The following criteria are currently supported. |
| 186 | + * |
| 187 | + * ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA |
| 188 | + * A Blob's area is the number of pixels contained within the Contour. Filter out any |
| 189 | + * that are too big or small. Start with a large range and then refine the range based |
| 190 | + * on the likely size of the desired object in the viewfinder. |
| 191 | + * |
| 192 | + * ColorBlobLocatorProcessor.BlobCriteria.BY_DENSITY |
| 193 | + * A blob's density is an indication of how "full" the contour is. |
| 194 | + * If you put a rubber band around the contour you would get the "Convex Hull" of the |
| 195 | + * contour. The density is the ratio of Contour-area to Convex Hull-area. |
| 196 | + * |
| 197 | + * ColorBlobLocatorProcessor.BlobCriteria.BY_ASPECT_RATIO |
| 198 | + * A blob's Aspect ratio is the ratio of boxFit long side to short side. |
| 199 | + * A perfect Square has an aspect ratio of 1. All others are > 1 |
| 200 | + * |
| 201 | + * ColorBlobLocatorProcessor.BlobCriteria.BY_ARC_LENGTH |
| 202 | + * A blob's arc length is the perimeter of the blob. |
| 203 | + * This can be used in conjunction with an area filter to detect oddly shaped blobs. |
| 204 | + * |
| 205 | + * ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY |
| 206 | + * A blob's circularity is how circular it is based on the known area and arc length. |
| 207 | + * A perfect circle has a circularity of 1. All others are < 1 |
| 208 | + */ |
| 209 | + ColorBlobLocatorProcessor.Util.filterByCriteria( |
| 210 | + ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, |
| 211 | + 50, 20000, blobs); // filter out very small blobs. |
| 212 | + |
| 213 | + ColorBlobLocatorProcessor.Util.filterByCriteria( |
| 214 | + ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY, |
| 215 | + 0.6, 1, blobs); /* filter out non-circular blobs. |
| 216 | + * NOTE: You may want to adjust the minimum value depending on your use case. |
| 217 | + * Circularity values will be affected by shadows, and will therefore vary based |
| 218 | + * on the location of the camera on your robot and venue lighting. It is strongly |
| 219 | + * encouraged to test your vision on the competition field if your event allows |
| 220 | + * sensor calibration time. |
| 221 | + */ |
| 222 | + |
| 223 | + /* |
| 224 | + * The list of Blobs can be sorted using the same Blob attributes as listed above. |
| 225 | + * No more than one sort call should be made. Sorting can use ascending or descending order. |
| 226 | + * Here is an example.: |
| 227 | + * ColorBlobLocatorProcessor.Util.sortByCriteria( |
| 228 | + * ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, SortOrder.DESCENDING, blobs); |
| 229 | + */ |
| 230 | + |
| 231 | + telemetry.addLine("Circularity Radius Center"); |
| 232 | + |
| 233 | + // Display the Blob's circularity, and the size (radius) and center location of its circleFit. |
| 234 | + for (ColorBlobLocatorProcessor.Blob b : blobs) { |
| 235 | + |
| 236 | + Circle circleFit = b.getCircle(); |
| 237 | + telemetry.addLine(String.format("%5.3f %3d (%3d,%3d)", |
| 238 | + b.getCircularity(), (int) circleFit.getRadius(), (int) circleFit.getX(), (int) circleFit.getY())); |
| 239 | + } |
| 240 | + |
| 241 | + telemetry.update(); |
| 242 | + sleep(100); // Match the telemetry update interval. |
| 243 | + } |
| 244 | + } |
| 245 | +} |
0 commit comments