diff --git a/README.md b/README.md index da84c40c..878d6784 100644 --- a/README.md +++ b/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). diff --git a/package-lock.json b/package-lock.json index 897c73ed..b82650c1 100644 --- a/package-lock.json +++ b/package-lock.json @@ -47,7 +47,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", @@ -3567,6 +3567,7 @@ "resolved": "https://registry.npmjs.org/mathjs/-/mathjs-11.3.0.tgz", "integrity": "sha512-HAUFLxyH8WMSDisNljQFIKjp//d1iL0C36nhGEJFwzdMb46gACMG1E8Ze06sES4hJ5jHDv5ieq7r3mDDcU/d4g==", "dev": true, + "license": "Apache-2.0", "dependencies": { "@babel/runtime": "^7.19.4", "complex.js": "^2.1.1", diff --git a/package.json b/package.json index 7b0ba394..c253116a 100644 --- a/package.json +++ b/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", diff --git a/src/hub/controllers/ThreeDimensionController.ts b/src/hub/controllers/ThreeDimensionController.ts index cecb289a..d9f1cef7 100644 --- a/src/hub/controllers/ThreeDimensionController.ts +++ b/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; diff --git a/src/hub/controllers/ThreeDimensionController_Config.ts b/src/hub/controllers/ThreeDimensionController_Config.ts index 332140c5..830de6dd 100644 --- a/src/hub/controllers/ThreeDimensionController_Config.ts +++ b/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" } ] }; diff --git a/src/shared/geometry.ts b/src/shared/geometry.ts index e8931fbd..6770051a 100644 --- a/src/shared/geometry.ts +++ b/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 [ { diff --git a/src/shared/renderers/ThreeDimensionRenderer.ts b/src/shared/renderers/ThreeDimensionRenderer.ts index 3de2e3ff..056b96e7 100644 --- a/src/shared/renderers/ThreeDimensionRenderer.ts +++ b/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; diff --git a/src/shared/renderers/ThreeDimensionRendererImpl.ts b/src/shared/renderers/ThreeDimensionRendererImpl.ts index 2437a9d3..c4b9eb59 100644 --- a/src/shared/renderers/ThreeDimensionRendererImpl.ts +++ b/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; diff --git a/src/shared/renderers/threeDimension/AxesField.ts b/src/shared/renderers/threeDimension/AxesField.ts index 6a195a27..0e851744 100644 --- a/src/shared/renderers/threeDimension/AxesField.ts +++ b/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(); diff --git a/src/shared/renderers/threeDimension/AxesTemplate.ts b/src/shared/renderers/threeDimension/AxesTemplate.ts index 18b1b83c..f29db1dc 100644 --- a/src/shared/renderers/threeDimension/AxesTemplate.ts +++ b/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; +} diff --git a/src/shared/renderers/threeDimension/ResizableInstancedMesh.ts b/src/shared/renderers/threeDimension/ResizableInstancedMesh.ts index 3430ce49..9fed2b0b 100644 --- a/src/shared/renderers/threeDimension/ResizableInstancedMesh.ts +++ b/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 = {}; + 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); }); } diff --git a/src/shared/renderers/threeDimension/objectManagers/AxesManager.ts b/src/shared/renderers/threeDimension/objectManagers/AxesManager.ts index 88418e3b..27cefab6 100644 --- a/src/shared/renderers/threeDimension/objectManagers/AxesManager.ts +++ b/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 { private instances: ResizableInstancedMesh | null = null; diff --git a/src/shared/renderers/threeDimension/objectManagers/PoseWithVarManager.ts b/src/shared/renderers/threeDimension/objectManagers/PoseWithVarManager.ts new file mode 100644 index 00000000..31c156c3 --- /dev/null +++ b/src/shared/renderers/threeDimension/objectManagers/PoseWithVarManager.ts @@ -0,0 +1,64 @@ +import * as THREE from "three"; +import { ThreeDimensionRendererCommand_AxesObj, ThreeDimensionRendererCommand_AxesWithVarianceObj } from "../../ThreeDimensionRenderer"; +import ObjectManager from "../ObjectManager"; +import optimizeGeometries from "../OptimizeGeometries"; +import ResizableInstancedMesh from "../ResizableInstancedMesh"; +import { makeAxesTemplate, makeCovarianceEllipseTemplate } from "../AxesTemplate"; + +export default class PoseWithVarManagerManager extends ObjectManager { + private instances: ResizableInstancedMesh | null = null; + private ballUuid: string | undefined = undefined; + + constructor( + root: THREE.Object3D, + materialSpecular: THREE.Color, + materialShininess: number, + mode: "low-power" | "standard" | "cinematic", + requestRender: () => void + ) { + super(root, materialSpecular, materialShininess, mode, requestRender); + + + let axes = makeAxesTemplate(this.materialSpecular, this.materialShininess); + axes.scale.set(0.25, 0.25, 0.25); + + optimizeGeometries(axes, this.mode, this.materialSpecular, this.materialShininess, false).then((result) => { + let sources: THREE.Mesh[] = [] + + let axesMerged = result.normal[0]; + if (axesMerged !== null) { + sources.push(axesMerged) + } + + let covBall = makeCovarianceEllipseTemplate(this.materialSpecular, this.materialShininess); + optimizeGeometries(covBall, this.mode, this.materialSpecular, this.materialShininess, false).then((result) => { + let axesMerged = result.transparent[0]; + if (axesMerged !== null) { + sources.push(axesMerged) + this.ballUuid = axesMerged.geometry.uuid; + } + + this.instances = new ResizableInstancedMesh(root, sources); + }); + + }); + + } + + dispose(): void { + this.instances?.dispose(); + } + + setObjectData(object: ThreeDimensionRendererCommand_AxesWithVarianceObj): void { + const variance = object.poses[0].pose.variance; + this.instances?.setScale(this.ballUuid || "", + new THREE.Vector3( + variance.tx, + variance.ty, + variance.tz + ) + ); + + this.instances?.setPoses(object.poses.map((x) => x.pose)); + } +}