Initial AprilTag support (#458)

(Very) beta AprilTag support in PhotonVision. Disables Picam GPU acceleration until we can debug auto exposure in the MMAL driver. Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com> Co-authored-by: Matt <matthew.morley.ca@gmail.com> Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com> Co-authored-by: Chris <chrisgerth010592@gmail.com> Co-authored-by: mdurrani808 <mdurrani808@gmail.com>
2026-06-25 01:41:40 +00:00 · 2022-09-28 18:21:41 -07:00
parent a3bcd3ac4f
commit a764ace7f2
114 changed files with 21488 additions and 10851 deletions
--- a/photon-client/src/components/pipeline/3D/MiniMap.vue
+++ b/photon-client/src/components/pipeline/3D/MiniMap.vue
@@ -1,154 +1,199 @@
 <template>
-  <div>
+  <div id="MapContainer" style="flex-grow:1">
    <v-row>
      <v-col
        align="center"
        cols="12"
      >
-        <span class="white--text">Target Location</span>
-        <canvas
-          id="canvasId"
-          class="mt-2"
-          width="800"
-          height="800"
-        />
+        <span class="white--text" >Target Location</span>
+      </v-col>
+    </v-row>
+    <v-row>
+      <v-col
+        align="center"
+        cols="12"
+        align-self="stretch"
+      >
+        <canvas id="canvasId" style="width:100%;height:100%"/>
      </v-col>
    </v-row>
  </div>
 </template>

 <script>
-    import theme from "../../../theme";

-    export default {
-        name: "MiniMap",
-        props: {
-          // eslint-disable-next-line vue/require-default-prop
-          targets: Array,
-          // eslint-disable-next-line vue/require-default-prop
-          horizontalFOV: Number
-        },
-        data() {
-            return {
-                ctx: undefined,
-                canvas: undefined,
-                x: 0,
-                y: 0,
-                targetWidth: 40,
-                targetHeight: 6
-            }
-        },
-        computed: {
-            hLen: {
-                get() {
-                    return Math.tan(this.horizontalFOV / 2 * Math.PI / 180) * 150;
-                }
-            }
-        },
-        watch: {
-            targets: {
-                deep: true,
-                handler() {
-                    this.draw();
-                }
-            },
-            horizontalFOV() {
-                this.draw();
-            }
-        },
-        mounted: function () {
-            const canvas = document.getElementById("canvasId"); // getting the canvas element
-            const ctx = canvas.getContext("2d"); // getting the canvas context
-            this.canvas = canvas; // setting the canvas as a vue variable
-            this.ctx = ctx; // setting the canvas context as a vue variable
-            this.grad = this.ctx.createLinearGradient(400, 800, 400, 600);
-            this.grad.addColorStop(0, "rgb(119,119,119)");
-            this.grad.addColorStop(0.05, "rgba(14,92,22,0.96)");
-            this.grad.addColorStop(0.8, 'rgba(43,43,43,0.48)');
+import {
+  ArrowHelper,
+  BoxGeometry,
+  Mesh,
+  MeshNormalMaterial,
+  PerspectiveCamera,
+  Quaternion,
+  Scene,
+  TrackballControls,
+  Vector3,
+    Color,
+  WebGLRenderer
+} from "three-full";

-            // setting canvas context values for drawing
+export default {
+  name: "MiniMap",
+  props: {
+    // eslint-disable-next-line vue/require-default-prop
+    targets: Array,
+    // eslint-disable-next-line vue/require-default-prop
+    horizontalFOV: Number
+  },
+  data() {
+    return {
+      scene: undefined,
+      cubes: [],

-
-            this.ctx.font = "26px Arial";
-            this.ctx.strokeStyle = "whitesmoke";
-            this.ctx.lineWidth = 2;
-
-            this.$nextTick(function () {
-                this.drawPlayer();
-            });
-        },
-        methods: {
-            draw() {
-                this.clearBoard();
-                this.drawPlayer();
-                for (let index in this.targets) {
-                    this.drawTarget(index, this.targets[index].pose);
-                }
-            },
-            drawTarget(index, target) {
-                // first save the untranslated/unrotated context
-                let x = 800 - (160 * target.x); // getting meters as pixels
-                let y = 400 - (160 * target.y);
-                this.ctx.save();
-                this.ctx.beginPath();
-                // move the rotation point to the center of the rect
-                this.ctx.translate(y + this.targetWidth / 2, x + this.targetHeight / 2); // wpi lib makes x forward and back and y left to right
-                // rotate the rect
-                this.ctx.rotate(target.rot * -1 * Math.PI / 180.0);
-
-                // draw the rect on the transformed context
-                // Note: after transforming [0,0] is visually [x,y]
-                //       so the rect needs to be offset accordingly when drawn
-                this.ctx.rect(-this.targetWidth / 2, -this.targetHeight / 2, this.targetWidth, this.targetHeight);
-
-                this.ctx.fillStyle = theme.accent;
-                this.ctx.fill();
-
-                // restore the context to its untranslated/unrotated state
-                this.ctx.restore();
-                this.ctx.fillStyle = "whitesmoke";
-                this.ctx.beginPath();
-                this.ctx.arc(y + this.targetWidth / 2, x + this.targetHeight / 2, 3, 0, 2 * Math.PI, true);
-                this.ctx.fill();
-                this.ctx.fillText(index, y - 30, x - 5);
-
-            },
-            drawPlayer() {
-                this.ctx.beginPath();
-                this.ctx.moveTo(400, 820);
-                this.ctx.lineTo(400 + this.hLen, 650);
-                this.ctx.lineTo(400 - this.hLen, 650);
-                this.ctx.closePath();
-                this.ctx.fillStyle = this.grad;
-                this.ctx.fill();
-                this.ctx.beginPath();
-                this.ctx.moveTo(400, 820);
-                this.ctx.lineTo(400 + this.hLen, 650);
-                this.ctx.stroke();
-                this.ctx.moveTo(400, 820);
-                this.ctx.lineTo(400 - this.hLen, 650);
-                this.ctx.stroke();
-
-            },
-            clearBoard() {
-                this.ctx.clearRect(0, 0, this.canvas.width, this.canvas.height); // clearing the canvas
-            }
-        }
    }
+  },
+  watch: {
+    targets: {
+      deep: true,
+      handler() {
+        this.drawTargets();
+      }
+    },
+  },
+  methods: {
+    drawTargets() {
+      this.scene.remove(...this.cubes)
+      this.cubes = []
+
+      for (const target of this.targets) {
+        const geometry = new BoxGeometry(0.2, 0.2, 0.3 / 5);
+        const material = new MeshNormalMaterial();
+        let quat = (new Quaternion(
+            target.pose.qx,
+            target.pose.qy,
+            target.pose.qz,
+            target.pose.qw,
+        ))
+        const cube = new Mesh(geometry, material);
+        cube.position.set(target.pose.x, target.pose.y, target.pose.z)
+        cube.rotation.setFromQuaternion(quat);
+        this.cubes.push(cube)
+
+        let arrow = (new ArrowHelper(new Vector3(1, 0, 0).normalize(), new Vector3(0, 0, 0),
+            1, // length
+            0xff0000,
+            0.2,
+            0.2,
+        ));
+        arrow.rotation.setFromQuaternion(quat)
+        arrow.rotateZ(-Math.PI / 2)
+        arrow.position.set(target.pose.x, target.pose.y, target.pose.z)
+        this.cubes.push(arrow);
+
+        arrow = (new ArrowHelper(new Vector3(1, 0, 0).normalize(), new Vector3(0, 0, 0),
+            1, // length
+            0x00ff00,
+            0.2,
+            0.2,
+        ));
+        arrow.rotation.setFromQuaternion(quat)
+        // arrow.rotateX(Math.PI / 2)
+        arrow.position.set(target.pose.x, target.pose.y, target.pose.z)
+        this.cubes.push(arrow);
+        arrow = (new ArrowHelper(new Vector3(1, 0, 0).normalize(), new Vector3(0, 0, 0),
+            1, // length
+            0x0000ff,
+            0.2,
+            0.2,
+        ));
+        arrow.setRotationFromQuaternion(quat)
+        arrow.rotateX(Math.PI / 2)
+        arrow.position.set(target.pose.x, target.pose.y, target.pose.z)
+        this.cubes.push(arrow);
+      }
+      if(this.cubes.length > 0)
+        this.scene.add(...this.cubes);
+    },
+    onWindowResize() {
+      var container = document.getElementById("MapContainer")
+      if(container){
+        this.canvas.width = container.clientWidth * 0.95;
+        this.canvas.height = container.clientWidth * 0.85;
+        this.camera.aspect = this.canvas.width / this.canvas.height;
+        this.camera.updateProjectionMatrix();
+        this.renderer.setSize( this.canvas.width, this.canvas.height );
+      }
+    },
+  },
+
+  mounted() {
+    const scene = new Scene();
+    this.scene = scene;
+    const camera = new PerspectiveCamera(75, 800 / 800, 0.1, 1000);
+    this.camera = camera;
+
+    const canvas = document.getElementById("canvasId"); // getting the canvas element
+    this.canvas = canvas;
+    const renderer = new WebGLRenderer({"canvas": canvas});
+    this.renderer = renderer;
+    scene.background = new Color(0xa9a9a9)
+
+
+    this.onWindowResize();
+    window.addEventListener( 'resize', this.onWindowResize, false );
+
+    scene.add(new ArrowHelper(new Vector3(1, 0, 0).normalize(), new Vector3(0, 0, 0),
+        1, // length
+        0xff0000,
+        0.5,
+        0.5,
+    ))
+    scene.add(new ArrowHelper(new Vector3(0, 1, 0).normalize(), new Vector3(0, 0, 0),
+        1, // length
+        0x00ff00,
+        0.5,
+        0.5,
+    ))
+    scene.add(new ArrowHelper(new Vector3(0, 0, 1).normalize(), new Vector3(0, 0, 0),
+        1, // length
+        0x0000ff,
+        0.5,
+        0.5,
+    ))
+
+    var controls = new TrackballControls(
+        camera,
+        renderer.domElement
+    );
+    controls.rotateSpeed = 1.0;
+    controls.zoomSpeed = 1.2;
+    controls.panSpeed = 0.8;
+    controls.noZoom = false;
+    controls.noPan = false;
+    controls.staticMoving = true;
+    controls.dynamicDampingFactor = 0.3;
+    controls.keys = [65, 83, 68];
+
+
+    camera.position.set(-0.1,0,0);
+    camera.rotation.set(-90, 0, 90);
+    camera.up.set(0,0,1);
+    controls.update();
+
+    function animate() {
+      requestAnimationFrame(animate);
+
+      controls.update();
+      renderer.render(scene, camera);
+    }
+
+    this.drawTargets()
+
+    animate();
+  }
+
+
+}
 </script>

 <style scoped>
-    #canvasId {
-        width: 400px;
-        height: 400px;
-        background-color: #232C37;
-        border-radius: 5px;
-        border: 2px solid grey;
-        box-shadow: 0 0 5px 1px;
-    }
-
-    th {
-        width: 80px;
-        text-align: center;
-    }
 </style>
--- a/photon-client/src/components/pipeline/CameraAndPipelineSelect.vue
+++ b/photon-client/src/components/pipeline/CameraAndPipelineSelect.vue
@@ -153,7 +153,7 @@
          v-model="currentPipelineType"
          name="Type"
          tooltip="Changes the pipeline type, which changes the type of processing that will happen on input frames"
-          :list="['Reflective Tape', 'Colored Shape']"
+          :list="['Reflective Tape', 'Colored Shape', 'AprilTag']"
          @input="e => showTypeDialog(e)"
        />
      </v-col>
--- a/photon-client/src/store/index.js
+++ b/photon-client/src/store/index.js
@@ -51,7 +51,7 @@ export default new Vuex.Store({
                isFovConfigurable: true,
                calibrated: false,
                currentPipelineSettings: {
-                    pipelineType: 2, // One of "calib", "driver", "reflective", "shape"
+                    pipelineType: 4, // One of "calib", "driver", "reflective", "shape", "AprilTag"
                    // 2 is reflective

                    // Settings that apply to all pipeline types
@@ -88,7 +88,13 @@ export default new Vuex.Store({

                    cornerDetectionAccuracyPercentage: 10,

-                    // Settings that apply to shape
+                    // Settings that apply to AprilTag
+                    tagFamily: 0,
+                    decimate: 1.0,
+                    blur: 0.0,
+                    threads: 1,
+                    debug: false,
+                    refineEdges: true
                }
            }
        ],
@@ -102,9 +108,18 @@ export default new Vuex.Store({
                    skew: 0,
                    area: 0,
                    // 3D only
-                    pose: {x: 0, y: 0, rot: 0},
-                }]
-            },
+                    pose: {x: 1, y: 1, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+                },
+            {
+                // Available in both 2D and 3D
+                pitch: 0,
+                yaw: 0,
+                skew: 0,
+                area: 0,
+                // 3D only
+                pose: {x: 2, y: 3, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+            }]
+        },
        settings: {
            general: {
                version: "Unknown",
--- a/photon-client/src/views/PipelineView.vue
+++ b/photon-client/src/views/PipelineView.vue
@@ -34,7 +34,7 @@
                :text-color="fpsTooLow ? 'white' : 'grey'"
              >
                <span class="pr-1">{{ Math.round($store.state.pipelineResults.fps) }}&nbsp;FPS &ndash;</span>
-                <span v-if="!fpsTooLow">{{ Math.min(Math.round($store.state.pipelineResults.latency), 100) }} ms latency</span>
+                <span v-if="!fpsTooLow">{{ Math.min(Math.round($store.state.pipelineResults.latency), 9999) }} ms latency</span>
                <span v-else-if="!$store.getters.currentPipelineSettings.inputShouldShow">HSV thresholds are too broad; narrow them for better performance</span>
                <span v-else>stop viewing the color stream for better performance</span>
              </v-chip>
@@ -175,7 +175,7 @@
              slider-color="accent"
            >
              <v-tab
-                v-for="(tab, i) in tabs.filter(it => it.name !== '3D' || $store.getters.currentPipelineSettings.solvePNPEnabled)"
+                v-for="(tab, i) in tabs"
                :key="i"
              >
                {{ tab.name }}
@@ -262,6 +262,7 @@ import ContoursTab from './PipelineViews/ContoursTab';
 import OutputTab from './PipelineViews/OutputTab';
 import TargetsTab from "./PipelineViews/TargetsTab";
 import PnPTab from './PipelineViews/PnPTab';
+import AprilTagTab from './PipelineViews/AprilTagTab';

 export default {
    name: 'Pipeline',
@@ -274,6 +275,7 @@ export default {
        OutputTab,
        TargetsTab,
        PnPTab,
+        AprilTagTab,
    },
    data() {
        return {
@@ -308,6 +310,10 @@ export default {
                        name: "Contours",
                        component: "ContoursTab",
                    },
+                    apriltag: {
+                        name: "AprilTag",
+                        component: "AprilTagTab",
+                    },
                    output: {
                        name: "Output",
                        component: "OutputTab",
@@ -322,6 +328,11 @@ export default {
                    }
                };

+                // If not in 3d, name "3D" is illegal
+                const allow3d = this.$store.getters.currentPipelineSettings.solvePNPEnabled;
+                // If in apriltag, "Threshold" and "Contours" are illegal -- otherwise "AprilTag" is
+                const isAprilTag = (this.$store.getters.currentPipelineSettings.pipelineType - 2) === 2;
+
                // 2D array of tab names and component names; each sub-array is a separate tab group
                let ret = [];
                if (this.$vuetify.breakpoint.smAndDown || this.$store.getters.isDriverMode || (this.$vuetify.breakpoint.mdAndDown && !this.$store.state.compactMode)) {
@@ -329,22 +340,36 @@ export default {
                    ret[0] = Object.values(tabs);
                } else if (this.$vuetify.breakpoint.mdAndDown || !this.$store.state.compactMode) {
                    // Two tab groups, one with "input, threshold, contours, output" and the other with "target info, 3D"
-                    ret[0] = [tabs.input, tabs.threshold, tabs.contours, tabs.output];
+                    ret[0] = [tabs.input, tabs.threshold, tabs.contours, tabs.apriltag, tabs.output];
                    ret[1] = [tabs.targets, tabs.pnp];
                } else if (this.$vuetify.breakpoint.lgAndDown) {
                    // Three tab groups, one with "input", one with "threshold, contours, output", and the other with "target info, 3D"
                    ret[0] = [tabs.input];
-                    ret[1] = [tabs.threshold, tabs.contours, tabs.output];
+                    ret[1] = [tabs.threshold, tabs.contours, tabs.apriltag, tabs.output];
                    ret[2] = [tabs.targets, tabs.pnp];
                } else if (this.$vuetify.breakpoint.xl) {
                    // Three tab groups, one with "input", one with "threshold, contours", and the other with "output, target info, 3D"
                    ret[0] = [tabs.input];
                    ret[1] = [tabs.threshold];
-                    ret[2] = [tabs.contours, tabs.output];
+                    ret[2] = [tabs.contours, tabs.apriltag, tabs.output];
                    ret[3] = [tabs.targets, tabs.pnp];
                }

-                return ret;
+                for(let i = 0; i < ret.length; i++) {
+                  const group = ret[i];
+
+                  // All the tabs we allow
+                  const filteredGroup = group.filter(it =>
+                          !(!allow3d && it.name === "3D")
+                       && !(isAprilTag && (it.name === "Threshold"))
+                       && !(isAprilTag && (it.name === "Contours"))
+                       && !(!isAprilTag && it.name === "AprilTag")
+                      );
+                  ret[i] = filteredGroup;
+                }
+
+                // One last filter to remove empty lists
+                return ret.filter(it => it !== undefined && it.length > 0);
            }
        },
        processingMode: {
--- a/photon-client/src/views/PipelineViews/AprilTagTab.vue
+++ b/photon-client/src/views/PipelineViews/AprilTagTab.vue
@@ -0,0 +1,113 @@
+<template>
+  <div>
+    <v-select
+      v-model="selectedFamily"
+      dark
+      color="accent"
+      item-color="secondary"
+      label="Select target family"
+      :items="familyList"
+      @input="handlePipelineUpdate('tagFamily', targetList.indexOf(selectedModel))"
+    />
+    <CVslider
+      v-model="decimate"
+      class="pt-2"
+      slider-cols="12"
+      name="Decimate"
+      min="0"
+      max="3"
+      step=".01"
+      @input="handlePipelineData('decimate')"
+    />
+    <CVslider
+      v-model="blur"
+      class="pt-2"
+      slider-cols="12"
+      name="Blur"
+      min="0"
+      max="5"
+      step=".01"
+      @input="handlePipelineData('blur')"
+    />
+    <CVslider
+      v-model="threads"
+      class="pt-2"
+      slider-cols="12"
+      name="Threads"
+      min="1"
+      max="8"
+      step="1"
+      @input="handlePipelineData('threads')"
+    />
+    <CVswitch
+      v-model="refineEdges"
+      class="pt-2"
+      slider-cols="12"
+      name="Refine Edges"
+      @input="handlePipelineData('refineEdges')"
+    />
+  </div>
+</template>
+
+<script>
+    import CVslider from '../../components/common/cv-slider'
+    import CVswitch from '../../components/common/cv-switch'
+
+    export default {
+        name: "AprilTag",
+        components: {
+          CVslider,
+          CVswitch,
+        },
+        data() {
+            return {
+              familyList: ["tag36h11"],
+            }
+        },
+        computed: {
+          selectedFamily: {
+            get() {
+                let ret = this.$store.getters.currentPipelineSettings.tagFamily
+                return this.familyList[ret];
+            },
+            set(val) {
+                this.$store.commit("mutatePipeline", {"tagFamily": this.familyList.indexOf(val)})
+            }
+          },
+          decimate: {
+            get() {
+                return this.$store.getters.currentPipelineSettings.decimate
+            },
+            set(val) {
+                this.$store.commit("mutatePipeline", {"decimate": val});
+            }
+          },
+          blur: {
+            get() {
+                return this.$store.getters.currentPipelineSettings.blur
+            },
+            set(val) {
+                this.$store.commit("mutatePipeline", {"blur": val});
+            }
+          },
+          threads: {
+            get() {
+                return this.$store.getters.currentPipelineSettings.threads
+            },
+            set(val) {
+                this.$store.commit("mutatePipeline", {"threads": val});
+            }
+          },
+          refineEdges: {
+            get() {
+                return this.$store.getters.currentPipelineSettings.refineEdges
+            },
+            set(val) {
+                this.$store.commit("mutatePipeline", {"refineEdges": val});
+            }
+          },
+        },
+        methods: {
+        }
+    }
+</script>
--- a/photon-client/src/views/PipelineViews/InputTab.vue
+++ b/photon-client/src/views/PipelineViews/InputTab.vue
@@ -1,6 +1,7 @@
 <template>
  <div>
    <CVslider
+      v-if="cameraExposure !== -1"
      v-model="cameraExposure"
      name="Exposure"
      min="0"
--- a/photon-client/src/views/PipelineViews/TargetsTab.vue
+++ b/photon-client/src/views/PipelineViews/TargetsTab.vue
@@ -18,29 +18,43 @@
              <th class="text-center">
                Target
              </th>
+              <th class="text-center" v-if="$store.getters.pipelineType === 4">
+                Fiducial ID
+              </th>
              <template v-if="!$store.getters.currentPipelineSettings.solvePNPEnabled">
                <th class="text-center">
-                  Pitch
+                  Pitch,&nbsp;&deg;
                </th>
                <th class="text-center">
-                  Yaw
+                  Yaw,&nbsp;&deg;
                </th>
                <th class="text-center">
-                  Skew
+                  Skew,&nbsp;&deg;
+                </th>
+                <th class="text-center">
+                  Area, %
                </th>
              </template>
-              <th class="text-center">
-                Area
-              </th>
-              <template v-if="$store.getters.currentPipelineSettings.solvePNPEnabled">
+              <template v-else-if="$store.getters.pipelineType === 4 && $store.getters.currentPipelineSettings.solvePNPEnabled">
                <th class="text-center">
-                  X
+                  X,&nbsp;m
                </th>
                <th class="text-center">
-                  Y
+                  Y,&nbsp;m
                </th>
                <th class="text-center">
-                  Angle
+                  Z Angle,&nbsp;&deg;
+                </th>
+              </template>
+              <template v-else-if="$store.getters.currentPipelineSettings.solvePNPEnabled">
+                <th class="text-center">
+                  X,&nbsp;m
+                </th>
+                <th class="text-center">
+                  Y,&nbsp;m
+                </th>
+                <th class="text-center">
+                  Z Angle,&nbsp;&deg;
                </th>
              </template>
            </tr>
@@ -51,17 +65,25 @@
              :key="index"
            >
              <td>{{ index }}</td>
+              <td v-if="$store.getters.pipelineType === 4">
+                {{ parseInt(value.fiducialId) }}
+              </td>
              <template v-if="!$store.getters.currentPipelineSettings.solvePNPEnabled">
                <td>{{ parseFloat(value.pitch).toFixed(2) }}</td>
                <td>{{ parseFloat(value.yaw).toFixed(2) }}</td>
                <td>{{ parseFloat(value.skew).toFixed(2) }}</td>
+                <td>{{ parseFloat(value.area).toFixed(2) }}</td>
              </template>
-              <td>{{ parseFloat(value.area).toFixed(2) }}</td>
-              <template v-if="$store.getters.currentPipelineSettings.solvePNPEnabled">
-                <!-- TODO: Make sure that units are correct -->
+              <template v-else-if="$store.getters.currentPipelineSettings.solvePNPEnabled && $store.getters.pipelineType === 4">
+                <td>{{ parseInt(value.fiducialId) }} </td>
                <td>{{ parseFloat(value.pose.x).toFixed(2) }}&nbsp;m</td>
                <td>{{ parseFloat(value.pose.y).toFixed(2) }}&nbsp;m</td>
-                <td>{{ parseFloat(value.pose.rot).toFixed(2) }}&deg;</td>
+                <td>{{ (parseFloat(value.pose.angle_z) * 180 / Math.PI).toFixed(2) }}&deg;</td>
+              </template>
+              <template v-else-if="$store.getters.currentPipelineSettings.solvePNPEnabled">
+                <td>{{ parseFloat(value.pose.x).toFixed(2) }}&nbsp;m</td>
+                <td>{{ parseFloat(value.pose.y).toFixed(2) }}&nbsp;m</td>
+                <td>{{ (parseFloat(value.pose.angle_z) * 180 / Math.PI).toFixed(2) }}&deg;</td>
              </template>
            </tr>
          </tbody>