From 80078dbe6e3c2c188a8e5497a8854645e462ade6 Mon Sep 17 00:00:00 2001 From: Matthew Morley Date: Sun, 15 Oct 2023 13:43:44 -0400 Subject: [PATCH] Create opencv DNN pipeline --- .../dashboard/CameraAndPipelineSelectCard.vue | 12 +- photon-client/src/types/PipelineTypes.ts | 19 +- photon-client/src/types/WebsocketDataTypes.ts | 3 +- .../vision/pipeline/CVPipelineSettings.java | 3 +- .../vision/pipeline/DnnPipeline.java | 273 ++++++++++++++++++ .../vision/pipeline/DnnPipelineSettings.java | 24 ++ .../vision/pipeline/PipelineType.java | 3 +- .../vision/processes/PipelineManager.java | 11 +- .../vision/target/TrackedTarget.java | 31 ++ 9 files changed, 370 insertions(+), 9 deletions(-) create mode 100644 photon-core/src/main/java/org/photonvision/vision/pipeline/DnnPipeline.java create mode 100644 photon-core/src/main/java/org/photonvision/vision/pipeline/DnnPipelineSettings.java diff --git a/photon-client/src/components/dashboard/CameraAndPipelineSelectCard.vue b/photon-client/src/components/dashboard/CameraAndPipelineSelectCard.vue index adb0fa846a..4580b9f746 100644 --- a/photon-client/src/components/dashboard/CameraAndPipelineSelectCard.vue +++ b/photon-client/src/components/dashboard/CameraAndPipelineSelectCard.vue @@ -24,6 +24,9 @@ const changeCurrentCameraIndex = (index: number) => { case PipelineType.Aruco: pipelineType.value = WebsocketPipelineType.Aruco; break; + case PipelineType.Dnn: + pipelineType.value = WebsocketPipelineType.Dnn; + break; } }; @@ -151,8 +154,9 @@ const pipelineTypesWrapper = computed<{ name: string; value: number }[]>(() => { const pipelineTypes = [ { name: "Reflective", value: WebsocketPipelineType.Reflective }, { name: "Colored Shape", value: WebsocketPipelineType.ColoredShape }, - { name: "AprilTag", value: WebsocketPipelineType.AprilTag } + { name: "AprilTag", value: WebsocketPipelineType.AprilTag }, // { name: "Aruco", value: WebsocketPipelineType.Aruco } + { name: "DNN", value: WebsocketPipelineType.Dnn } ]; if (useCameraSettingsStore().isDriverMode) { @@ -203,6 +207,9 @@ useCameraSettingsStore().$subscribe((mutation, state) => { case PipelineType.Aruco: pipelineType.value = WebsocketPipelineType.Aruco; break; + case PipelineType.Dnn: + pipelineType.value = WebsocketPipelineType.Dnn; + break; } }); @@ -322,8 +329,9 @@ useCameraSettingsStore().$subscribe((mutation, state) => { :items="[ { name: 'Reflective', value: WebsocketPipelineType.Reflective }, { name: 'Colored Shape', value: WebsocketPipelineType.ColoredShape }, - { name: 'AprilTag', value: WebsocketPipelineType.AprilTag } + { name: 'AprilTag', value: WebsocketPipelineType.AprilTag }, // { name: 'Aruco', value: WebsocketPipelineType.Aruco } + { name: 'Dnn', value: WebsocketPipelineType.Dnn } ]" /> diff --git a/photon-client/src/types/PipelineTypes.ts b/photon-client/src/types/PipelineTypes.ts index 2f2a090316..4b58528169 100644 --- a/photon-client/src/types/PipelineTypes.ts +++ b/photon-client/src/types/PipelineTypes.ts @@ -5,7 +5,8 @@ export enum PipelineType { Reflective = 2, ColoredShape = 3, AprilTag = 4, - Aruco = 5 + Aruco = 5, + Dnn = 6 } export enum AprilTagFamily { @@ -239,6 +240,16 @@ export const DefaultAprilTagPipelineSettings: AprilTagPipelineSettings = { tagFamily: AprilTagFamily.Family16h5 }; +export interface DnnPipelineSettings extends PipelineSettings { + pipelineType: PipelineType.Dnn; +} +export type ConfigurableDnnPipelineSettings = Partial> & + ConfigurablePipelineSettings; +export const DefaultDnnPipelineSettings: DnnPipelineSettings = { + ...DefaultPipelineSettings, + pipelineType: PipelineType.Dnn +}; + export interface ArucoPipelineSettings extends PipelineSettings { pipelineType: PipelineType.Aruco; decimate: number; @@ -269,9 +280,11 @@ export type ActivePipelineSettings = | ReflectivePipelineSettings | ColoredShapePipelineSettings | AprilTagPipelineSettings - | ArucoPipelineSettings; + | ArucoPipelineSettings + | DnnPipelineSettings; export type ActiveConfigurablePipelineSettings = | ConfigurableReflectivePipelineSettings | ConfigurableColoredShapePipelineSettings | ConfigurableAprilTagPipelineSettings - | ConfigurableArucoPipelineSettings; + | ConfigurableArucoPipelineSettings + | ConfigurableDnnPipelineSettings; diff --git a/photon-client/src/types/WebsocketDataTypes.ts b/photon-client/src/types/WebsocketDataTypes.ts index 0a31efb769..ecd89dc335 100644 --- a/photon-client/src/types/WebsocketDataTypes.ts +++ b/photon-client/src/types/WebsocketDataTypes.ts @@ -119,5 +119,6 @@ export enum WebsocketPipelineType { Reflective = 0, ColoredShape = 1, AprilTag = 2, - Aruco = 3 + Aruco = 3, + Dnn = 4 } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java index f836e30999..6ead92a0e8 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java @@ -32,7 +32,8 @@ @JsonSubTypes.Type(value = ReflectivePipelineSettings.class), @JsonSubTypes.Type(value = DriverModePipelineSettings.class), @JsonSubTypes.Type(value = AprilTagPipelineSettings.class), - @JsonSubTypes.Type(value = ArucoPipelineSettings.class) + @JsonSubTypes.Type(value = ArucoPipelineSettings.class), + @JsonSubTypes.Type(value = DnnPipelineSettings.class) }) public class CVPipelineSettings implements Cloneable { public int pipelineIndex = 0; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/DnnPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/DnnPipeline.java new file mode 100644 index 0000000000..f6156c79a8 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/DnnPipeline.java @@ -0,0 +1,273 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision.vision.pipeline; + +import java.util.ArrayList; +import java.util.List; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.MatOfFloat; +import org.opencv.core.MatOfInt; +import org.opencv.core.MatOfRect2d; +import org.opencv.core.Point; +import org.opencv.core.Rect2d; +import org.opencv.core.Scalar; +import org.opencv.core.Size; +import org.opencv.dnn.Dnn; +import org.opencv.dnn.Net; +import org.opencv.imgproc.Imgproc; +import org.opencv.utils.Converters; +import org.photonvision.common.util.ColorHelper; +import org.photonvision.vision.frame.Frame; +import org.photonvision.vision.frame.FrameThresholdType; +import org.photonvision.vision.pipe.impl.*; +import org.photonvision.vision.pipeline.result.CVPipelineResult; +import org.photonvision.vision.target.TrackedTarget; +import org.photonvision.vision.target.TrackedTarget.TargetCalculationParameters; + +public class DnnPipeline extends CVPipeline { + private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe(); + + private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE; + + public DnnPipeline() { + super(PROCESSING_TYPE); + settings = new DnnPipelineSettings(); + } + + Net net = null; + private List outBlobNames = List.of(); + + private List coco_names; + + public DnnPipeline(DnnPipelineSettings settings) { + super(PROCESSING_TYPE); + this.settings = settings; + + // Downloaded from https://dev.to/kojix2/yolov7-object-detection-in-ruby-in-10-minutes-5cjh + // https://s3.ap-northeast-2.wasabisys.com/pinto-model-zoo/307_YOLOv7/with-postprocess/resources_post.tar.gz + try { + // this.net = Dnn.readNetFromONNX("/home/matt/Downloads/best_1.onnx"); + // this.net = Dnn.readNet("/home/matt/Downloads/yolov7_post_640x640.onnx"); + this.net = + Dnn.readNetFromDarknet( + "/home/matt/Downloads/yolov4-csp-swish.cfg", + "/home/matt/Downloads/yolov4-csp-swish.weights"); + Core.setNumThreads(8); + } catch (Exception e) { + System.out.println(e); + } + this.outBlobNames = getOutputNames(net); + + this.coco_names = + List.of( + "person", + "bicycle", + "car", + "motorcycle", + "airplane", + "bus", + "train", + "truck", + "boat", + "traffic light", + "fire hydrant", + "stop sign", + "parking meter", + "bench", + "bird", + "cat", + "dog", + "horse", + "sheep", + "cow", + "elephant", + "bear", + "zebra", + "giraffe", + "backpack", + "umbrella", + "handbag", + "tie", + "suitcase", + "frisbee", + "skis", + "snowboard", + "sports ball", + "kite", + "baseball bat", + "baseball glove", + "skateboard", + "surfboard", + "tennis racket", + "bottle", + "wine glass", + "cup", + "fork", + "knife", + "spoon", + "bowl", + "banana", + "apple", + "sandwich", + "orange", + "broccoli", + "carrot", + "hot dog", + "pizza", + "donut", + "cake", + "chair", + "couch", + "potted plant", + "bed", + "dining table", + "toilet", + "tv", + "laptop", + "mouse", + "remote", + "keyboard", + "cell phone", + "microwave", + "oven", + "toaster", + "sink", + "refrigerator", + "book", + "clock", + "vase", + "scissors", + "teddy bear", + "hair drier", + "toothbrush"); + } + + @Override + protected void setPipeParamsImpl() {} + + private static List getOutputNames(Net net) { + List names = new ArrayList<>(); + + List outLayers = net.getUnconnectedOutLayers().toList(); + List layersNames = net.getLayerNames(); + + outLayers.forEach( + (item) -> names.add(layersNames.get(item - 1))); // unfold and create R-CNN layers from the + // loaded YOLO model// + return names; + } + + @Override + protected CVPipelineResult process(Frame input_frame, DnnPipelineSettings settings) { + long sumPipeNanosElapsed = 0L; + + // ====================== + + var frame = input_frame.colorImage.getMat(); + + if (frame.empty()) { + return new CVPipelineResult(sumPipeNanosElapsed, 0, List.of(), input_frame); + } + + var blob = Dnn.blobFromImage(frame, 1.0 / 255.0, new Size(640, 640)); + net.setInput(blob); + + List result = new ArrayList<>(); + net.forward(result, outBlobNames); // outputlayer : output1 and output2 + + // From https://github.com/suddh123/YOLO-object-detection-in-java/blob/code/yolo.java + + float confThreshold = 0.3f; // Insert thresholding beyond which the model will detect objects// + List clsIds = new ArrayList<>(); + List confs = new ArrayList<>(); + List rects = new ArrayList<>(); + for (int i = 0; i < result.size(); ++i) { + // each row is a candidate detection, the 1st 4 numbers are + // [center_x, center_y, width, height], followed by (N-4) class probabilities + Mat level = result.get(i); + for (int j = 0; j < level.rows(); ++j) { + Mat row = level.row(j); + Mat scores = row.colRange(5, level.cols()); + Core.MinMaxLocResult mm = Core.minMaxLoc(scores); + float confidence = (float) mm.maxVal; + Point classIdPoint = mm.maxLoc; + if (confidence > confThreshold) { + // scaling for drawing the bounding boxes// + int centerX = (int) (row.get(0, 0)[0] * frame.cols()); + int centerY = (int) (row.get(0, 1)[0] * frame.rows()); + int width = (int) (row.get(0, 2)[0] * frame.cols()); + int height = (int) (row.get(0, 3)[0] * frame.rows()); + int left = centerX - width / 2; + int top = centerY - height / 2; + + clsIds.add((int) classIdPoint.x); + confs.add((float) confidence); + rects.add(new Rect2d(left, top, width, height)); + } + } + } + float nmsThresh = 0.5f; + MatOfFloat confidences = new MatOfFloat(Converters.vector_float_to_Mat(confs)); + Rect2d[] boxesArray = rects.toArray(new Rect2d[0]); + MatOfRect2d boxes = new MatOfRect2d(boxesArray); + MatOfInt indices = new MatOfInt(); + Dnn.NMSBoxes( + boxes, + confidences, + confThreshold, + nmsThresh, + indices); // We draw the bounding boxes for objects + // here// + + List targetList = new ArrayList<>(); + + int[] ind = indices.toArray(); + for (int i = 0; i < ind.length; ++i) { + int idx = ind[i]; + var box = boxesArray[idx]; + Imgproc.rectangle(frame, box.tl(), box.br(), new Scalar(0, 0, 255), 2); + + var name = String.format("%s (%f)", coco_names.get(clsIds.get(idx)), confs.get(idx)); + + Imgproc.putText( + frame, + name, + new Point(box.x + box.width / 2.0, box.y + box.height / 2.0), + 0, + 0.6, + ColorHelper.colorToScalar(java.awt.Color.white), + 2); + + targetList.add( + new TrackedTarget( + box, + clsIds.get(idx), + confs.get(idx), + new TargetCalculationParameters( + false, null, null, null, null, frameStaticProperties))); + } + + // ====================== + + var fpsResult = calculateFPSPipe.run(null); + var fps = fpsResult.output; + + return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, input_frame); + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/DnnPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/DnnPipelineSettings.java new file mode 100644 index 0000000000..e5ae95a7bc --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/DnnPipelineSettings.java @@ -0,0 +1,24 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision.vision.pipeline; + +public class DnnPipelineSettings extends CVPipelineSettings { + public DnnPipelineSettings() { + this.pipelineType = PipelineType.Dnn; + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/PipelineType.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/PipelineType.java index a2f6346b89..c050a55d6f 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/PipelineType.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/PipelineType.java @@ -24,7 +24,8 @@ public enum PipelineType { Reflective(0, ReflectivePipeline.class), ColoredShape(1, ColoredShapePipeline.class), AprilTag(2, AprilTagPipeline.class), - Aruco(3, ArucoPipeline.class); + Aruco(3, ArucoPipeline.class), + Dnn(4, DnnPipeline.class); public final int baseIndex; public final Class clazz; diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java index e4f6df3a97..c6d55211cd 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java @@ -205,11 +205,14 @@ private void setPipelineInternal(int newIndex) { currentUserPipeline = new AprilTagPipeline((AprilTagPipelineSettings) desiredPipelineSettings); break; - case Aruco: logger.debug("Creating Aruco Pipeline"); currentUserPipeline = new ArucoPipeline((ArucoPipelineSettings) desiredPipelineSettings); break; + case Dnn: + logger.debug("Creating DNN Pipeline"); + currentUserPipeline = new DnnPipeline((DnnPipelineSettings) desiredPipelineSettings); + break; default: // Can be calib3d or drivermode, both of which are special cases break; @@ -311,6 +314,12 @@ private CVPipelineSettings createSettingsForType(PipelineType type, String nickn added.pipelineNickname = nickname; return added; } + case Dnn: + { + var added = new DnnPipelineSettings(); + added.pipelineNickname = nickname; + return added; + } default: { logger.error("Got invalid pipeline type: " + type); diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java index 72c5cdf600..b0b89273b7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java @@ -187,6 +187,37 @@ public TrackedTarget(ArucoDetectionResult result, TargetCalculationParameters pa } } + public TrackedTarget( + Rect2d box, int class_id, double confidence, TargetCalculationParameters params) { + m_targetOffsetPoint = new Point(box.x, box.y); + m_robotOffsetPoint = new Point(); + + m_pitch = + TargetCalculations.calculatePitch( + box.y + box.height / 2.0, params.cameraCenterPoint.y, params.verticalFocalLength); + m_yaw = + TargetCalculations.calculateYaw( + box.x + box.width / 2.0, params.cameraCenterPoint.x, params.horizontalFocalLength); + + Point[] cornerPoints = + new Point[] { + // Box.x/y is the top-left corner, not the center + new Point(box.x, box.y), // tl + new Point(box.x + box.width, box.y), // tr + new Point(box.x + box.width, box.y + box.height), // br + new Point(box.x, box.y + box.height), // bl + }; + + m_targetCorners = List.of(cornerPoints); + MatOfPoint contourMat = new MatOfPoint(cornerPoints); + m_approximateBoundingPolygon = new MatOfPoint2f(cornerPoints); + + m_mainContour = new Contour(contourMat); + m_area = m_mainContour.getArea() / params.imageArea * 100; + + m_fiducialId = class_id; + } + public void setFiducialId(int id) { m_fiducialId = id; }