First pass at covariance blob

README.md

@@ -64,6 +64,8 @@ npm run watch
 npm start
 ```
 
+If you only need to change a subset of bundles, we can specify that using `npm run watch -- --configLargeRenderers` (or similar). See the [rollup config](rollup.config.mjs) for more info.
+
 ## Assets
 
 For details on adding custom assets, see [Custom Assets](https://docs.advantagescope.org/more-features/custom-assets).

package.json

@@ -54,7 +54,7 @@
     "fuse.js": "^7.0.0",
     "gunzip-file": "^0.1.1",
     "heatmap.js": "https://github.com/jwbonner/heatmap.js.git",
-    "mathjs": "11.3.0",
+    "mathjs": "^11.3.0",
     "node-fetch": "^3.3.2",
     "prettier": "3.0.3",
     "prettier-plugin-organize-imports": "^4.0.0",

src/hub/controllers/ThreeDimensionController.ts

@@ -3,8 +3,10 @@ import {
   APRIL_TAG_16H5_COUNT,
   APRIL_TAG_36H11_COUNT,
   AnnotatedPose3d,
+  AnnotatedPose3dWithVariance,
   SwerveState,
   grabHeatmapData,
+  grabPose3dWithVariance,
   grabPosesAuto,
   grabSwerveStates
 } from "../../shared/geometry";
@@ -295,6 +297,7 @@ export default class ThreeDimensionController implements TabController {
       let components: AnnotatedPose3d[] = [];
       let mechanisms: MechanismState[] = [];
       let visionTargets: AnnotatedPose3d[] = [];
+      let posesWithVar: AnnotatedPose3dWithVariance[] = [];
       let swerveStates: {
         values: SwerveState[];
         color: string;
@@ -413,6 +416,10 @@ export default class ThreeDimensionController implements TabController {
               });
               break;
             }
+
+            case "Pose3WithVariance": {
+              console.log("adsfasdfasdf");
+            }
           }
         });
       }
@@ -489,6 +496,20 @@ export default class ThreeDimensionController implements TabController {
             poses: poses
           });
           break;
+        case "Pose3WithVariance":
+          if (!time) {
+            break;
+          }
+
+          let newPoses = grabPose3dWithVariance(window.log, source.logKey, time!, this.UUID);
+
+          posesWithVar = newPoses;
+
+          objects.push({
+            type: "axesWithVar",
+            poses: posesWithVar
+          });
+          break;
         case "cone":
         case "coneLegacy":
           let positionRaw = source.options.position;

src/hub/controllers/ThreeDimensionController_Config.ts

@@ -793,6 +793,18 @@ const ThreeDimensionController_Config: SourceListConfig = {
       showDocs: true,
       options: [],
       previewType: "Translation2d"
+    },
+    {
+      key: "Pose3WithVariance",
+      display: "Pose3WithVariance",
+      symbol: "move.3d",
+      showInTypeName: true,
+      color: "#000000",
+      darkColor: "#ffffff",
+      sourceTypes: ["Pose3WithCovariance"],
+      showDocs: true,
+      options: [],
+      previewType: "Pose3d"
     }
   ]
 };

src/shared/geometry.ts

@@ -1,3 +1,4 @@
+import { column, cross, divide, dot, eigs, index, MathCollection, MathType, matrix, norm, sqrt, subset } from "mathjs";
 import Log from "./log/Log";
 import { getOrDefault, getRobotStateRanges } from "./log/LogUtil";
 import LoggableType from "./log/LoggableType";
@@ -23,6 +24,21 @@ export type Pose3d = {
   translation: Translation3d;
   rotation: Rotation3d;
 };
+
+export type Pose3Variance = {
+  rx: number;
+  ry: number;
+  rz: number;
+  tx: number;
+  ty: number;
+  tz: number;
+};
+
+export type Pose3dWithVariance = {
+  translation: Translation3d;
+  rotation: Rotation3d;
+  variance: Pose3Variance;
+};
 export const Translation3dZero: Translation3d = [0, 0, 0];
 export const Rotation3dZero: Rotation3d = [1, 0, 0, 0];
 export const Pose3dZero: Pose3d = {
@@ -38,6 +54,10 @@ export type AnnotatedPose3d = {
   pose: Pose3d;
   annotation: PoseAnnotations;
 };
+export type AnnotatedPose3dWithVariance = {
+  pose: Pose3dWithVariance;
+  annotation: PoseAnnotations;
+};
 export type PoseAnnotations = {
   is2DSource: boolean;
   zebraTeam?: number;
@@ -199,6 +219,7 @@ export function grabPosesAuto(
       return grabPose2d(log, key, timestamp, uuid);
     case "Pose3d":
     case "Transform3d":
+    case "Pose3WithVariance":
       return grabPose3d(log, key, timestamp, uuid);
     case "Pose2d[]":
     case "Transform2d[]":
@@ -438,6 +459,91 @@ export function grabPose2d(log: Log, key: string, timestamp: number, uuid?: stri
   ];
 }
 
+export function grabPose3dWithVariance(
+  log: Log,
+  key: string,
+  timestamp: number,
+  uuid?: string
+): AnnotatedPose3dWithVariance[] {
+  let ret = grabPose3d(log, key + "/pose", timestamp, uuid);
+  return ret.map((it) => {
+    // huge hack lol
+
+    const matrixData: number[] = [...Array(36).keys()].map((it) =>
+      getOrDefault(log, key + "/covariance/" + it, LoggableType.Number, timestamp, 0.0, uuid)
+    );
+
+    let out: number[][] = [];
+    for (let i = 0; i < matrixData.length; i += 6) {
+      out.push(matrixData.slice(i, i + 6));
+    }
+
+    const mat = matrix(out);
+    console.log(mat);
+
+    const trans = mat.subset(
+      index([3, 4, 5], [3, 4, 5])
+    )
+    console.log(trans)
+
+    const vecnorm = (it: MathCollection) => {
+      let sum = 0;
+      it.forEach(n => sum += n * n);
+      return sqrt(sum);
+    };
+
+    const eigenSol = eigs(trans);
+
+    let vec0: MathCollection = column(eigenSol.vectors, 0)
+    vec0 = divide(vec0, vecnorm(vec0));
+    let val0 = subset(eigenSol.values, index(0))
+
+    let vec1 = column(eigenSol.vectors, 1)
+    vec1 = divide(vec1, vecnorm(vec1));
+    let val1 = subset(eigenSol.values, index(1))
+
+    let vec2 = column(eigenSol.vectors, 2)
+    vec2 = divide(vec2, vecnorm(vec2));
+    const val2 = subset(eigenSol.values, index(2))
+
+    // check right-handed-ness
+    const cc = cross(vec0, vec1);
+    if (dot(cc, vec2) < 1) {
+      const newVec0 = vec1;
+      const newVal0 = val1;
+      const newVec1 = vec0;
+      const newVal1 = val0;
+
+      vec0 = newVec0;
+      val0 = newVal0;
+      vec1 = newVec1;
+      val1 = newVal1;
+    }
+
+    // see https://github.com/ros-visualization/rviz/pull/1576/files
+    // convert vec0/1/2 into a rotation matrix, and use that as orientation
+    // use x/y/z scale as sqrt(eigenvalues) to convert variance->stddev
+    // make sure to prerotate by pose orientation - see the diff in the above PR
+
+    const ret: AnnotatedPose3dWithVariance = {
+      pose: {
+        translation: it.pose.translation,
+        rotation: it.pose.rotation,
+        variance: {
+          rx: 0,
+          ry: 0,
+          rz: 0,
+          tx: getOrDefault(log, key + "/covariance/3", LoggableType.Number, timestamp, 0.01, uuid),
+          ty: getOrDefault(log, key + "/covariance/4", LoggableType.Number, timestamp, 0.01, uuid),
+          tz: getOrDefault(log, key + "/covariance/5", LoggableType.Number, timestamp, 0.01, uuid)
+        }
+      },
+      annotation: it.annotation
+    };
+    return ret;
+  });
+}
+
 export function grabPose3d(log: Log, key: string, timestamp: number, uuid?: string): AnnotatedPose3d[] {
   return [
     {

src/shared/renderers/ThreeDimensionRenderer.ts

@@ -1,4 +1,4 @@
-import { AnnotatedPose3d, SwerveState } from "../geometry";
+import { AnnotatedPose3d, AnnotatedPose3dWithVariance, SwerveState } from "../geometry";
 import { MechanismState } from "../log/LogUtil";
 import TabRenderer from "./TabRenderer";
 import ThreeDimensionRendererImpl from "./ThreeDimensionRendererImpl";
@@ -121,7 +121,7 @@ export type ThreeDimensionRendererCommand_AnyObj =
   | ThreeDimensionRendererCommand_AprilTagObj
   | ThreeDimensionRendererCommand_AxesObj
   | ThreeDimensionRendererCommand_ConeObj
-  | ThreeDimensionRendererCommand_ZebraMarkerObj;
+  | ThreeDimensionRendererCommand_ZebraMarkerObj | ThreeDimensionRendererCommand_AxesWithVarianceObj;
 
 export type ThreeDimensionRendererCommand_GenericRobotObj = {
   model: string;
@@ -173,6 +173,11 @@ export type ThreeDimensionRendererCommand_AxesObj = {
   poses: AnnotatedPose3d[];
 };
 
+export type ThreeDimensionRendererCommand_AxesWithVarianceObj = {
+  type: "axesWithVar";
+  poses: AnnotatedPose3dWithVariance[];
+};
+
 export type ThreeDimensionRendererCommand_ConeObj = {
   type: "cone";
   color: string;

src/shared/renderers/ThreeDimensionRendererImpl.ts

@@ -29,6 +29,7 @@ import HeatmapManager from "./threeDimension/objectManagers/HeatmapManager";
 import RobotManager from "./threeDimension/objectManagers/RobotManager";
 import TrajectoryManager from "./threeDimension/objectManagers/TrajectoryManager";
 import ZebraManager from "./threeDimension/objectManagers/ZebraManager";
+import PoseWithVarManagerManager from "./threeDimension/objectManagers/PoseWithVarManager";
 
 export default class ThreeDimensionRendererImpl implements TabRenderer {
   private LOWER_POWER_MAX_FPS = 30;
@@ -447,6 +448,9 @@ export default class ThreeDimensionRendererImpl implements TabRenderer {
       case "axes":
         manager = new AxesManager(...args);
         break;
+      case "axesWithVar":
+        manager = new PoseWithVarManagerManager(...args);
+        break;
       case "cone":
         manager = new ConeManager(...args);
         break;

src/shared/renderers/threeDimension/AxesField.ts

@@ -1,6 +1,6 @@
 import * as THREE from "three";
 import { STANDARD_FIELD_LENGTH, STANDARD_FIELD_WIDTH } from "../../AdvantageScopeAssets";
-import makeAxesTemplate from "./AxesTemplate";
+import { makeAxesTemplate } from "./AxesTemplate";
 
 export default function makeAxesField(materialSpecular: THREE.Color, materialShininess: number): THREE.Object3D {
   let field = new THREE.Group();

src/shared/renderers/threeDimension/AxesTemplate.ts

@@ -1,6 +1,6 @@
 import * as THREE from "three";
 
-export default function makeAxesTemplate(materialSpecular: THREE.Color, materialShininess: number): THREE.Object3D {
+export function makeAxesTemplate(materialSpecular: THREE.Color, materialShininess: number): THREE.Object3D {
   let axes = new THREE.Object3D();
   const radius = 0.02;
 
@@ -59,3 +59,25 @@ export default function makeAxesTemplate(materialSpecular: THREE.Color, material
 
   return axes;
 }
+
+export function makeCovarianceEllipseTemplate(materialSpecular: THREE.Color, materialShininess: number): THREE.Object3D {
+  let axes = new THREE.Object3D();
+
+  const radius = 1.0;
+
+  const center = new THREE.Mesh(
+    new THREE.SphereGeometry(radius, 32, 16),
+    new THREE.MeshPhongMaterial({
+      color: 0xffffff,
+      transparent: true,
+      opacity: 0.15,
+      specular: materialSpecular,
+      shininess: materialShininess
+    })
+  );
+  center.castShadow = true;
+  center.receiveShadow = true;
+  axes.add(center);
+
+  return axes;
+}

src/shared/renderers/threeDimension/ResizableInstancedMesh.ts

@@ -11,6 +11,8 @@ export default class ResizableInstancedMesh {
   private dummy = new THREE.Object3D();
   private meshes: (THREE.InstancedMesh | null)[] = [];
 
+  private scalemap: Record<string, THREE.Vector3> = {};
+
   constructor(
     parent: THREE.Object3D,
     sources: (THREE.Mesh | { geometry: THREE.BufferGeometry; material: THREE.Material | THREE.Material[] })[],
@@ -51,6 +53,10 @@ export default class ResizableInstancedMesh {
     });
   }
 
+  setScale(geom_uuid: string, scale: THREE.Vector3): void {
+    this.scalemap[geom_uuid] = scale;
+  }
+
   setPoses(poses: Pose3d[]): void {
     // Resize instanced mesh
     if (poses.length > this.count) {
@@ -80,8 +86,18 @@ export default class ResizableInstancedMesh {
       } else {
         this.dummy.position.set(1e6, 1e6, 1e6);
       }
-      this.dummy.updateMatrix();
       this.meshes.forEach((mesh) => {
+        const scale = this.scalemap[mesh?.geometry.uuid || ""];
+        if (scale) {
+          this.dummy.scale.set(
+            scale.x,
+            scale.y,
+            scale.z
+          );
+        } else {
+          this.dummy.scale.set(1,1,1);
+        }
+        this.dummy.updateMatrix();
         mesh?.setMatrixAt(i, this.dummy.matrix);
       });
     }

src/shared/renderers/threeDimension/objectManagers/AxesManager.ts

@@ -1,9 +1,9 @@
 import * as THREE from "three";
 import { ThreeDimensionRendererCommand_AxesObj } from "../../ThreeDimensionRenderer";
-import makeAxesTemplate from "../AxesTemplate";
 import ObjectManager from "../ObjectManager";
 import optimizeGeometries from "../OptimizeGeometries";
 import ResizableInstancedMesh from "../ResizableInstancedMesh";
+import { makeAxesTemplate } from "../AxesTemplate";
 
 export default class AxesManager extends ObjectManager<ThreeDimensionRendererCommand_AxesObj> {
   private instances: ResizableInstancedMesh | null = null;