diff --git a/gui/electron/main/index.ts b/gui/electron/main/index.ts index 35f29090f3..a593b77f4b 100644 --- a/gui/electron/main/index.ts +++ b/gui/electron/main/index.ts @@ -32,13 +32,12 @@ import { writeFileSync } from 'node:fs'; import { spawn } from 'node:child_process'; import { discordPresence } from './presence'; import { options } from './cli'; -import { ServerStatusEvent } from 'electron/preload/interface'; +import { ServerStatusEvent, WebcamOfferRequest } from 'electron/preload/interface'; import { mkdir } from 'node:fs/promises'; import { MenuItem } from 'electron/main'; - -app.setPath('userData', getGuiDataFolder()) -app.setPath('sessionData', join(getGuiDataFolder(), 'electron')) +app.setPath('userData', getGuiDataFolder()); +app.setPath('sessionData', join(getGuiDataFolder(), 'electron')); // Register custom protocol to handle asset paths with leading slashes protocol.registerSchemesAsPrivileged([ @@ -55,6 +54,13 @@ protocol.registerSchemesAsPrivileged([ let mainWindow: BrowserWindow | null = null; +function buildWebcamOfferUrl(host: string, port: number) { + const normalizedHost = + host.includes(':') && !host.startsWith('[') ? `[${host}]` : host; + + return `http://${normalizedHost}:${port}/offer`; +} + handleIpc(IPC_CHANNELS.GH_FETCH, async (e, options) => { if (options.type === 'fw-releases') { return fetch( @@ -153,6 +159,31 @@ handleIpc(IPC_CHANNELS.DISCORD_PRESENCE, async (e, options) => { } }); +handleIpc(IPC_CHANNELS.WEBCAM_OFFER, async (e, request: WebcamOfferRequest) => { + const response = await fetch(buildWebcamOfferUrl(request.host, request.port), { + method: 'POST', + headers: { + 'Content-Type': 'application/json', + }, + body: JSON.stringify({ + sdp: request.sdp, + }), + }); + + if (!response.ok) { + throw new Error(`Offer request failed with status ${response.status}`); + } + + const body = (await response.json()) as { sdp?: unknown }; + if (typeof body.sdp !== 'string' || body.sdp.length === 0) { + throw new Error('Webcam response did not contain an SDP answer'); + } + + return { + sdp: body.sdp, + }; +}); + handleIpc(IPC_CHANNELS.OPEN_FILE, (e, folder) => { const requestedPath = path.resolve(folder); @@ -339,8 +370,7 @@ function createWindow() { menu.append(new MenuItem({ label: 'Copy', role: 'copy' })); menu.append(new MenuItem({ label: 'Paste', role: 'paste' })); - if (mainWindow) - menu.popup({ window: mainWindow }); + if (mainWindow) menu.popup({ window: mainWindow }); }); } diff --git a/gui/electron/preload/index.ts b/gui/electron/preload/index.ts index e9682ca0d3..9b1636d380 100644 --- a/gui/electron/preload/index.ts +++ b/gui/electron/preload/index.ts @@ -31,9 +31,18 @@ contextBridge.exposeInMainWorld('electronAPI', { setTranslations: () => {}, openDialog: (options) => ipcRenderer.invoke(IPC_CHANNELS.OPEN_DIALOG, options), saveDialog: (options) => ipcRenderer.invoke(IPC_CHANNELS.SAVE_DIALOG, options), - openConfigFolder: async () => ipcRenderer.invoke(IPC_CHANNELS.OPEN_FILE, await ipcRenderer.invoke(IPC_CHANNELS.GET_FOLDER, 'config')), - openLogsFolder: async () => ipcRenderer.invoke(IPC_CHANNELS.OPEN_FILE, await ipcRenderer.invoke(IPC_CHANNELS.GET_FOLDER, 'logs')), + openConfigFolder: async () => + ipcRenderer.invoke( + IPC_CHANNELS.OPEN_FILE, + await ipcRenderer.invoke(IPC_CHANNELS.GET_FOLDER, 'config') + ), + openLogsFolder: async () => + ipcRenderer.invoke( + IPC_CHANNELS.OPEN_FILE, + await ipcRenderer.invoke(IPC_CHANNELS.GET_FOLDER, 'logs') + ), openFile: (path) => ipcRenderer.invoke(IPC_CHANNELS.OPEN_FILE, path), ghGet: (req) => ipcRenderer.invoke(IPC_CHANNELS.GH_FETCH, req), - setPresence: (options) => ipcRenderer.invoke(IPC_CHANNELS.DISCORD_PRESENCE, options) + setPresence: (options) => ipcRenderer.invoke(IPC_CHANNELS.DISCORD_PRESENCE, options), + webcamOffer: (request) => ipcRenderer.invoke(IPC_CHANNELS.WEBCAM_OFFER, request), } satisfies IElectronAPI); diff --git a/gui/electron/preload/interface.d.ts b/gui/electron/preload/interface.d.ts index 03c47a1f7b..478a05c694 100644 --- a/gui/electron/preload/interface.d.ts +++ b/gui/electron/preload/interface.d.ts @@ -34,7 +34,15 @@ export type GHReturn = { | null; }; -export type DiscordPresence = { enable: false } | { enable: true, activity: string } +export type DiscordPresence = { enable: false } | { enable: true; activity: string }; +export type WebcamOfferRequest = { + host: string; + port: number; + sdp: string; +}; +export type WebcamOfferResponse = { + sdp: string; +}; export interface IElectronAPI { onServerStatus: (cb: (data: ServerStatusEvent) => void) => () => void; @@ -55,6 +63,7 @@ export interface IElectronAPI { openFile: (path: string) => void; ghGet: (options: T) => Promise; setPresence: (options: DiscordPresence) => void; + webcamOffer: (request: WebcamOfferRequest) => Promise; } declare global { diff --git a/gui/electron/shared.ts b/gui/electron/shared.ts index 1364550bda..615d6a3ef6 100644 --- a/gui/electron/shared.ts +++ b/gui/electron/shared.ts @@ -4,7 +4,14 @@ import { SaveDialogOptions, SaveDialogReturnValue, } from 'electron'; -import { DiscordPresence, GHGet, GHReturn, OSStats } from './preload/interface'; +import { + DiscordPresence, + GHGet, + GHReturn, + OSStats, + WebcamOfferRequest, + WebcamOfferResponse, +} from './preload/interface'; export const IPC_CHANNELS = { SERVER_STATUS: 'server-status', @@ -19,7 +26,8 @@ export const IPC_CHANNELS = { OPEN_FILE: 'open-file', GET_FOLDER: 'get-folder', GH_FETCH: 'gh-fetch', - DISCORD_PRESENCE: 'discord-presence' + DISCORD_PRESENCE: 'discord-presence', + WEBCAM_OFFER: 'webcam-offer', } as const; export interface IpcInvokeMap { @@ -46,4 +54,7 @@ export interface IpcInvokeMap { options: T ) => Promise; [IPC_CHANNELS.DISCORD_PRESENCE]: (options: DiscordPresence) => void; + [IPC_CHANNELS.WEBCAM_OFFER]: ( + request: WebcamOfferRequest + ) => Promise; } diff --git a/gui/package.json b/gui/package.json index 1657a509c3..2f29428195 100644 --- a/gui/package.json +++ b/gui/package.json @@ -17,6 +17,7 @@ }, "scripts": { "start": "vite --force", + "dev:clean": "rd /s /q \"node_modules/.vite\" && pnpm run gui", "gui": "electron-vite dev --config electron.vite.config.ts --watch", "build": "electron-vite build --config electron.vite.config.ts", "package": "electron-builder", diff --git a/gui/public/i18n/en/translation.ftl b/gui/public/i18n/en/translation.ftl index 7158573da2..7b871e1134 100644 --- a/gui/public/i18n/en/translation.ftl +++ b/gui/public/i18n/en/translation.ftl @@ -273,10 +273,44 @@ navbar-home = Home navbar-body_proportions = Body Proportions navbar-trackers_assign = Tracker Assignment navbar-mounting = Mounting Calibration +navbar-video_calibration = Video Calibration navbar-onboarding = Setup Wizard navbar-settings = Settings navbar-connect_trackers = Connect Trackers +## Video calibration +video-calibration-loading = Looking for a webcam... +video-calibration-connecting = Connecting to the webcam video stream... +video-calibration-no_webcam = No webcam is currently available. +video-calibration-error = Could not start the webcam video stream. +video-calibration-sidebar-title = Video Calibration +video-calibration-sidebar-description = Start calibration and monitor its progress here. +video-calibration-sidebar-description-active = Current progress is shown under Status below. +video-calibration-start = Start Calibration +video-calibration-status-label = Status +video-calibration-status-idle = Waiting to start calibration. +video-calibration-status-starting = Waiting for calibration progress... +video-calibration-status-calibrate_camera = Calibrating camera +video-calibration-status-capture_forward_pose = Capture forward pose +video-calibration-status-capture_bent_over_pose = Capture bent-over pose +video-calibration-status-calibrate_trackers = Calibrating trackers +video-calibration-status-calibrate_skeleton_offsets = Calibrating skeleton offsets +video-calibration-status-done = Calibration complete +video-calibration-instruction-calibrate_camera = Move your right controller in a ∞ loop +video-calibration-instruction-capture_forward_pose = Stand straight and face forward +video-calibration-instruction-capture_bent_over_pose = Carefully lean forward +video-calibration-instruction-calibrate_trackers = Walk around in a small circle +video-calibration-camera-label = Camera +video-calibration-camera-available = Camera data available +video-calibration-camera-unavailable = Camera data is not available yet. +video-calibration-camera-resolution = Resolution +video-calibration-camera-focal_length = Focal length +video-calibration-camera-principal_point = Principal point +video-calibration-done-trackers = Done trackers +video-calibration-pending-trackers = Pending trackers +video-calibration-none = None +video-calibration-error-label = Error + ## Biovision hierarchy recording bvh-start_recording = Record BVH bvh-stop_recording = Save BVH recording diff --git a/gui/src/App.tsx b/gui/src/App.tsx index 5b55a90617..754b0dcbac 100644 --- a/gui/src/App.tsx +++ b/gui/src/App.tsx @@ -53,6 +53,7 @@ import { ChecklistPage } from './components/tracking-checklist/TrackingChecklist import { ElectronContextC, provideElectron } from './hooks/electron'; import { AppLocalizationProvider } from './i18n/config'; import { openUrl } from './hooks/crossplatform'; +import { VideoCalibrationPage } from './components/video-calibration/VideoCalibrationPage'; export const GH_REPO = 'SlimeVR/SlimeVR-Server'; export const VersionContext = createContext(''); @@ -104,6 +105,10 @@ function Layout() { } /> + } + /> @@ -78,7 +84,8 @@ export function MainLayout({
)} - {full && ( + {full && showToolbar && (
)} {!isMobile && full && (
- + {rightSidebar || }
)}
diff --git a/gui/src/components/Navbar.tsx b/gui/src/components/Navbar.tsx index b3bce34f18..7d013a1c9c 100644 --- a/gui/src/components/Navbar.tsx +++ b/gui/src/components/Navbar.tsx @@ -9,6 +9,7 @@ import { useBreakpoint } from '@/hooks/breakpoint'; import { HomeIcon } from './commons/icon/HomeIcon'; import { SkiIcon } from './commons/icon/SkiIcon'; import { WifiIcon } from './commons/icon/WifiIcon'; +import { EyeIcon } from './commons/icon/EyeIcon'; export function NavButton({ to, @@ -94,6 +95,9 @@ export function MainLinks() { > {l10n.getString('navbar-body_proportions')} + }> + {l10n.getString('navbar-video_calibration')} + } diff --git a/gui/src/components/tracker/TrackersTable.tsx b/gui/src/components/tracker/TrackersTable.tsx index b8ef08d0c9..8ef98ceeef 100644 --- a/gui/src/components/tracker/TrackersTable.tsx +++ b/gui/src/components/tracker/TrackersTable.tsx @@ -378,7 +378,7 @@ export function TrackersTable({
{filteredSortedTrackers.map((data) => ( ((resolve) => { + const onIceGatheringStateChange = () => { + console.log( + VC_WEBRTC_LOG, + 'ICE gathering state:', + peerConnection.iceGatheringState + ); + if (peerConnection.iceGatheringState !== 'complete') return; + + peerConnection.removeEventListener( + 'icegatheringstatechange', + onIceGatheringStateChange + ); + console.log(VC_WEBRTC_LOG, 'ICE gathering finished'); + resolve(); + }; + + peerConnection.addEventListener( + 'icegatheringstatechange', + onIceGatheringStateChange + ); + }); +} + +function asText(value: string | Uint8Array | null | undefined) { + if (typeof value === 'string') return value; + if (value instanceof Uint8Array) return new TextDecoder().decode(value); + return ''; +} + +function getCalibrationStatusId( + status: VideoTrackerCalibrationStatus | null, + startRequested: boolean +) { + if (status === null) { + return startRequested + ? 'video-calibration-status-starting' + : 'video-calibration-status-idle'; + } + + switch (status) { + case VideoTrackerCalibrationStatus.CALIBRATE_CAMERA: + return 'video-calibration-status-calibrate_camera'; + case VideoTrackerCalibrationStatus.CAPTURE_FORWARD_POSE: + return 'video-calibration-status-capture_forward_pose'; + case VideoTrackerCalibrationStatus.CAPTURE_BENT_OVER_POSE: + return 'video-calibration-status-capture_bent_over_pose'; + case VideoTrackerCalibrationStatus.CALIBRATE_TRACKERS: + return 'video-calibration-status-calibrate_trackers'; + case VideoTrackerCalibrationStatus.CALIBRATE_SKELETON_OFFSETS: + return 'video-calibration-status-calibrate_skeleton_offsets'; + case VideoTrackerCalibrationStatus.DONE: + return 'video-calibration-status-done'; + default: + return 'video-calibration-status-idle'; + } +} + +function getCalibrationInstructionId( + status: VideoTrackerCalibrationStatus | null +): string | null { + if (status == null) return null; + + switch (status) { + case VideoTrackerCalibrationStatus.CALIBRATE_CAMERA: + return 'video-calibration-instruction-calibrate_camera'; + case VideoTrackerCalibrationStatus.CAPTURE_FORWARD_POSE: + return 'video-calibration-instruction-capture_forward_pose'; + case VideoTrackerCalibrationStatus.CAPTURE_BENT_OVER_POSE: + return 'video-calibration-instruction-capture_bent_over_pose'; + case VideoTrackerCalibrationStatus.CALIBRATE_TRACKERS: + case VideoTrackerCalibrationStatus.CALIBRATE_SKELETON_OFFSETS: + return 'video-calibration-instruction-calibrate_trackers'; + default: + return null; + } +} + +function applyCalibrationCameraToView( + view: SkeletonPreviewView, + camera: VideoTrackerCalibrationCameraT +) { + const near = 0.01; + const far = 1000; + + const worldToCamera = QuaternionFromQuatT(camera.worldToCamera).normalize(); + const cameraToWorld = worldToCamera.clone().invert(); + const cvCameraToThreeCamera = new Quaternion().setFromAxisAngle( + new Vector3(1, 0, 0), + Math.PI + ); + const cameraRotation = cameraToWorld.clone().multiply(cvCameraToThreeCamera); + + const worldOriginInCamera = camera.worldOriginInCamera; + const cameraPosition = worldOriginInCamera + ? new Vector3( + worldOriginInCamera.x, + worldOriginInCamera.y, + worldOriginInCamera.z + ) + .applyQuaternion(cameraToWorld) + .multiplyScalar(-1) + : new Vector3(); + + const projectionMatrix = new Matrix4().set( + (2 * camera.fx) / camera.width, + 0, + 1 - (2 * camera.tx) / camera.width, + 0, + 0, + (2 * camera.fy) / camera.height, + (2 * camera.ty) / camera.height - 1, + 0, + 0, + 0, + -(far + near) / (far - near), + (-2 * far * near) / (far - near), + 0, + 0, + -1, + 0 + ); + + view.interactive = false; + view.manualProjectionMatrix = true; + view.controls.enabled = false; + view.controls.enableRotate = false; + view.controls.enablePan = false; + view.controls.enableZoom = false; + view.controls.target.copy(cameraPosition); + view.camera.near = near; + view.camera.far = far; + view.camera.zoom = 1; + view.camera.position.copy(cameraPosition); + view.camera.quaternion.copy(cameraRotation); + view.camera.projectionMatrix.copy(projectionMatrix); + view.camera.projectionMatrixInverse.copy(projectionMatrix).invert(); + view.camera.updateMatrixWorld(true); +} + +function TrackerList({ trackers }: { trackers: BodyPart[] }) { + const { l10n } = useLocalization(); + + if (!trackers.length) { + return ; + } + + return ( +
+ {trackers.map((tracker) => ( + + {l10n.getString('body_part-' + BodyPart[tracker])} + + ))} +
+ ); +} + +function CameraDetails({ + camera, +}: { + camera: VideoTrackerCalibrationCameraT | null; +}) { + if (!camera) { + return ( + + ); + } + + return ( +
+ +
+ + + {camera.width} x {camera.height} + +
+
+ + + {camera.fx.toFixed(1)} / {camera.fy.toFixed(1)} + +
+
+ + + {camera.tx.toFixed(1)} / {camera.ty.toFixed(1)} + +
+
+ ); +} + +function VideoCalibrationSidebar({ + progress, + startRequested, + onStartCalibration, + showVideo, + onToggleVideo, +}: { + progress: VideoTrackerCalibrationProgressResponseT | null; + startRequested: boolean; + onStartCalibration: () => void; + showVideo: boolean; + onToggleVideo: () => void; +}) { + const error = asText(progress?.error); + + return ( +
+
+ + +
+ +
+ + +
+ +
+ + +
+ +
+ + +
+ +
+ + +
+ + {!!error && ( +
+ + {error} +
+ )} + + +
+ ); +} + +function VideoCalibrationContent({ + videoRef, + skeletonViewRef, + calibrationCamera, + calibrationStatus, + showVideo, + status, + errorMessage, +}: { + videoRef: RefObject; + skeletonViewRef: React.MutableRefObject; + calibrationCamera: VideoTrackerCalibrationCameraT | null; + calibrationStatus: VideoTrackerCalibrationStatus | null; + showVideo: boolean; + status: VideoStreamStatus; + errorMessage: string; +}) { + const showSkeleton = + calibrationStatus != null && + calibrationStatus !== VideoTrackerCalibrationStatus.CALIBRATE_CAMERA; + + const instructionId = getCalibrationInstructionId(calibrationStatus); + + return ( +
+
+
+ {!!instructionId && ( +
+
+ +
+
+ )} +
+
+ ); +} + +export function VideoCalibrationPage({ isMobile }: { isMobile?: boolean }) { + const videoRef = useRef(null); + const skeletonViewRef = useRef(null); + const peerConnectionRef = useRef(null); + const remoteMediaStreamRef = useRef(null); + const attemptRef = useRef(0); + const webrtcConnectTxRef = useRef(0); + const webrtcPendingTxIdRef = useRef(null); + const videoPlayRafRef = useRef(null); + const videoFrameLogHandleRef = useRef(null); + const videoFrameLogStreamRef = useRef(null); + const videoDiagPollRef = useRef | null>(null); + const videoDiagLastKeyRef = useRef(null); + const videoTrackDebugIdsRef = useRef>(new Set()); + const [status, setStatus] = useState('loading'); + const [errorMessage, setErrorMessage] = useState(''); + const [startRequested, setStartRequested] = useState(false); + const [showVideo, setShowVideo] = useState(true); + const [progress, setProgress] = + useState(null); + const { sendRPCPacket, useRPCPacket, isConnected } = useWebsocketAPI(); + const { config } = useConfig(); + const lastProgressStatusRef = useRef( + null + ); + + const scheduleVideoPlay = useCallback(() => { + if (videoPlayRafRef.current != null) { + cancelAnimationFrame(videoPlayRafRef.current); + } + videoPlayRafRef.current = requestAnimationFrame(() => { + videoPlayRafRef.current = null; + const el = videoRef.current; + if (!el?.srcObject) return; + void el.play().catch((e) => { + console.warn(VC_WEBRTC_LOG, 'video.play() rejected', e); + }); + }); + }, []); + + const stopVideoFrameLogging = useCallback(() => { + const el = videoRef.current; + const handle = videoFrameLogHandleRef.current; + if ( + el != null && + handle != null && + typeof el.cancelVideoFrameCallback === 'function' + ) { + try { + el.cancelVideoFrameCallback(handle); + } catch { + /* ignore */ + } + } + videoFrameLogHandleRef.current = null; + videoFrameLogStreamRef.current = null; + if (videoDiagPollRef.current != null) { + clearInterval(videoDiagPollRef.current); + videoDiagPollRef.current = null; + } + videoDiagLastKeyRef.current = null; + }, []); + + const ensureVideoFrameLogging = useCallback(() => { + const el = videoRef.current; + if (!el) return; + + const src = el.srcObject; + if ( + !(src instanceof MediaStream) || + videoFrameLogHandleRef.current != null + ) { + return; + } + + if (typeof el.requestVideoFrameCallback !== 'function') { + el.addEventListener( + 'loadeddata', + () => { + console.log( + VC_WEBRTC_LOG, + 'video decoded data ready (first frame path; no requestVideoFrameCallback)' + ); + }, + { once: true } + ); + return; + } + + videoFrameLogStreamRef.current = src; + + // requestVideoFrameCallback only runs when the compositor presents a NEW frame — not on a timer. + // A single 2×2 "frame" usually means one placeholder decode (often track.muted / waiting for keyframe). + if (import.meta.env.DEV && videoDiagPollRef.current == null) { + videoDiagLastKeyRef.current = null; + videoDiagPollRef.current = setInterval(() => { + const v = videoRef.current; + const expected = videoFrameLogStreamRef.current; + if (!v?.srcObject || v.srcObject !== expected) { + if (videoDiagPollRef.current != null) { + clearInterval(videoDiagPollRef.current); + videoDiagPollRef.current = null; + } + return; + } + const vt = v.srcObject.getVideoTracks()[0]; + const settings = vt?.getSettings?.(); + const key = [ + v.videoWidth, + v.videoHeight, + v.readyState, + v.paused, + vt?.muted, + vt?.readyState, + settings?.width ?? '', + settings?.height ?? '', + settings?.frameRate ?? '', + ].join('|'); + + if (key === videoDiagLastKeyRef.current) return; + videoDiagLastKeyRef.current = key; + }, 2000); + } + + const onFrame: VideoFrameRequestCallback = (_now, _metadata) => { + const v = videoRef.current; + const expectedStream = videoFrameLogStreamRef.current; + + if (v?.srcObject === expectedStream && expectedStream) { + videoFrameLogHandleRef.current = v.requestVideoFrameCallback(onFrame); + } else { + videoFrameLogHandleRef.current = null; + } + }; + + videoFrameLogHandleRef.current = el.requestVideoFrameCallback(onFrame); + }, []); + + const syncRemoteVideoFromPeer = useCallback(() => { + const peerConnection = peerConnectionRef.current; + const remoteStream = remoteMediaStreamRef.current; + if (!peerConnection || !remoteStream) { + console.log( + VC_WEBRTC_LOG, + 'syncRemoteVideoFromPeer: skip (no peer or stream)', + { + hasPeer: !!peerConnection, + hasStream: !!remoteStream, + } + ); + return; + } + + const receivers = peerConnection.getReceivers(); + let added = 0; + for (const receiver of receivers) { + const track = receiver.track; + if (!track || track.kind !== 'video') continue; + if (!remoteStream.getTracks().some(({ id }) => id === track.id)) { + remoteStream.addTrack(track); + added += 1; + } + } + + const videoTracks = remoteStream.getVideoTracks(); + console.log(VC_WEBRTC_LOG, 'syncRemoteVideoFromPeer', { + receiverCount: receivers.length, + videoReceiversWithTrack: receivers.filter( + (r) => r.track?.kind === 'video' + ).length, + tracksAddedThisCall: added, + streamVideoTrackCount: videoTracks.length, + signalingState: peerConnection.signalingState, + connectionState: peerConnection.connectionState, + iceConnectionState: peerConnection.iceConnectionState, + }); + + if (videoTracks.length > 0 && videoRef.current) { + const el = videoRef.current; + // Re-assigning the same stream still counts as a "new load" and aborts an in-flight play(). + if (el.srcObject !== remoteStream) { + stopVideoFrameLogging(); + el.srcObject = remoteStream; + } + scheduleVideoPlay(); + ensureVideoFrameLogging(); + + for (const track of videoTracks) { + if (videoTrackDebugIdsRef.current.has(track.id)) continue; + videoTrackDebugIdsRef.current.add(track.id); + console.log(VC_WEBRTC_LOG, 'MediaStreamTrack (video)', { + id: track.id, + label: track.label, + muted: track.muted, + enabled: track.enabled, + readyState: track.readyState, + settings: track.getSettings(), + }); + track.addEventListener('unmute', () => { + console.log(VC_WEBRTC_LOG, 'video track unmute', { + id: track.id, + settings: track.getSettings(), + }); + }); + track.addEventListener('mute', () => { + console.log(VC_WEBRTC_LOG, 'video track mute', { id: track.id }); + }); + } + + setStatus('ready'); + // console.log(VC_WEBRTC_LOG, 'Video element bound; UI status -> ready'); + } + }, [ensureVideoFrameLogging, scheduleVideoPlay, stopVideoFrameLogging]); + + const cleanupConnection = useCallback(() => { + console.log(VC_WEBRTC_LOG, 'cleanupConnection'); + if (videoPlayRafRef.current != null) { + cancelAnimationFrame(videoPlayRafRef.current); + videoPlayRafRef.current = null; + } + stopVideoFrameLogging(); + webrtcPendingTxIdRef.current = null; + remoteMediaStreamRef.current = null; + + const peerConnection = peerConnectionRef.current; + if (peerConnection) { + peerConnection.ontrack = null; + peerConnection.onconnectionstatechange = null; + peerConnection.onicegatheringstatechange = null; + peerConnection.oniceconnectionstatechange = null; + peerConnection.onsignalingstatechange = null; + peerConnection + .getReceivers() + .forEach((receiver) => receiver.track?.stop()); + peerConnection.close(); + peerConnectionRef.current = null; + } + + const currentStream = videoRef.current?.srcObject; + if (currentStream instanceof MediaStream) { + currentStream.getTracks().forEach((track) => track.stop()); + } + + if (videoRef.current) { + videoRef.current.srcObject = null; + } + + videoTrackDebugIdsRef.current.clear(); + }, [stopVideoFrameLogging]); + + const connectVideoViaWebRTC = useCallback(async () => { + const attempt = ++attemptRef.current; + console.log(VC_WEBRTC_LOG, 'connectVideoViaWebRTC start', { attempt }); + const peerConnection = new RTCPeerConnection(); + const remoteStream = new MediaStream(); + + cleanupConnection(); + peerConnectionRef.current = peerConnection; + remoteMediaStreamRef.current = remoteStream; + setStatus('connecting'); + setErrorMessage(''); + + peerConnection.onicegatheringstatechange = () => { + console.log( + VC_WEBRTC_LOG, + 'peer iceGatheringState:', + peerConnection.iceGatheringState + ); + }; + peerConnection.oniceconnectionstatechange = () => { + console.log( + VC_WEBRTC_LOG, + 'peer iceConnectionState:', + peerConnection.iceConnectionState + ); + }; + peerConnection.onsignalingstatechange = () => { + console.log( + VC_WEBRTC_LOG, + 'peer signalingState:', + peerConnection.signalingState + ); + }; + + peerConnection.ontrack = (event) => { + const incomingTracks = event.streams[0]?.getTracks() ?? [event.track]; + console.log(VC_WEBRTC_LOG, 'ontrack', { + streams: event.streams.length, + trackCount: incomingTracks.length, + kinds: incomingTracks.map((t) => t.kind), + }); + + incomingTracks.forEach((track) => { + if (track.kind !== 'video') return; + if (!remoteStream.getTracks().some(({ id }) => id === track.id)) { + remoteStream.addTrack(track); + } + }); + + syncRemoteVideoFromPeer(); + }; + + peerConnection.onconnectionstatechange = () => { + console.log( + VC_WEBRTC_LOG, + 'peer connectionState:', + peerConnection.connectionState + ); + if (peerConnection.connectionState === 'connected') { + syncRemoteVideoFromPeer(); + } + }; + + try { + peerConnection.addTransceiver('video', { direction: 'recvonly' }); + console.log(VC_WEBRTC_LOG, 'recvonly video transceiver added'); + + const offer = await peerConnection.createOffer(); + console.log(VC_WEBRTC_LOG, 'createOffer done', { + type: offer.type, + sdpChars: offer.sdp?.length ?? 0, + }); + await peerConnection.setLocalDescription(offer); + console.log(VC_WEBRTC_LOG, 'setLocalDescription done'); + await waitForIceGatheringComplete(peerConnection); + + const localSdp = peerConnection.localDescription?.sdp; + if (!localSdp) { + throw new Error('Peer connection did not produce a local SDP offer'); + } + + if (attempt !== attemptRef.current) { + console.log( + VC_WEBRTC_LOG, + 'aborted before send (stale attempt)', + attempt, + 'current', + attemptRef.current + ); + return; + } + + const txId = ++webrtcConnectTxRef.current >>> 0; + webrtcPendingTxIdRef.current = txId; + + console.log(VC_WEBRTC_LOG, 'sending ConnectToWebRTCRequest', { + txId, + offerSdpChars: localSdp.length, + provider: 'VIDEO_CALIBRATION', + }); + sendRPCPacket( + RpcMessage.ConnectToWebRTCRequest, + new ConnectToWebRTCRequestT( + WebRTCVideoProvider.VIDEO_CALIBRATION, + localSdp + ), + txId + ); + console.log( + VC_WEBRTC_LOG, + 'ConnectToWebRTCRequest dispatched; awaiting ConnectToWebRTCResponse' + ); + } catch (error) { + if (attempt !== attemptRef.current) { + console.log( + VC_WEBRTC_LOG, + 'connect error ignored (stale attempt)', + attempt + ); + return; + } + + console.error(VC_WEBRTC_LOG, 'connectVideoViaWebRTC failed', error); + cleanupConnection(); + setStatus('error'); + setErrorMessage( + error instanceof Error ? error.message : 'Unknown WebRTC error' + ); + } + }, [cleanupConnection, sendRPCPacket, syncRemoteVideoFromPeer]); + + const connectVideoViaWebRTCRef = useRef(connectVideoViaWebRTC); + connectVideoViaWebRTCRef.current = connectVideoViaWebRTC; + + const onConnectToWebRTCResponse = useCallback( + (detail: ConnectToWebRTCRpcDetail) => { + void (async () => { + const response = detail.message; + + console.log(VC_WEBRTC_LOG, 'ConnectToWebRTCResponse received', { + responseTxId: detail.txId, + pendingTxId: webrtcPendingTxIdRef.current, + }); + + if ( + detail.txId != null && + detail.txId !== webrtcPendingTxIdRef.current + ) { + console.log( + VC_WEBRTC_LOG, + 'ConnectToWebRTCResponse ignored (txId mismatch)' + ); + return; + } + + const err = asText(response.error); + const answerSdp = asText(response.answerSdp); + + console.log(VC_WEBRTC_LOG, 'ConnectToWebRTCResponse payload', { + hasError: !!err, + errorPreview: err ? err.slice(0, 120) : '', + answerSdpChars: answerSdp.length, + }); + + if (err) { + console.error(VC_WEBRTC_LOG, 'server error on WebRTC connect', err); + cleanupConnection(); + setStatus('error'); + setErrorMessage(err); + return; + } + + if (!answerSdp) { + console.error(VC_WEBRTC_LOG, 'no answer SDP in response'); + cleanupConnection(); + setStatus('error'); + setErrorMessage('Server did not return an SDP answer'); + return; + } + + const peerConnection = peerConnectionRef.current; + if (!peerConnection) { + console.warn( + VC_WEBRTC_LOG, + 'no peerConnection when applying answer (already cleaned up?)' + ); + return; + } + + try { + console.log(VC_WEBRTC_LOG, 'setRemoteDescription(answer) …', { + sdpChars: answerSdp.length, + }); + await peerConnection.setRemoteDescription({ + type: 'answer', + sdp: answerSdp, + }); + console.log(VC_WEBRTC_LOG, 'setRemoteDescription(answer) OK'); + webrtcPendingTxIdRef.current = null; + syncRemoteVideoFromPeer(); + } catch (error) { + console.error( + VC_WEBRTC_LOG, + 'setRemoteDescription(answer) failed', + error + ); + cleanupConnection(); + setStatus('error'); + setErrorMessage( + error instanceof Error + ? error.message + : 'Failed to apply SDP answer' + ); + } + })(); + }, + [cleanupConnection, syncRemoteVideoFromPeer] + ); + + useRPCPacket(RpcMessage.ConnectToWebRTCResponse, onConnectToWebRTCResponse); + + const onVideoTrackerCalibrationProgress = useCallback( + (response: VideoTrackerCalibrationProgressResponseT) => { + console.log(VC_CALIB_LOG, 'VideoTrackerCalibrationProgressResponse', { + status: response.status, + error: asText(response.error)?.slice(0, 200) ?? '', + camera: response.camera + ? `${response.camera.width}x${response.camera.height}` + : null, + trackersDone: response.trackersDone?.length ?? 0, + trackersPending: response.trackersPending?.length ?? 0, + }); + setStartRequested(false); + // New object so React state updates even if the wire protocol reuses instances, + // and so useRPCPacket's listener stays stable (see useCallback) instead of + // resubscribing every render (which can drop messages between remove/add). + setProgress( + new VideoTrackerCalibrationProgressResponseT( + response.status, + response.camera, + response.trackersDone != null ? [...response.trackersDone] : [], + response.trackersPending != null ? [...response.trackersPending] : [], + response.error + ) + ); + }, + [] + ); + + useRPCPacket( + RpcMessage.VideoTrackerCalibrationProgressResponse, + onVideoTrackerCalibrationProgress + ); + + useEffect(() => { + const next = progress?.status ?? null; + if (next == null) return; + + const prev = lastProgressStatusRef.current; + if (prev === next) return; + + lastProgressStatusRef.current = next; + if (!config?.feedbackSound) return; + restartAndPlay(resetChimeSound, config.feedbackSoundVolume ?? 1); + }, [progress?.status, config?.feedbackSound, config?.feedbackSoundVolume]); + + const startCalibration = useCallback(() => { + console.log(VC_CALIB_LOG, 'StartVideoTrackerCalibrationRequest'); + setStartRequested(true); + setProgress(null); + sendRPCPacket( + RpcMessage.StartVideoTrackerCalibrationRequest, + new StartVideoTrackerCalibrationRequestT() + ); + }, [sendRPCPacket]); + + // Only re-run when the socket connects/disconnects — not when connectVideoViaWebRTC’s + // identity changes (parent re-renders would otherwise cleanup the peer connection mid-session). + useEffect(() => { + if (!isConnected) { + console.log(VC_WEBRTC_LOG, 'WebSocket not connected; skip WebRTC start'); + return; + } + + console.log( + VC_WEBRTC_LOG, + 'WebSocket connected; starting WebRTC handshake' + ); + void connectVideoViaWebRTCRef.current(); + + return () => { + console.log( + VC_WEBRTC_LOG, + 'effect cleanup (WebSocket disconnected or page unmount)' + ); + attemptRef.current += 1; + skeletonViewRef.current = null; + cleanupConnection(); + }; + }, [isConnected, cleanupConnection]); + + useEffect(() => { + if (!progress?.camera || !skeletonViewRef.current) return; + + applyCalibrationCameraToView(skeletonViewRef.current, progress.camera); + }, [progress?.camera]); + + return ( + setShowVideo((value) => !value)} + /> + } + > + + + ); +} diff --git a/gui/src/components/widgets/SkeletonVisualizerWidget.tsx b/gui/src/components/widgets/SkeletonVisualizerWidget.tsx index 3954b65235..4f7433f4e4 100644 --- a/gui/src/components/widgets/SkeletonVisualizerWidget.tsx +++ b/gui/src/components/widgets/SkeletonVisualizerWidget.tsx @@ -50,13 +50,24 @@ export type SkeletonPreviewView = { camera: PerspectiveCamera; controls: OrbitControls; hidden: boolean; + interactive: boolean; + manualProjectionMatrix: boolean; tween: Tween; onHeightChange: (view: SkeletonPreviewView, newHeight: number) => void; }; function initializePreview( canvas: HTMLCanvasElement, - skeleton: (BoneKind | Bone)[] + skeleton: (BoneKind | Bone)[], + { + showGrid = true, + stabilizeSkeleton = true, + anchorToHmdPosition = false, + }: { + showGrid?: boolean; + stabilizeSkeleton?: boolean; + anchorToHmdPosition?: boolean; + } = {} ) { let lastRenderTimeRef = 0; let frameInterval = 0; @@ -72,9 +83,11 @@ function initializePreview( }); renderer.setSize(canvas.clientWidth, canvas.clientHeight); - const grid = new GridHelper(10, 50, GROUND_COLOR, GROUND_COLOR); - grid.position.set(0, 0, 0); - scene.add(grid); + if (showGrid) { + const grid = new GridHelper(10, 50, GROUND_COLOR, GROUND_COLOR); + grid.position.set(0, 0, 0); + scene.add(grid); + } const skeletonGroup = new Group(); let skeletonHelper = new BasedSkeletonHelper(skeleton[0]); @@ -102,21 +115,25 @@ function initializePreview( skeletonGroup.add(skeletonHelper); scene.add(newSkeleton[0]); - const hmd = bones.get(BodyPart.HEAD); - const chest = bones.get(BodyPart.UPPER_CHEST); - // Check if HMD is identity, if it's then use upper chest's rotation - const quat = isIdentity(hmd?.rotationG) - ? QuaternionFromQuatT(chest?.rotationG).normalize().invert() - : QuaternionFromQuatT(hmd?.rotationG).normalize().invert(); - - // Project quat to (0x, 1y, 0z) - const VEC_Y = new Vector3(0, 1, 0); - const vec = VEC_Y.multiplyScalar( - new Vector3(quat.x, quat.y, quat.z).dot(VEC_Y) / VEC_Y.lengthSq() - ); - const yawReset = new Quaternion(vec.x, vec.y, vec.z, quat.w).normalize(); + if (stabilizeSkeleton) { + const hmd = bones.get(BodyPart.HEAD); + const chest = bones.get(BodyPart.UPPER_CHEST); + // Check if HMD is identity, if it's then use upper chest's rotation + const quat = isIdentity(hmd?.rotationG) + ? QuaternionFromQuatT(chest?.rotationG).normalize().invert() + : QuaternionFromQuatT(hmd?.rotationG).normalize().invert(); + + // Project quat to (0x, 1y, 0z) + const VEC_Y = new Vector3(0, 1, 0); + const vec = VEC_Y.multiplyScalar( + new Vector3(quat.x, quat.y, quat.z).dot(VEC_Y) / VEC_Y.lengthSq() + ); + const yawReset = new Quaternion(vec.x, vec.y, vec.z, quat.w).normalize(); - skeletonGroup.rotation.setFromQuaternion(yawReset); + skeletonGroup.rotation.setFromQuaternion(yawReset); + } else { + skeletonGroup.rotation.set(0, 0, 0); + } }; const computeUserHeight = (bones: Map) => { @@ -146,7 +163,9 @@ function initializePreview( const render = (delta: number) => { views.forEach((v) => { if (v.hidden || !renderer) return; - v.controls.update(delta); + if (v.interactive) { + v.controls.update(delta); + } const left = Math.floor(resolution.x * v.left); const bottom = Math.floor(resolution.y * v.bottom); @@ -159,8 +178,10 @@ function initializePreview( v.tween.update(); - v.camera.aspect = width / height; - v.camera.updateProjectionMatrix(); + if (!v.manualProjectionMatrix) { + v.camera.aspect = width / height; + v.camera.updateProjectionMatrix(); + } renderer.render(scene, v.camera); }); @@ -185,6 +206,7 @@ function initializePreview( const y = 1 - event.offsetY / resolution.y; views.forEach((v) => { if ( + v.interactive && x >= v.left && x <= v.left + v.width && y >= v.bottom && @@ -212,6 +234,7 @@ function initializePreview( skeleton.forEach( (bone) => bone instanceof BoneKind && bone.updateData(bones) ); + const hmd = bones.get(BodyPart.HEAD); const newHeight = computeUserHeight(bones); if (newHeight !== heightOffset) { heightOffset = newHeight; @@ -220,10 +243,16 @@ function initializePreview( }); } - const newSkeletinOffset = computeSkeletonOffset(bones); - if (newSkeletinOffset != skeletonOffset) { - skeletonOffset = newSkeletinOffset; - skeletonGroup.position.set(0, skeletonOffset, 0); + if (stabilizeSkeleton) { + const newSkeletinOffset = computeSkeletonOffset(bones); + if (newSkeletinOffset != skeletonOffset) { + skeletonOffset = newSkeletinOffset; + skeletonGroup.position.set(0, skeletonOffset, 0); + } + } else if (anchorToHmdPosition && hmd?.headPositionG) { + skeletonGroup.position.set(hmd.headPositionG.x, 0, hmd.headPositionG.z); + } else { + skeletonGroup.position.set(0, 0, 0); } }, destroy: () => { @@ -240,6 +269,8 @@ function initializePreview( height, position, hidden = false, + interactive = true, + manualProjectionMatrix = false, onHeightChange, }: { left: number; @@ -248,6 +279,8 @@ function initializePreview( height: number; position: Vector3; hidden?: boolean; + interactive?: boolean; + manualProjectionMatrix?: boolean; onHeightChange: (view: SkeletonPreviewView, newHeight: number) => void; }) => { if (!renderer) return; @@ -282,6 +315,8 @@ function initializePreview( controls, tween, hidden, + interactive, + manualProjectionMatrix, onHeightChange, }; @@ -300,9 +335,15 @@ type PreviewContext = ReturnType; function SkeletonVisualizer({ onInit, disabled = false, + showGrid = true, + stabilizeSkeleton = true, + anchorToHmdPosition = false, }: { onInit: (context: PreviewContext) => void; disabled?: boolean; + showGrid?: boolean; + stabilizeSkeleton?: boolean; + anchorToHmdPosition?: boolean; }) { const { config } = useConfig(); @@ -357,7 +398,8 @@ function SkeletonVisualizer({ previewContext.current = initializePreview( canvasRef.current, - createChildren(bones, BoneKind.root) + createChildren(bones, BoneKind.root), + { showGrid, stabilizeSkeleton, anchorToHmdPosition } ); if (!config?.devSettings.fastDataFeed) previewContext.current.setFrameInterval(1000 / LOW_FRAMERATE); @@ -379,11 +421,17 @@ function SkeletonVisualizer({ containerRef.current.removeEventListener('mouseenter', onEnter); containerRef.current.removeEventListener('mouseleave', onLeave); }; - }, [disabled]); + }, [disabled, showGrid, stabilizeSkeleton, anchorToHmdPosition]); return ( -
- +
+
); } @@ -405,28 +453,42 @@ export function SkeletonVisualizerWidget({ }, disabled = false, toggleDisabled, + showGrid = true, + stabilizeSkeleton = true, + anchorToHmdPosition = false, + className, }: { onInit?: (context: PreviewContext) => void; disabled?: boolean; toggleDisabled?: () => void; + showGrid?: boolean; + stabilizeSkeleton?: boolean; + anchorToHmdPosition?: boolean; + className?: string; }) { const { l10n } = useLocalization(); const [error, setError] = useState(false); return ( -
+
setError(true)} fallback={<>}> - +
@@ -446,7 +508,7 @@ export function SkeletonVisualizerWidget({
diff --git a/gui/src/hooks/websocket-api.ts b/gui/src/hooks/websocket-api.ts index 39fb832fcd..55edf12bb9 100644 --- a/gui/src/hooks/websocket-api.ts +++ b/gui/src/hooks/websocket-api.ts @@ -1,4 +1,11 @@ -import { createContext, useContext, useEffect, useRef, useState } from 'react'; +import { + createContext, + useCallback, + useContext, + useEffect, + useRef, + useState, +} from 'react'; import { DataFeedMessage, @@ -9,6 +16,7 @@ import { PubSubUnion, RpcMessage, RpcMessageHeaderT, + TransactionIdT, } from 'solarxr-protocol'; import { Builder, ByteBuffer } from 'flatbuffers'; @@ -22,7 +30,7 @@ export interface WebSocketApi { reconnect: () => void; useRPCPacket: (type: RpcMessage, callback: (packet: T) => void) => void; useDataFeedPacket: (type: DataFeedMessage, callback: (packet: T) => void) => void; - sendRPCPacket: (type: RpcMessage, data: RPCPacketType) => void; + sendRPCPacket: (type: RpcMessage, data: RPCPacketType, txId?: number) => void; sendDataFeedPacket: (type: DataFeedMessage, data: DataFeedPacketType) => void; usePubSubPacket: (type: PubSubUnion, callback: (packet: T) => void) => void; sendPubSubPacket: (type: PubSubUnion, data: PubSubPacketType) => void; @@ -77,9 +85,23 @@ export function useProvideWebsocketApi(): WebSocketApi { const message = MessageBundle.getRootAsMessageBundle(fbb).unpack(); message.rpcMsgs.forEach((rpcHeader) => { + const detail = + rpcHeader.messageType === RpcMessage.ConnectToWebRTCResponse + ? { + message: rpcHeader.message, + txId: rpcHeader.txId?.id ?? null, + } + : rpcHeader.message; + + if (rpcHeader.messageType === RpcMessage.ConnectToWebRTCResponse) { + console.log('[websocket-api]', 'incoming ConnectToWebRTCResponse', { + txId: rpcHeader.txId?.id ?? null, + }); + } + rpclistenerRef.current?.dispatchEvent( new CustomEvent(RpcMessage[rpcHeader.messageType], { - detail: rpcHeader.message, + detail, }) ); }); @@ -101,23 +123,45 @@ export function useProvideWebsocketApi(): WebSocketApi { }); }; - const sendRPCPacket = (type: RpcMessage, data: RPCPacketType): void => { - if (webSocketRef?.current?.readyState !== WebSocket.OPEN) return; - const fbb = new Builder(1); - - const message = new MessageBundleT(); - - const rpcHeader = new RpcMessageHeaderT(); - rpcHeader.messageType = type; - rpcHeader.message = data; - - message.rpcMsgs = [rpcHeader]; - fbb.finish(message.pack(fbb)); - - webSocketRef.current.send(fbb.asUint8Array()); - - rpcPacketCounterRef.current++; - }; + const sendRPCPacket = useCallback( + (type: RpcMessage, data: RPCPacketType, txId?: number): void => { + if (webSocketRef?.current?.readyState !== WebSocket.OPEN) { + if (type === RpcMessage.ConnectToWebRTCRequest) { + console.warn('[websocket-api]', 'sendRPCPacket skipped: WebSocket not OPEN', { + txId, + }); + } + return; + } + const fbb = new Builder(1); + + const message = new MessageBundleT(); + + const rpcHeader = new RpcMessageHeaderT(); + rpcHeader.messageType = type; + rpcHeader.message = data; + if (txId !== undefined) { + rpcHeader.txId = new TransactionIdT(txId >>> 0); + } + + message.rpcMsgs = [rpcHeader]; + fbb.finish(message.pack(fbb)); + + const payload = fbb.asUint8Array(); + webSocketRef.current!.send(payload); + + if (type === RpcMessage.ConnectToWebRTCRequest) { + console.log('[websocket-api]', 'sendRPCPacket', { + type: 'ConnectToWebRTCRequest', + txId, + bytes: payload.byteLength, + }); + } + + rpcPacketCounterRef.current++; + }, + [] + ); const sendDataFeedPacket = ( type: DataFeedMessage, diff --git a/gui/src/sounds/sounds.ts b/gui/src/sounds/sounds.ts index 8d9206619c..9454cfc020 100644 --- a/gui/src/sounds/sounds.ts +++ b/gui/src/sounds/sounds.ts @@ -53,6 +53,8 @@ const resetSounds: Record< export const trackingPauseSound = createAudio('/sounds/tracking/pause.ogg'); export const trackingPlaySound = createAudio('/sounds/tracking/play.ogg'); +// Used as a short "reset chime" for UI feedback. +export const resetChimeSound = createAudio('/sounds/yaw-reset/yaw-reset.ogg'); let lastTap = 0; export async function playTapSetupSound(volume = 1) { diff --git a/gui/vite.config.ts b/gui/vite.config.ts index a09fed7b64..462a066fee 100644 --- a/gui/vite.config.ts +++ b/gui/vite.config.ts @@ -34,6 +34,90 @@ export function i18nHotReload(): PluginOption { }; } +export function videoCalibrationProxy(): PluginOption { + function buildWebcamOfferUrl(host: string, port: number) { + const normalizedHost = + host.includes(':') && !host.startsWith('[') ? `[${host}]` : host; + + return `http://${normalizedHost}:${port}/offer`; + } + + async function handleRequest(req: NodeJS.ReadableStream) { + const chunks: Buffer[] = []; + + for await (const chunk of req) { + chunks.push(Buffer.isBuffer(chunk) ? chunk : Buffer.from(chunk)); + } + + const body = JSON.parse(Buffer.concat(chunks).toString('utf8')) as { + host?: unknown; + port?: unknown; + sdp?: unknown; + }; + + if ( + typeof body.host !== 'string' || + typeof body.port !== 'number' || + typeof body.sdp !== 'string' + ) { + throw new Error('Invalid webcam offer payload'); + } + + const response = await fetch(buildWebcamOfferUrl(body.host, body.port), { + method: 'POST', + headers: { + 'Content-Type': 'application/json', + }, + body: JSON.stringify({ + sdp: body.sdp, + }), + }); + + if (!response.ok) { + throw new Error(`Offer request failed with status ${response.status}`); + } + + const responseBody = (await response.json()) as { sdp?: unknown }; + if (typeof responseBody.sdp !== 'string' || responseBody.sdp.length === 0) { + throw new Error('Webcam response did not contain an SDP answer'); + } + + return JSON.stringify({ + sdp: responseBody.sdp, + }); + } + + return { + name: 'video-calibration-proxy', + configureServer(server) { + server.middlewares.use('/video-calibration-api/offer', async (req, res) => { + if (req.method !== 'POST') { + res.statusCode = 405; + res.setHeader('Content-Type', 'application/json'); + res.end(JSON.stringify({ error: 'Method not allowed' })); + return; + } + + try { + const responseBody = await handleRequest(req); + res.statusCode = 200; + res.setHeader('Content-Type', 'application/json'); + res.end(responseBody); + } catch (error) { + res.statusCode = 502; + res.setHeader('Content-Type', 'application/json'); + res.end( + JSON.stringify({ + error: + error instanceof Error ? error.message : 'Unknown webcam proxy error', + }) + ); + } + }); + }, + }; +} + // https://vitejs.dev/config/ export default defineConfig({ define: { @@ -44,6 +128,7 @@ export default defineConfig({ plugins: [ react({ babel: { plugins: [jotaiReactRefresh] } }), i18nHotReload(), + videoCalibrationProxy(), visualizer() as PluginOption, sentryVitePlugin({ org: 'slimevr', diff --git a/package.json b/package.json index 51a7f97260..65d08cee4e 100644 --- a/package.json +++ b/package.json @@ -8,7 +8,7 @@ "gui" ], "scripts": { - "gui": "pnpm run update-solarxr && cd gui && pnpm run gui", + "gui": "pnpm run update-solarxr && cd gui && pnpm run dev:clean", "lint:fix": "cd gui && pnpm lint:fix", "skipbundler": "cd gui && pnpm run skipbundler", "build": "cd gui && pnpm build", diff --git a/server/core/build.gradle.kts b/server/core/build.gradle.kts index 0f34ebb915..eea5dd8116 100644 --- a/server/core/build.gradle.kts +++ b/server/core/build.gradle.kts @@ -65,9 +65,13 @@ dependencies { implementation("com.google.flatbuffers:flatbuffers-java:22.10.26") implementation("commons-cli:commons-cli:1.11.0") implementation("com.fasterxml.jackson.core:jackson-databind:2.21.0") + implementation("com.fasterxml.jackson.module:jackson-module-kotlin:2.21.0") implementation("com.fasterxml.jackson.dataformat:jackson-dataformat-yaml:2.21.0") - implementation("com.github.jonpeterson:jackson-module-model-versioning:1.2.2") + implementation( + "com.github.jonpeterso" + + "n:jackson-module-model-versioning:1.2.2", + ) implementation("org.apache.commons:commons-math3:3.6.1") implementation("org.apache.commons:commons-lang3:3.20.0") implementation("org.apache.commons:commons-collections4:4.5.0") @@ -79,6 +83,15 @@ dependencies { implementation("org.jetbrains.kotlinx:kotlinx-coroutines-core:1.10.2") implementation("com.mayakapps.kache:kache:2.1.1") + implementation("com.microsoft.onnxruntime:onnxruntime:1.24.3") + implementation("dev.onvoid.webrtc:webrtc-java:0.14.0") + implementation(group = "dev.onvoid.webrtc", name = "webrtc-java", version = "0.14.0", classifier = "windows-x86_64") + implementation("org.jmdns:jmdns:3.6.3") + implementation("io.ktor:ktor-client-core:3.4.1") + implementation("io.ktor:ktor-client-cio:3.4.1") + implementation("io.ktor:ktor-client-content-negotiation:3.4.1") + implementation("io.ktor:ktor-serialization-kotlinx-json:3.4.1") + api("com.github.loucass003:EspflashKotlin:v0.11.0") // Allow the use of reflection diff --git a/server/core/resources/rtmpose-m_simcc-body7_pt-body7_420e-256x192-e48f03d0_20230504.onnx b/server/core/resources/rtmpose-m_simcc-body7_pt-body7_420e-256x192-e48f03d0_20230504.onnx new file mode 100644 index 0000000000..9980016473 Binary files /dev/null and b/server/core/resources/rtmpose-m_simcc-body7_pt-body7_420e-256x192-e48f03d0_20230504.onnx differ diff --git a/server/core/src/main/java/dev/slimevr/ServerTickListener.kt b/server/core/src/main/java/dev/slimevr/ServerTickListener.kt new file mode 100644 index 0000000000..68676ced25 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/ServerTickListener.kt @@ -0,0 +1,5 @@ +package dev.slimevr + +interface ServerTickListener { + fun onTick() +} diff --git a/server/core/src/main/java/dev/slimevr/VRServer.kt b/server/core/src/main/java/dev/slimevr/VRServer.kt index 4db758d06d..b5198a3a3b 100644 --- a/server/core/src/main/java/dev/slimevr/VRServer.kt +++ b/server/core/src/main/java/dev/slimevr/VRServer.kt @@ -31,6 +31,9 @@ import dev.slimevr.tracking.processor.HumanPoseManager import dev.slimevr.tracking.processor.skeleton.HumanSkeleton import dev.slimevr.tracking.trackers.* import dev.slimevr.tracking.trackers.udp.TrackersUDPServer +import dev.slimevr.tracking.videocalibration.VideoCalibrationService +import dev.slimevr.tracking.videocalibration.networking.MDNSRegistry +import dev.slimevr.tracking.videocalibration.networking.WebRTCManager import dev.slimevr.trackingchecklist.TrackingChecklistManager import dev.slimevr.util.ann.VRServerThread import dev.slimevr.websocketapi.WebSocketVRBridge @@ -73,6 +76,8 @@ class VRServer @JvmOverloads constructor( private val newTrackersConsumers: MutableList> = FastList() private val trackerStatusListeners: MutableList = FastList() private val onTick: MutableList = FastList() + private val onTickListeners = FastList() + private val tickListenersToRemove = mutableListOf() private val lock = acquireMulticastLock() val oSCRouter: OSCRouter @@ -125,6 +130,10 @@ class VRServer @JvmOverloads constructor( val serverGuards = ServerGuards() + val mdnsRegistry = MDNSRegistry() + val webRTCManager = WebRTCManager() + var videoCalibrationService: VideoCalibrationService? = null + init { // UwU deviceManager = DeviceManager(this) @@ -208,6 +217,14 @@ class VRServer @JvmOverloads constructor( onTick.add(runnable) } + fun addTickListener(listener: ServerTickListener) { + onTickListeners.add(listener) + } + + fun removeTickListener(listener: ServerTickListener) { + tickListenersToRemove.add(listener) + } + @ThreadSafe fun addNewTrackerConsumer(consumer: Consumer) { queueTask { @@ -237,6 +254,7 @@ class VRServer @JvmOverloads constructor( @VRServerThread override fun run() { trackersServer.start() + mdnsRegistry.start() while (true) { // final long start = System.currentTimeMillis(); fpsTimer.update() @@ -247,6 +265,11 @@ class VRServer @JvmOverloads constructor( for (task in onTick) { task.run() } + for (listener in onTickListeners) { + listener.onTick() + } + onTickListeners.removeAll(tickListenersToRemove) + tickListenersToRemove.clear() for (bridge in bridges) { bridge.dataRead() } @@ -259,6 +282,7 @@ class VRServer @JvmOverloads constructor( } vrcOSCHandler.update() vMCHandler.update() + videoCalibrationService?.onTick() // final long time = System.currentTimeMillis() - start; try { sleep(1) // 1000Hz diff --git a/server/core/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.kt b/server/core/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.kt index d1c63d76f0..04027117ce 100644 --- a/server/core/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.kt +++ b/server/core/src/main/java/dev/slimevr/protocol/rpc/RPCHandler.kt @@ -27,6 +27,9 @@ import dev.slimevr.tracking.trackers.TrackerPosition import dev.slimevr.tracking.trackers.TrackerPosition.Companion.getByBodyPart import dev.slimevr.tracking.trackers.TrackerStatus import dev.slimevr.tracking.trackers.TrackerUtils.getTrackerForSkeleton +import dev.slimevr.tracking.videocalibration.VideoCalibrationService +import dev.slimevr.tracking.videocalibration.networking.MDNSRegistry +import dev.slimevr.tracking.videocalibration.networking.WebRTCManager import io.eiren.util.logging.LogManager import io.github.axisangles.ktmath.Quaternion import kotlinx.coroutines.* @@ -34,9 +37,11 @@ import solarxr_protocol.MessageBundle import solarxr_protocol.datatypes.TransactionId import solarxr_protocol.rpc.* import kotlin.io.path.Path +import kotlin.time.Duration.Companion.seconds class RPCHandler(private val api: ProtocolAPI) : ProtocolHandler() { private val mainScope = CoroutineScope(SupervisorJob()) + private val ioScope = CoroutineScope(Dispatchers.IO + SupervisorJob()) init { RPCResetHandler(this, api) @@ -149,6 +154,21 @@ class RPCHandler(private val api: ProtocolAPI) : ProtocolHandler WebRTCManager.VideoProvider.VIDEO_CALIBRATION + + else -> { + LogManager.warning("ConnectToWebRTC is missing or has unknown WebRTC video provider: ${request.provider()}") + return + } + } + + val offerSDP = request.offerSdp() + if (offerSDP.isNullOrEmpty()) { + LogManager.warning("ConnectToWebRTC request is missing offer SDP") + } + + // TODO: Is GenericConnection thread safe? + ioScope.launch { + val answerSDP: String + try { + answerSDP = api.server.webRTCManager.connect(provider, offerSDP) + } catch (e: Exception) { + LogManager.warning("Failed to create WebRTC answer for client ${conn.connectionId}: $e", e) + sendConnectToWebRTCResponse(conn, e, messageHeader) + return@launch + } + LogManager.info("Sending WebRTC answer to client ${conn.connectionId}...") + sendConnectToWebRTCResponse(conn, answerSDP, messageHeader) + } + } + + private fun sendConnectToWebRTCResponse(conn: GenericConnection, answerSDP: String, respondTo: RpcMessageHeader) { + val fbb = FlatBufferBuilder(512) + val answerSDPOffset = fbb.createString(answerSDP) + val responseOffset = ConnectToWebRTCResponse.createConnectToWebRTCResponse(fbb, answerSDPOffset, 0) + val messageOffset = createRPCMessage(fbb, RpcMessage.ConnectToWebRTCResponse, responseOffset, respondTo) + fbb.finish(messageOffset) + conn.send(fbb.dataBuffer()) + } + + private fun sendConnectToWebRTCResponse(conn: GenericConnection, error: Exception, respondTo: RpcMessageHeader) { + val fbb = FlatBufferBuilder(512) + val errorOffset = fbb.createString(error.message) + val responseOffset = ConnectToWebRTCResponse.createConnectToWebRTCResponse(fbb, 0, errorOffset) + val messageOffset = createRPCMessage(fbb, RpcMessage.ConnectToWebRTCResponse, responseOffset, respondTo) + fbb.finish(messageOffset) + conn.send(fbb.dataBuffer()) + } + + private fun onStartVideoTrackerCalibrationRequest(conn: GenericConnection, messageHeader: RpcMessageHeader) { + LogManager.info("Received video calibration request...") + + val webcamService = api.server.mdnsRegistry.findService(MDNSRegistry.ServiceType.WEBCAM) + if (webcamService == null) { + LogManager.warning("Webcam service is not available") + val fbb = FlatBufferBuilder(512) + val errorOffset = fbb.createString("Webcam service is not available") + val progressOffset = + VideoTrackerCalibrationProgressResponse.createVideoTrackerCalibrationProgressResponse( + fbb, + VideoTrackerCalibrationStatus.DONE, + 0, + 0, + 0, + errorOffset, + ) + val messageOffset = createRPCMessage(fbb, RpcMessage.VideoTrackerCalibrationProgressResponse, progressOffset) + fbb.finish(messageOffset) + conn.send(fbb.dataBuffer()) + return + } + + // TODO: Is this being run on the server thread? + val videoCalibrator: VideoCalibrationService + try { + videoCalibrator = VideoCalibrationService(api.server, webcamService, conn) + } catch (e: Exception) { + LogManager.warning("Failed to create video calibration service", e) + val fbb = FlatBufferBuilder(512) + val errorOffset = fbb.createString("Failed to create video calibration service: $e") + val progressOffset = + VideoTrackerCalibrationProgressResponse.createVideoTrackerCalibrationProgressResponse( + fbb, + VideoTrackerCalibrationStatus.DONE, + 0, + 0, + 0, + errorOffset, + ) + val messageOffset = createRPCMessage(fbb, RpcMessage.VideoTrackerCalibrationProgressResponse, progressOffset) + fbb.finish(messageOffset) + conn.send(fbb.dataBuffer()) + return + } + + videoCalibrator.start(120.seconds) + } + + private fun onCancelVideoTrackerCalibrationRequest(conn: GenericConnection, messageHeader: RpcMessageHeader) { + // TODO: Is this being run on the server thread? + api.server.videoCalibrationService?.requestStop() + } + fun sendSettingsChangedResponse(conn: GenericConnection, messageHeader: RpcMessageHeader?) { val fbb = FlatBufferBuilder(32) val settings = createSettingsResponse(fbb, api.server) diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt index f1fa122695..8b240594be 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/HumanPoseManager.kt @@ -30,7 +30,7 @@ import kotlin.math.* class HumanPoseManager(val server: VRServer?) { val computedTrackers: MutableList = FastList() private val onSkeletonUpdated: MutableList> = FastList() - private val skeletonConfigManager = SkeletonConfigManager(true, this) + val skeletonConfigManager = SkeletonConfigManager(true, this) @get:ThreadSafe lateinit var skeleton: HumanSkeleton diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.kt index 45ea6a0548..33163440a8 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigManager.kt @@ -14,7 +14,7 @@ class SkeletonConfigManager( private val autoUpdateOffsets: Boolean, private val humanPoseManager: HumanPoseManager? = null, ) { - private val configOffsets: EnumMap = EnumMap( + val configOffsets: EnumMap = EnumMap( SkeletonConfigOffsets::class.java, ) private val configToggles: EnumMap = EnumMap( diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/refactor/Skeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/refactor/Skeleton.kt new file mode 100644 index 0000000000..ebab1563ae --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/refactor/Skeleton.kt @@ -0,0 +1,161 @@ +package dev.slimevr.tracking.processor.skeleton.refactor + +import dev.slimevr.tracking.processor.Bone +import dev.slimevr.tracking.processor.BoneType +import dev.slimevr.tracking.processor.Constraint +import dev.slimevr.tracking.processor.Constraint.Companion.ConstraintType + +class Skeleton( + val isTrackingLeftArmFromController: Boolean, + val isTrackingRightArmFromController: Boolean, +) { + // Upper body bones + val headBone = Bone(BoneType.HEAD, Constraint(ConstraintType.COMPLETE)) + val neckBone = Bone(BoneType.NECK, Constraint(ConstraintType.COMPLETE)) + val upperChestBone = Bone(BoneType.UPPER_CHEST, Constraint(ConstraintType.TWIST_SWING, 90f, 120f)) + val chestBone = Bone(BoneType.CHEST, Constraint(ConstraintType.TWIST_SWING, 60f, 120f)) + val waistBone = Bone(BoneType.WAIST, Constraint(ConstraintType.TWIST_SWING, 60f, 120f)) + val hipBone = Bone(BoneType.HIP, Constraint(ConstraintType.TWIST_SWING, 60f, 120f)) + + // Lower body bones + val leftHipBone = Bone(BoneType.LEFT_HIP, Constraint(ConstraintType.TWIST_SWING, 0f, 15f)) + val rightHipBone = Bone(BoneType.RIGHT_HIP, Constraint(ConstraintType.TWIST_SWING, 0f, 15f)) + val leftUpperLegBone = Bone(BoneType.LEFT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 120f, 180f)) + val rightUpperLegBone = Bone(BoneType.RIGHT_UPPER_LEG, Constraint(ConstraintType.TWIST_SWING, 120f, 180f)) + val leftLowerLegBone = Bone(BoneType.LEFT_LOWER_LEG, Constraint(ConstraintType.LOOSE_HINGE, 180f, 0f, 50f)) + val rightLowerLegBone = Bone(BoneType.RIGHT_LOWER_LEG, Constraint(ConstraintType.LOOSE_HINGE, 180f, 0f, 50f)) + val leftFootBone = Bone(BoneType.LEFT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 60f)) + val rightFootBone = Bone(BoneType.RIGHT_FOOT, Constraint(ConstraintType.TWIST_SWING, 60f, 60f)) + + // Arm bones + val leftUpperShoulderBone = Bone(BoneType.LEFT_UPPER_SHOULDER, Constraint(ConstraintType.COMPLETE)) + val rightUpperShoulderBone = Bone(BoneType.RIGHT_UPPER_SHOULDER, Constraint(ConstraintType.COMPLETE)) + val leftShoulderBone = Bone(BoneType.LEFT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 0f, 30f)) + val rightShoulderBone = Bone(BoneType.RIGHT_SHOULDER, Constraint(ConstraintType.TWIST_SWING, 0f, 30f)) + val leftUpperArmBone = Bone(BoneType.LEFT_UPPER_ARM, Constraint(ConstraintType.TWIST_SWING, 120f, 180f)) + val rightUpperArmBone = Bone(BoneType.RIGHT_UPPER_ARM, Constraint(ConstraintType.TWIST_SWING, 120f, 180f)) + val leftLowerArmBone = Bone(BoneType.LEFT_LOWER_ARM, Constraint(ConstraintType.LOOSE_HINGE, 0f, -180f, 40f)) + val rightLowerArmBone = Bone(BoneType.RIGHT_LOWER_ARM, Constraint(ConstraintType.LOOSE_HINGE, 0f, -180f, 40f)) + val leftHandBone = Bone(BoneType.LEFT_HAND, Constraint(ConstraintType.TWIST_SWING, 120f, 120f)) + val rightHandBone = Bone(BoneType.RIGHT_HAND, Constraint(ConstraintType.TWIST_SWING, 120f, 120f)) + + // Tracker bones + val headTrackerBone = Bone(BoneType.HEAD_TRACKER, Constraint(ConstraintType.COMPLETE)) + val chestTrackerBone = Bone(BoneType.CHEST_TRACKER, Constraint(ConstraintType.COMPLETE)) + val hipTrackerBone = Bone(BoneType.HIP_TRACKER, Constraint(ConstraintType.COMPLETE)) + val leftKneeTrackerBone = Bone(BoneType.LEFT_KNEE_TRACKER, Constraint(ConstraintType.COMPLETE)) + val rightKneeTrackerBone = Bone(BoneType.RIGHT_KNEE_TRACKER, Constraint(ConstraintType.COMPLETE)) + val leftFootTrackerBone = Bone(BoneType.LEFT_FOOT_TRACKER, Constraint(ConstraintType.COMPLETE)) + val rightFootTrackerBone = Bone(BoneType.RIGHT_FOOT_TRACKER, Constraint(ConstraintType.COMPLETE)) + val leftElbowTrackerBone = Bone(BoneType.LEFT_ELBOW_TRACKER, Constraint(ConstraintType.COMPLETE)) + val rightElbowTrackerBone = Bone(BoneType.RIGHT_ELBOW_TRACKER, Constraint(ConstraintType.COMPLETE)) + val leftHandTrackerBone = Bone(BoneType.LEFT_HAND_TRACKER, Constraint(ConstraintType.COMPLETE)) + val rightHandTrackerBone = Bone(BoneType.RIGHT_HAND_TRACKER, Constraint(ConstraintType.COMPLETE)) + + init { + assembleSkeleton() + } + + private fun assembleSkeleton() { + // Assemble upper skeleton (head to hip) + headBone.attachChild(neckBone) + neckBone.attachChild(upperChestBone) + upperChestBone.attachChild(chestBone) + chestBone.attachChild(waistBone) + waistBone.attachChild(hipBone) + + // Assemble lower skeleton (hip to feet) + hipBone.attachChild(leftHipBone) + hipBone.attachChild(rightHipBone) + leftHipBone.attachChild(leftUpperLegBone) + rightHipBone.attachChild(rightUpperLegBone) + leftUpperLegBone.attachChild(leftLowerLegBone) + rightUpperLegBone.attachChild(rightLowerLegBone) + leftLowerLegBone.attachChild(leftFootBone) + rightLowerLegBone.attachChild(rightFootBone) + + // Attach tracker bones for tracker offsets + neckBone.attachChild(headTrackerBone) + upperChestBone.attachChild(chestTrackerBone) + hipBone.attachChild(hipTrackerBone) + leftUpperLegBone.attachChild(leftKneeTrackerBone) + rightUpperLegBone.attachChild(rightKneeTrackerBone) + leftFootBone.attachChild(leftFootTrackerBone) + rightFootBone.attachChild(rightFootTrackerBone) + + assembleSkeletonArms() + } + + private fun assembleSkeletonArms() { + // Shoulders + neckBone.attachChild(leftUpperShoulderBone) + neckBone.attachChild(rightUpperShoulderBone) + leftUpperShoulderBone.attachChild(leftShoulderBone) + rightUpperShoulderBone.attachChild(rightShoulderBone) + + // Upper arm + leftShoulderBone.attachChild(leftUpperArmBone) + rightShoulderBone.attachChild(rightUpperArmBone) + + // Lower arm and hand + if (isTrackingLeftArmFromController) { + leftHandTrackerBone.attachChild(leftHandBone) + leftHandBone.attachChild(leftLowerArmBone) + leftLowerArmBone.attachChild(leftElbowTrackerBone) + } else { + leftUpperArmBone.attachChild(leftLowerArmBone) + leftUpperArmBone.attachChild(leftElbowTrackerBone) + leftLowerArmBone.attachChild(leftHandBone) + leftHandBone.attachChild(leftHandTrackerBone) + } + if (isTrackingRightArmFromController) { + rightHandTrackerBone.attachChild(rightHandBone) + rightHandBone.attachChild(rightLowerArmBone) + rightLowerArmBone.attachChild(rightElbowTrackerBone) + } else { + rightUpperArmBone.attachChild(rightLowerArmBone) + rightUpperArmBone.attachChild(rightElbowTrackerBone) + rightLowerArmBone.attachChild(rightHandBone) + rightHandBone.attachChild(rightHandTrackerBone) + } + } + + fun getBone(bone: BoneType): Bone? = when (bone) { + BoneType.HEAD -> headBone + BoneType.HEAD_TRACKER -> headTrackerBone + BoneType.NECK -> neckBone + BoneType.UPPER_CHEST -> upperChestBone + BoneType.CHEST_TRACKER -> chestTrackerBone + BoneType.CHEST -> chestBone + BoneType.WAIST -> waistBone + BoneType.HIP -> hipBone + BoneType.HIP_TRACKER -> hipTrackerBone + BoneType.LEFT_HIP -> leftHipBone + BoneType.RIGHT_HIP -> rightHipBone + BoneType.LEFT_UPPER_LEG -> leftUpperLegBone + BoneType.RIGHT_UPPER_LEG -> rightUpperLegBone + BoneType.LEFT_KNEE_TRACKER -> leftKneeTrackerBone + BoneType.RIGHT_KNEE_TRACKER -> rightKneeTrackerBone + BoneType.LEFT_LOWER_LEG -> leftLowerLegBone + BoneType.RIGHT_LOWER_LEG -> rightLowerLegBone + BoneType.LEFT_FOOT -> leftFootBone + BoneType.RIGHT_FOOT -> rightFootBone + BoneType.LEFT_FOOT_TRACKER -> leftFootTrackerBone + BoneType.RIGHT_FOOT_TRACKER -> rightFootTrackerBone + BoneType.LEFT_UPPER_SHOULDER -> leftUpperShoulderBone + BoneType.RIGHT_UPPER_SHOULDER -> rightUpperShoulderBone + BoneType.LEFT_SHOULDER -> leftShoulderBone + BoneType.RIGHT_SHOULDER -> rightShoulderBone + BoneType.LEFT_UPPER_ARM -> leftUpperArmBone + BoneType.RIGHT_UPPER_ARM -> rightUpperArmBone + BoneType.LEFT_ELBOW_TRACKER -> leftElbowTrackerBone + BoneType.RIGHT_ELBOW_TRACKER -> rightElbowTrackerBone + BoneType.LEFT_LOWER_ARM -> leftLowerArmBone + BoneType.RIGHT_LOWER_ARM -> rightLowerArmBone + BoneType.LEFT_HAND -> leftHandBone + BoneType.RIGHT_HAND -> rightHandBone + BoneType.LEFT_HAND_TRACKER -> leftHandTrackerBone + BoneType.RIGHT_HAND_TRACKER -> rightHandTrackerBone + else -> null + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/refactor/SkeletonUpdater.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/refactor/SkeletonUpdater.kt new file mode 100644 index 0000000000..4513e2e868 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/refactor/SkeletonUpdater.kt @@ -0,0 +1,718 @@ +package dev.slimevr.tracking.processor.skeleton.refactor + +import dev.slimevr.tracking.processor.Bone +import dev.slimevr.tracking.processor.BoneType +import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets +import dev.slimevr.tracking.trackers.TrackerPosition +import dev.slimevr.tracking.videocalibration.snapshots.TrackerSnapshot +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 + +class SkeletonUpdater( + val skeleton: Skeleton, + val trackers: TrackersData, + val config: HumanSkeletonConfig, + val skeletonOffsets: Map, +) { + + class TrackerData( + val rotation: Quaternion, + val position: Vector3?, + ) { + companion object { + fun fromSnapshot(snapshot: TrackerSnapshot) = TrackerData( + snapshot.adjustedTrackerToWorld.toFloat(), + snapshot.trackerOriginInWorld?.toFloat(), + ) + } + } + + // TODO: Just use this instead of TrackersSnapshot + class TrackersData( + val head: TrackerData?, + val neck: TrackerData?, + val upperChest: TrackerData?, + val chest: TrackerData?, + val waist: TrackerData?, + val hip: TrackerData?, + val leftUpperLeg: TrackerData?, + val leftLowerLeg: TrackerData?, + val leftFoot: TrackerData?, + val rightUpperLeg: TrackerData?, + val rightLowerLeg: TrackerData?, + val rightFoot: TrackerData?, + val leftShoulder: TrackerData?, + val leftUpperArm: TrackerData?, + val leftLowerArm: TrackerData?, + val leftHand: TrackerData?, + val rightShoulder: TrackerData?, + val rightUpperArm: TrackerData?, + val rightLowerArm: TrackerData?, + val rightHand: TrackerData?, + ) { + companion object { + fun fromSnapshot(trackers: Map) = TrackersData( + head = trackers[TrackerPosition.HEAD]?.let(TrackerData::fromSnapshot), + neck = trackers[TrackerPosition.NECK]?.let(TrackerData::fromSnapshot), + upperChest = trackers[TrackerPosition.UPPER_CHEST]?.let(TrackerData::fromSnapshot), + chest = trackers[TrackerPosition.CHEST]?.let(TrackerData::fromSnapshot), + waist = trackers[TrackerPosition.WAIST]?.let(TrackerData::fromSnapshot), + hip = trackers[TrackerPosition.HIP]?.let(TrackerData::fromSnapshot), + leftUpperLeg = trackers[TrackerPosition.LEFT_UPPER_LEG]?.let(TrackerData::fromSnapshot), + leftLowerLeg = trackers[TrackerPosition.LEFT_LOWER_LEG]?.let(TrackerData::fromSnapshot), + leftFoot = trackers[TrackerPosition.LEFT_FOOT]?.let(TrackerData::fromSnapshot), + rightUpperLeg = trackers[TrackerPosition.RIGHT_UPPER_LEG]?.let(TrackerData::fromSnapshot), + rightLowerLeg = trackers[TrackerPosition.RIGHT_LOWER_LEG]?.let(TrackerData::fromSnapshot), + rightFoot = trackers[TrackerPosition.RIGHT_FOOT]?.let(TrackerData::fromSnapshot), + leftShoulder = trackers[TrackerPosition.LEFT_SHOULDER]?.let(TrackerData::fromSnapshot), + leftUpperArm = trackers[TrackerPosition.LEFT_UPPER_ARM]?.let(TrackerData::fromSnapshot), + leftLowerArm = trackers[TrackerPosition.LEFT_LOWER_ARM]?.let(TrackerData::fromSnapshot), + leftHand = trackers[TrackerPosition.LEFT_HAND]?.let(TrackerData::fromSnapshot), + rightShoulder = trackers[TrackerPosition.RIGHT_SHOULDER]?.let(TrackerData::fromSnapshot), + rightUpperArm = trackers[TrackerPosition.RIGHT_UPPER_ARM]?.let(TrackerData::fromSnapshot), + rightLowerArm = trackers[TrackerPosition.RIGHT_LOWER_ARM]?.let(TrackerData::fromSnapshot), + rightHand = trackers[TrackerPosition.RIGHT_HAND]?.let(TrackerData::fromSnapshot), + ) + } + } + + class HumanSkeletonConfig( + val extendedSpineModel: Boolean = true, + val extendedPelvisModel: Boolean = true, + val extendedKneeModel: Boolean = true, + val waistFromChestHipAveraging: Float = 0.3f, + val waistFromChestLegsAveraging: Float = 0.3f, + val hipFromChestLegsAveraging: Float = 0.5f, + val hipFromWaistLegsAveraging: Float = 0.4f, + val hipLegsAveraging: Float = 0.25f, + val kneeTrackerAnkleAveraging: Float = 0.85f, + val kneeAnkleAveraging: Float = 0.0f, + ) + + fun update() { + updateBoneLengths() + updateBoneRotations() + updateBonePositions() + } + + /** + * Update all the bones' transforms from trackers + */ + private fun updateBoneRotations() { + // Head + updateHeadTransforms() + + // Spine + updateSpineTransforms() + + // Left leg + updateLegTransforms( + skeleton.leftUpperLegBone, + skeleton.leftKneeTrackerBone, + skeleton.leftLowerLegBone, + skeleton.leftFootBone, + skeleton.leftFootTrackerBone, + trackers.leftUpperLeg, + trackers.leftLowerLeg, + trackers.leftFoot, + ) + + // Right leg + updateLegTransforms( + skeleton.rightUpperLegBone, + skeleton.rightKneeTrackerBone, + skeleton.rightLowerLegBone, + skeleton.rightFootBone, + skeleton.rightFootTrackerBone, + trackers.rightUpperLeg, + trackers.rightLowerLeg, + trackers.rightFoot, + ) + + // Left arm + updateArmTransforms( + skeleton.isTrackingLeftArmFromController, + skeleton.leftUpperShoulderBone, + skeleton.leftShoulderBone, + skeleton.leftUpperArmBone, + skeleton.leftElbowTrackerBone, + skeleton.leftLowerArmBone, + skeleton.leftHandBone, + skeleton.leftHandTrackerBone, + trackers.leftShoulder, + trackers.leftUpperArm, + trackers.leftLowerArm, + trackers.leftHand, + ) + + // Right arm + updateArmTransforms( + skeleton.isTrackingRightArmFromController, + skeleton.rightUpperShoulderBone, + skeleton.rightShoulderBone, + skeleton.rightUpperArmBone, + skeleton.rightElbowTrackerBone, + skeleton.rightLowerArmBone, + skeleton.rightHandBone, + skeleton.rightHandTrackerBone, + trackers.rightShoulder, + trackers.rightUpperArm, + trackers.rightLowerArm, + trackers.rightHand, + ) + } + + /** + * Update the head and neck bone transforms + */ + private fun updateHeadTransforms() { + trackers.head?.let { head -> + // Set head position + head.position?.let { skeleton.headBone.setPosition(it) } + + // Get head rotation + var headRot = head.rotation + + // Set head rotation + skeleton.headBone.setRotation(headRot) + skeleton.headTrackerBone.setRotation(headRot) + + // Get neck rotation + trackers.neck?.let { headRot = it.rotation } + + // Set neck rotation + skeleton.neckBone.setRotation(headRot) + } + } + + /** + * Update the spine transforms, from the upper chest to the hip + */ + private fun updateSpineTransforms() { + val hasSpineTracker = getFirstAvailableTracker(trackers.upperChest, trackers.chest, trackers.waist, trackers.hip) != null + val hasKneeTrackers = getFirstAvailableTracker(trackers.leftUpperLeg, trackers.rightUpperLeg) != null + + if (hasSpineTracker) { + // Upper chest and chest tracker + getFirstAvailableTracker(trackers.upperChest, trackers.chest, trackers.waist, trackers.hip)?.let { + skeleton.upperChestBone.setRotation(it.rotation) + skeleton.chestTrackerBone.setRotation(it.rotation) + } + + // Chest + getFirstAvailableTracker(trackers.chest, trackers.upperChest, trackers.waist, trackers.hip)?.let { + skeleton.chestBone.setRotation(it.rotation) + } + + // Waist + getFirstAvailableTracker(trackers.waist, trackers.chest, trackers.hip, trackers.upperChest)?.let { + skeleton.waistBone.setRotation(it.rotation) + } + + // Hip and hip tracker + getFirstAvailableTracker(trackers.hip, trackers.waist, trackers.chest, trackers.upperChest)?.let { + skeleton.hipBone.setRotation(it.rotation) + skeleton.hipTrackerBone.setRotation(it.rotation) + } + } else if (trackers.head != null) { + // Align with neck's yaw + val yawRot = skeleton.neckBone.getGlobalRotation().project(Vector3.Companion.POS_Y).unit() + skeleton.upperChestBone.setRotation(yawRot) + skeleton.chestTrackerBone.setRotation(yawRot) + skeleton.chestBone.setRotation(yawRot) + skeleton.waistBone.setRotation(yawRot) + skeleton.hipBone.setRotation(yawRot) + skeleton.hipTrackerBone.setRotation(yawRot) + } + + // Extended spine model + if (config.extendedSpineModel && hasSpineTracker) { + // Tries to guess missing lower spine trackers by interpolating rotations + if (trackers.waist == null) { + getFirstAvailableTracker(trackers.chest, trackers.upperChest)?.let { chest -> + trackers.hip?.let { + // Calculates waist from chest + hip + var hipRot = it.rotation + var chestRot = chest.rotation + + // Interpolate between the chest and the hip + chestRot = chestRot.interpQ(hipRot, config.waistFromChestHipAveraging) + + // Set waist's rotation + skeleton.waistBone.setRotation(chestRot) + } ?: run { + if (hasKneeTrackers) { + // Calculates waist from chest + legs + var leftLegRot = trackers.leftUpperLeg?.rotation ?: Quaternion.Companion.IDENTITY + var rightLegRot = trackers.rightUpperLeg?.rotation ?: Quaternion.Companion.IDENTITY + var chestRot = chest.rotation + + // Interpolate between the pelvis, averaged from the legs, and the chest + chestRot = chestRot.interpQ(leftLegRot.lerpQ(rightLegRot, 0.5f), config.waistFromChestLegsAveraging).unit() + + // Set waist's rotation + skeleton.waistBone.setRotation(chestRot) + } + } + } + } + if (trackers.hip == null && hasKneeTrackers) { + trackers.waist?.let { + // Calculates hip from waist + legs + var leftLegRot = trackers.leftUpperLeg?.rotation ?: Quaternion.Companion.IDENTITY + var rightLegRot = trackers.rightUpperLeg?.rotation ?: Quaternion.Companion.IDENTITY + var waistRot = it.rotation + + // Interpolate between the pelvis, averaged from the legs, and the chest + waistRot = waistRot.interpQ(leftLegRot.lerpQ(rightLegRot, 0.5f), config.hipFromWaistLegsAveraging).unit() + + // Set hip rotation + skeleton.hipBone.setRotation(waistRot) + skeleton.hipTrackerBone.setRotation(waistRot) + } ?: run { + getFirstAvailableTracker(trackers.chest, trackers.upperChest)?.let { + // Calculates hip from chest + legs + var leftLegRot = trackers.leftUpperLeg?.rotation ?: Quaternion.Companion.IDENTITY + var rightLegRot = trackers.rightUpperLeg?.rotation ?: Quaternion.Companion.IDENTITY + var chestRot = it.rotation + + // Interpolate between the pelvis, averaged from the legs, and the chest + chestRot = chestRot.interpQ(leftLegRot.lerpQ(rightLegRot, 0.5f), config.hipFromChestLegsAveraging).unit() + + // Set hip rotation + skeleton.hipBone.setRotation(chestRot) + skeleton.hipTrackerBone.setRotation(chestRot) + } + } + } + } + + // Extended pelvis model + if (config.extendedPelvisModel && hasKneeTrackers && trackers.hip == null) { + val leftLegRot = trackers.leftUpperLeg?.rotation ?: Quaternion.Companion.IDENTITY + val rightLegRot = trackers.rightUpperLeg?.rotation ?: Quaternion.Companion.IDENTITY + val hipRot = skeleton.hipBone.getLocalRotation() + + val extendedPelvisRot = extendedPelvisYawRoll(leftLegRot, rightLegRot, hipRot) + + // Interpolate between the hipRot and extendedPelvisRot + val newHipRot = hipRot.interpR( + if (extendedPelvisRot.lenSq() != 0.0f) extendedPelvisRot else Quaternion.Companion.IDENTITY, + config.hipLegsAveraging, + ) + + // Set new hip rotation + skeleton.hipBone.setRotation(newHipRot) + skeleton.hipTrackerBone.setRotation(newHipRot) + } + + // Set left and right hip rotations to the hip's + skeleton.leftHipBone.setRotation(skeleton.hipBone.getLocalRotation()) + skeleton.rightHipBone.setRotation(skeleton.hipBone.getLocalRotation()) + } + + /** + * Update a leg's transforms, from its hip to its foot + */ + private fun updateLegTransforms( + upperLegBone: Bone, + kneeTrackerBone: Bone, + lowerLegBone: Bone, + footBone: Bone, + footTrackerBone: Bone, + upperLegTracker: TrackerData?, + lowerLegTracker: TrackerData?, + footTracker: TrackerData?, + ) { + var legRot = Quaternion.Companion.IDENTITY + + upperLegTracker?.let { + // Get upper leg rotation + legRot = it.rotation + } ?: run { + // Use hip's yaw + legRot = skeleton.hipBone.getLocalRotation().project(Vector3.Companion.POS_Y).unit() + } + // Set upper leg rotation + upperLegBone.setRotation(legRot) + kneeTrackerBone.setRotation(legRot) + + lowerLegTracker?.let { + // Get lower leg rotation + legRot = it.rotation + } ?: run { + // Use lower leg or hip's yaw + legRot = legRot.project(Vector3.Companion.POS_Y).unit() + } + // Set lower leg rotation + lowerLegBone.setRotation(legRot) + + // Get foot rotation + footTracker?.let { legRot = it.rotation } + // Set foot rotation + footBone.setRotation(legRot) + footTrackerBone.setRotation(legRot) + + // Extended knee model + if (config.extendedKneeModel) { + upperLegTracker?.let { upper -> + lowerLegTracker?.let { lower -> + // Averages the upper leg's rotation with the local lower leg's + // pitch and roll and apply to the tracker node. + val upperRot = upper.rotation + val lowerRot = lower.rotation + val extendedRot = extendedKneeYawRoll(upperRot, lowerRot) + + upperLegBone.setRotation(upperRot.interpR(extendedRot, config.kneeAnkleAveraging)) + kneeTrackerBone.setRotation(upperRot.interpR(extendedRot, config.kneeTrackerAnkleAveraging)) + } + } + } + } + + /** + * Update an arm's transforms, from its shoulder to its hand + */ + private fun updateArmTransforms( + isTrackingFromController: Boolean, + upperShoulderBone: Bone, + shoulderBone: Bone, + upperArmBone: Bone, + elbowTrackerBone: Bone, + lowerArmBone: Bone, + handBone: Bone, + handTrackerBone: Bone, + shoulderTracker: TrackerData?, + upperArmTracker: TrackerData?, + lowerArmTracker: TrackerData?, + handTracker: TrackerData?, + ) { + if (isTrackingFromController) { // From controller + // Set hand rotation and position from tracker + handTracker?.let { + it.position?.let { handTrackerBone.setPosition(it) } + handTrackerBone.setRotation(it.rotation) + handBone.setRotation(it.rotation) + } + + // Get lower arm rotation + var armRot = getFirstAvailableTracker(lowerArmTracker, upperArmTracker) + ?.rotation ?: Quaternion.Companion.IDENTITY + // Set lower arm rotation + lowerArmBone.setRotation(armRot) + + // Get upper arm rotation + armRot = getFirstAvailableTracker(upperArmTracker, lowerArmTracker) + ?.rotation ?: Quaternion.Companion.IDENTITY + // Set elbow tracker rotation + elbowTrackerBone.setRotation(armRot) + } else { // From HMD + // Get shoulder rotation + var armRot = shoulderTracker?.rotation ?: skeleton.upperChestBone.getLocalRotation() + // Set shoulder rotation + upperShoulderBone.setRotation(skeleton.upperChestBone.getLocalRotation()) + shoulderBone.setRotation(armRot) + + if (upperArmTracker != null || lowerArmTracker != null) { + // Get upper arm rotation + getFirstAvailableTracker(upperArmTracker, lowerArmTracker)?.let { armRot = it.rotation } + // Set upper arm and elbow tracker rotation + upperArmBone.setRotation(armRot) + elbowTrackerBone.setRotation(armRot) + + // Get lower arm rotation + getFirstAvailableTracker(lowerArmTracker, upperArmTracker)?.let { armRot = it.rotation } + // Set lower arm rotation + lowerArmBone.setRotation(armRot) + } else { + // Fallback arm rotation as upper chest + armRot = skeleton.upperChestBone.getLocalRotation() + upperArmBone.setRotation(armRot) + elbowTrackerBone.setRotation(armRot) + lowerArmBone.setRotation(armRot) + } + + // Get hand rotation + handTracker?.let { armRot = it.rotation } + // Set hand, and hand tracker rotation + handBone.setRotation(armRot) + handTrackerBone.setRotation(armRot) + } + } + + fun updateBonePositions() { + skeleton.headBone.update() + if (skeleton.isTrackingLeftArmFromController) skeleton.leftHandTrackerBone.update() + if (skeleton.isTrackingRightArmFromController) skeleton.rightHandTrackerBone.update() + } + + /** + * Rotates the first Quaternion to match its yaw and roll to the rotation of + * the second Quaternion + * + * @param knee the first Quaternion + * @param ankle the second Quaternion + * @return the rotated Quaternion + */ + private fun extendedKneeYawRoll(knee: Quaternion, ankle: Quaternion): Quaternion { + val r = knee.inv() * ankle + val c = Quaternion(r.w, -r.x, 0f, 0f) + return (knee * r * c).unit() + } + + /** + * Rotates the third Quaternion to match its yaw and roll to the rotation of + * the average of the first and second quaternions. + * + * @param leftKnee the first Quaternion + * @param rightKnee the second Quaternion + * @param hip the third Quaternion + * @return the rotated Quaternion + */ + private fun extendedPelvisYawRoll( + leftKnee: Quaternion, + rightKnee: Quaternion, + hip: Quaternion, + ): Quaternion { + // R = InverseHip * (LeftLeft + RightLeg) + // C = Quaternion(R.w, -R.x, 0, 0) + // Pelvis = Hip * R * C + // normalize(Pelvis) + val r = hip.inv() * (leftKnee + rightKnee) + val c = Quaternion(r.w, -r.x, 0f, 0f) + return (hip * r * c).unit() + } + + private fun getFirstAvailableTracker( + vararg trackers: TrackerData?, + ): TrackerData? = trackers.firstOrNull { it != null } + + private fun updateBoneLengths() { + for (boneType in BoneType.values) { + computeNodeOffset(boneType) + } + } + + private fun computeNodeOffset(nodeOffset: BoneType) { + when (nodeOffset) { + BoneType.HEAD -> setNodeOffset(nodeOffset, 0f, 0f, getOffset(SkeletonConfigOffsets.HEAD)) + + BoneType.NECK -> setNodeOffset(nodeOffset, 0f, -getOffset(SkeletonConfigOffsets.NECK), 0f) + + BoneType.UPPER_CHEST -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.UPPER_CHEST), + 0f, + ) + + BoneType.CHEST_TRACKER -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.CHEST_OFFSET) - + getOffset(SkeletonConfigOffsets.CHEST), + -getOffset(SkeletonConfigOffsets.SKELETON_OFFSET), + ) + + BoneType.CHEST -> setNodeOffset(nodeOffset, 0f, -getOffset(SkeletonConfigOffsets.CHEST), 0f) + + BoneType.WAIST -> setNodeOffset(nodeOffset, 0f, -getOffset(SkeletonConfigOffsets.WAIST), 0f) + + BoneType.HIP -> setNodeOffset(nodeOffset, 0f, -getOffset(SkeletonConfigOffsets.HIP), 0f) + + BoneType.HIP_TRACKER -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.HIP_OFFSET), + -getOffset(SkeletonConfigOffsets.SKELETON_OFFSET), + ) + + BoneType.LEFT_HIP -> setNodeOffset( + nodeOffset, + -getOffset(SkeletonConfigOffsets.HIPS_WIDTH) / 2f, + 0f, + 0f, + ) + + BoneType.RIGHT_HIP -> setNodeOffset( + nodeOffset, + getOffset(SkeletonConfigOffsets.HIPS_WIDTH) / 2f, + 0f, + 0f, + ) + + BoneType.LEFT_UPPER_LEG, BoneType.RIGHT_UPPER_LEG -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.UPPER_LEG), + 0f, + ) + + BoneType.LEFT_KNEE_TRACKER, BoneType.RIGHT_KNEE_TRACKER, BoneType.LEFT_FOOT_TRACKER, BoneType.RIGHT_FOOT_TRACKER -> setNodeOffset( + nodeOffset, + 0f, + 0f, + -getOffset(SkeletonConfigOffsets.SKELETON_OFFSET), + ) + + BoneType.LEFT_LOWER_LEG, BoneType.RIGHT_LOWER_LEG -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.LOWER_LEG), + -getOffset(SkeletonConfigOffsets.FOOT_SHIFT), + ) + + BoneType.LEFT_FOOT, BoneType.RIGHT_FOOT -> setNodeOffset( + nodeOffset, + 0f, + 0f, + -getOffset(SkeletonConfigOffsets.FOOT_LENGTH), + ) + + BoneType.LEFT_UPPER_SHOULDER -> setNodeOffset( + nodeOffset, + 0f, + 0f, + 0f, + ) + + BoneType.RIGHT_UPPER_SHOULDER -> setNodeOffset( + nodeOffset, + 0f, + 0f, + 0f, + ) + + BoneType.LEFT_SHOULDER -> setNodeOffset( + nodeOffset, + -getOffset(SkeletonConfigOffsets.SHOULDERS_WIDTH) / 2f, + -getOffset(SkeletonConfigOffsets.SHOULDERS_DISTANCE), + 0f, + ) + + BoneType.RIGHT_SHOULDER -> setNodeOffset( + nodeOffset, + getOffset(SkeletonConfigOffsets.SHOULDERS_WIDTH) / 2f, + -getOffset(SkeletonConfigOffsets.SHOULDERS_DISTANCE), + 0f, + ) + + BoneType.LEFT_UPPER_ARM, BoneType.RIGHT_UPPER_ARM -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.UPPER_ARM), + 0f, + ) + + BoneType.LEFT_LOWER_ARM, BoneType.RIGHT_LOWER_ARM -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.LOWER_ARM), + 0f, + ) + + BoneType.LEFT_HAND, BoneType.RIGHT_HAND -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.HAND_Y), + -getOffset(SkeletonConfigOffsets.HAND_Z), + ) + + BoneType.LEFT_ELBOW_TRACKER, BoneType.RIGHT_ELBOW_TRACKER -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.ELBOW_OFFSET), + 0f, + ) + + BoneType.LEFT_THUMB_METACARPAL, BoneType.LEFT_THUMB_PROXIMAL, BoneType.LEFT_THUMB_DISTAL, + BoneType.RIGHT_THUMB_METACARPAL, BoneType.RIGHT_THUMB_PROXIMAL, BoneType.RIGHT_THUMB_DISTAL, + -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.HAND_Y) * 0.2f, + -getOffset(SkeletonConfigOffsets.HAND_Y) * 0.1f, + ) + + BoneType.LEFT_INDEX_PROXIMAL, BoneType.LEFT_INDEX_INTERMEDIATE, BoneType.LEFT_INDEX_DISTAL, + BoneType.RIGHT_INDEX_PROXIMAL, BoneType.RIGHT_INDEX_INTERMEDIATE, BoneType.RIGHT_INDEX_DISTAL, + -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.HAND_Y) * 0.25f, + 0f, + ) + + BoneType.LEFT_MIDDLE_PROXIMAL, BoneType.LEFT_MIDDLE_INTERMEDIATE, BoneType.LEFT_MIDDLE_DISTAL, + BoneType.RIGHT_MIDDLE_PROXIMAL, BoneType.RIGHT_MIDDLE_INTERMEDIATE, BoneType.RIGHT_MIDDLE_DISTAL, + -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.HAND_Y) * 0.3f, + 0f, + ) + + BoneType.LEFT_RING_PROXIMAL, BoneType.LEFT_RING_INTERMEDIATE, BoneType.LEFT_RING_DISTAL, + BoneType.RIGHT_RING_PROXIMAL, BoneType.RIGHT_RING_INTERMEDIATE, BoneType.RIGHT_RING_DISTAL, + -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.HAND_Y) * 0.28f, + 0f, + ) + + BoneType.LEFT_LITTLE_PROXIMAL, BoneType.LEFT_LITTLE_INTERMEDIATE, BoneType.LEFT_LITTLE_DISTAL, + BoneType.RIGHT_LITTLE_PROXIMAL, BoneType.RIGHT_LITTLE_INTERMEDIATE, BoneType.RIGHT_LITTLE_DISTAL, + -> setNodeOffset( + nodeOffset, + 0f, + -getOffset(SkeletonConfigOffsets.HAND_Y) * 0.2f, + 0f, + ) + + else -> {} + } + } + + private fun getOffset(config: SkeletonConfigOffsets): Float { + val configOffset = skeletonOffsets[config] + return configOffset ?: config.defaultValue + } + + private fun setNodeOffset(boneType: BoneType, x: Float, y: Float, z: Float) { + val bone = skeleton.getBone(boneType) ?: return + + var transOffset = Vector3(x, y, z) + + // If no head position, headShift and neckLength = 0 + if ((boneType == BoneType.HEAD || boneType == BoneType.NECK) && (trackers.head == null || trackers.head!!.position == null)) { + transOffset = Vector3.Companion.NULL + } + // If trackingArmFromController, reverse + if (((boneType == BoneType.LEFT_LOWER_ARM || boneType == BoneType.LEFT_HAND) && skeleton.isTrackingLeftArmFromController) || + ( + (boneType == BoneType.RIGHT_LOWER_ARM || boneType == BoneType.RIGHT_HAND) && + skeleton.isTrackingRightArmFromController + ) + ) { + transOffset = -transOffset + } + + // Compute bone rotation + val rotOffset = if (transOffset.len() > 0f) { + if (transOffset.unit().y == 1f) { + Quaternion.Companion.I + } else { + Quaternion.Companion.fromTo(Vector3.Companion.NEG_Y, transOffset) + } + } else { + Quaternion.Companion.IDENTITY + } + + // Update bone length + bone.length = transOffset.len() + + // Set bone rotation offset + bone.rotationOffset = rotOffset + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt index 25a5fd3426..41993fb5b7 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt @@ -7,6 +7,7 @@ import dev.slimevr.config.DriftCompensationConfig import dev.slimevr.config.ResetsConfig import dev.slimevr.filtering.CircularArrayList import dev.slimevr.tracking.trackers.udp.TrackerDataType +import dev.slimevr.tracking.videocalibration.data.TrackerResetOverride import io.github.axisangles.ktmath.EulerAngles import io.github.axisangles.ktmath.EulerOrder import io.github.axisangles.ktmath.Quaternion @@ -116,6 +117,8 @@ class TrackerResetsHandler(val tracker: Tracker) { */ private var tposeDownFix = Quaternion.IDENTITY + var trackerResetOverride: TrackerResetOverride? = null + /** * Reads/loads drift compensation settings from given config */ @@ -202,6 +205,12 @@ class TrackerResetsHandler(val tracker: Tracker) { */ private fun adjustToReference(rotation: Quaternion): Quaternion { var rot = rotation + + val override = trackerResetOverride + if (override != null) { + return override.toBoneRotation(rot) + } + // Align heading axis with bone space if (!tracker.isHmd || tracker.trackerPosition != TrackerPosition.HEAD) { rot *= mountingOrientation @@ -258,6 +267,8 @@ class TrackerResetsHandler(val tracker: Tracker) { * 0). This allows the tracker to be strapped to body at any pitch and roll. */ fun resetFull(reference: Quaternion) { + trackerResetOverride = null + constraintFix = Quaternion.IDENTITY if (tracker.trackerDataType == TrackerDataType.FLEX_RESISTANCE) { @@ -339,7 +350,7 @@ class TrackerResetsHandler(val tracker: Tracker) { postProcessResetFull(reference) } - private fun postProcessResetFull(reference: Quaternion) { + fun postProcessResetFull(reference: Quaternion) { if (this.tracker.needReset) { this.tracker.needReset = false } @@ -398,6 +409,8 @@ class TrackerResetsHandler(val tracker: Tracker) { * and stores it in mountRotFix, and adjusts yawFix */ fun resetMounting(reference: Quaternion) { + trackerResetOverride = null + if (tracker.trackerDataType == TrackerDataType.FLEX_RESISTANCE) { tracker.trackerFlexHandler.resetMax() tracker.resetFilteringQuats(reference) diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt index ea5c6d5ba1..be6cd870bd 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/udp/TrackersUDPServer.kt @@ -297,6 +297,7 @@ class TrackersUDPServer(private val port: Int, name: String, private val tracker val serialBuffer2 = StringBuilder() try { socket = DatagramSocket(port) + socket.trafficClass = 0x10 var prevPacketTime = System.currentTimeMillis() socket.soTimeout = 250 while (true) { diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/VideoCalibrationService.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/VideoCalibrationService.kt new file mode 100644 index 0000000000..d323b6eb47 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/VideoCalibrationService.kt @@ -0,0 +1,139 @@ +package dev.slimevr.tracking.videocalibration + +import dev.slimevr.VRServer +import dev.slimevr.protocol.GenericConnection +import dev.slimevr.tracking.trackers.TrackerStatus +import dev.slimevr.tracking.videocalibration.networking.MDNSRegistry +import dev.slimevr.tracking.videocalibration.sources.HumanPoseSource +import dev.slimevr.tracking.videocalibration.sources.PhoneWebcamSource +import dev.slimevr.tracking.videocalibration.sources.SnapshotsDatabase +import dev.slimevr.tracking.videocalibration.sources.TrackersSource +import dev.slimevr.tracking.videocalibration.steps.Step +import dev.slimevr.tracking.videocalibration.steps.VideoCalibrator +import dev.slimevr.tracking.videocalibration.util.DebugOutput +import io.eiren.util.logging.LogManager +import kotlin.time.Duration +import kotlin.time.Duration.Companion.seconds +import kotlin.time.TimeSource + +class VideoCalibrationService( + private val server: VRServer, + webcamService: MDNSRegistry.Service, + websocket: GenericConnection, +) { + private val debugOutput = DebugOutput(DebugOutput.DEFAULT_DIR) + + private val webcamSource: PhoneWebcamSource + private val humanPoseSource: HumanPoseSource + private val trackersSource: TrackersSource + private val snapshotsDatabase: SnapshotsDatabase + private val videoCalibrator: VideoCalibrator + + private var timeoutInstant = TimeSource.Monotonic.markNow() + + init { + webcamSource = + PhoneWebcamSource(webcamService, debugOutput) + + humanPoseSource = + HumanPoseSource( + webcamSource.imageSnapshots, + server.webRTCManager, + debugOutput, + ) + + val trackersToRecord = + server.allTrackers + .filter { + !it.isInternal && + it.trackerPosition != null && + it.status == TrackerStatus.OK + } + .associateBy { it.trackerPosition!! } + + trackersSource = + TrackersSource( + trackersToRecord, + TRACKERS_SNAPSHOT_INTERVAL, + debugOutput, + ) + + snapshotsDatabase = + SnapshotsDatabase( + TRACKERS_SNAPSHOT_INTERVAL * 2.0, + humanPoseSource.humanPoseSnapshots, + trackersSource.trackersSnapshots, + ) + + val trackersToReset = trackersToRecord.filter { it.value.isImu() } + + videoCalibrator = + VideoCalibrator( + trackersToReset, + server.humanPoseManager.skeletonConfigManager, + snapshotsDatabase, + websocket, + debugOutput, + ) + } + + /** + * Starts calibration. + */ + fun start(timeout: Duration) { + LogManager.info("Starting video calibration...") + + if (server.videoCalibrationService != null) { + error("Video calibration is already started") + } + + server.videoCalibrationService = this + + timeoutInstant = TimeSource.Monotonic.markNow() + timeout + + webcamSource.start() + humanPoseSource.start() + trackersSource.start() + videoCalibrator.start() + } + + /** + * Must be called on each server tick. + */ + fun onTick() { + if (TimeSource.Monotonic.markNow() >= timeoutInstant) { + LogManager.warning("Video calibration timed out") + this.requestStop() + return + } + + if (webcamSource.status.get() == PhoneWebcamSource.Status.DONE || + humanPoseSource.status.get() == HumanPoseSource.Status.DONE || + trackersSource.status == TrackersSource.Status.DONE || + videoCalibrator.step.get() == Step.DONE + ) { + this.requestStop() + return + } + + trackersSource.onTick() + } + + /** + * Stops the calibration. + */ + fun requestStop() { + LogManager.info("Stopping video calibration...") + + server.videoCalibrationService = null + + webcamSource.requestStop() + trackersSource.requestStop() + humanPoseSource.requestStop() + videoCalibrator.requestStop() + } + + companion object { + private val TRACKERS_SNAPSHOT_INTERVAL = 1.seconds / 120.0 + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/Camera.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/Camera.kt new file mode 100644 index 0000000000..a3f3922248 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/Camera.kt @@ -0,0 +1,28 @@ +package dev.slimevr.tracking.videocalibration.data + +import io.github.axisangles.ktmath.Vector2D +import io.github.axisangles.ktmath.Vector3D +import java.awt.Dimension + +data class Camera( + val extrinsic: CameraExtrinsic, + val intrinsic: CameraIntrinsic, + val imageSize: Dimension, +) { + /** + * Projects a point in the world into the camera's image space. + */ + fun project(pointInWorld: Vector3D) = intrinsic.project(extrinsic.toCamera(pointInWorld)) + + /** + * Finds the image vector that corresponds to projecting a vector in world space. + */ + fun project(vectorInWorld: Vector3D, originInImage: Vector2D, depth: Double): Vector2D? { + val originInWorld = extrinsic.toWorld(intrinsic.ray(originInImage) * depth) + val tipInWorld = originInWorld + vectorInWorld + val tipInImage = project(tipInWorld) ?: return null + return tipInImage - originInImage + } + + override fun toString() = "Camera($extrinsic $intrinsic $imageSize)" +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/CameraExtrinsic.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/CameraExtrinsic.kt new file mode 100644 index 0000000000..6b25616313 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/CameraExtrinsic.kt @@ -0,0 +1,33 @@ +package dev.slimevr.tracking.videocalibration.data + +import dev.slimevr.tracking.videocalibration.util.toEulerYZXString +import io.github.axisangles.ktmath.QuaternionD +import io.github.axisangles.ktmath.Vector3D + +class CameraExtrinsic( + val worldToCamera: QuaternionD, + val worldOriginInCamera: Vector3D, +) { + val cameraToWorld = worldToCamera.inv() + val cameraOriginInWorld = -cameraToWorld.sandwich(worldOriginInCamera) + + /** + * Transforms a point in world space to camera space. + */ + fun toCamera(pointInWorld: Vector3D) = worldToCamera.sandwich(pointInWorld) + worldOriginInCamera + + /** + * Transforms a point in camera space to world space. + */ + fun toWorld(pointInCamera: Vector3D) = cameraToWorld.sandwich(pointInCamera) + cameraOriginInWorld + + override fun toString(): String = "Extrinsic(cameraToWorld=${cameraToWorld.toEulerYZXString()} cameraOriginInWorld=$cameraOriginInWorld)" + + companion object { + + fun fromCameraPose(cameraToWorld: QuaternionD, cameraOriginInWorld: Vector3D): CameraExtrinsic { + val worldToCamera = cameraToWorld.inv() + return CameraExtrinsic(worldToCamera, -worldToCamera.sandwich(cameraOriginInWorld)) + } + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/CameraIntrinsic.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/CameraIntrinsic.kt new file mode 100644 index 0000000000..a752da7864 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/CameraIntrinsic.kt @@ -0,0 +1,36 @@ +package dev.slimevr.tracking.videocalibration.data + +import io.github.axisangles.ktmath.Vector2D +import io.github.axisangles.ktmath.Vector3D + +class CameraIntrinsic( + val fx: Double, + val fy: Double, + val tx: Double, + val ty: Double, +) { + /** + * Projects a point in camera space into the camera's image space. + */ + fun project(pointInCamera: Vector3D): Vector2D? { + if (pointInCamera.z < 0.1) { + return null + } + + return Vector2D( + pointInCamera.x / pointInCamera.z * fx + tx, + pointInCamera.y / pointInCamera.z * fy + ty, + ) + } + + /** + * A ray from the camera center to the point on the image plane. + */ + fun ray(imagePoint: Vector2D): Vector3D = Vector3D( + (imagePoint.x - tx) / fx, + (imagePoint.y - ty) / fy, + 1.0, + ) + + override fun toString() = "Intrinsic(fx=${"%.1f".format(fx)} fy=${"%.1f".format(fy)} tx=${"%.1f".format(tx)} ty=${"%.1f".format(ty)})" +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/CocoWholeBodyKeypoint.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/CocoWholeBodyKeypoint.kt new file mode 100644 index 0000000000..7d8933f1cd --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/CocoWholeBodyKeypoint.kt @@ -0,0 +1,144 @@ +package dev.slimevr.tracking.videocalibration.data + +enum class CocoWholeBodyKeypoint(val key: String) { + // 0-16: body + NOSE("nose"), + LEFT_EYE("left_eye"), + RIGHT_EYE("right_eye"), + LEFT_EAR("left_ear"), + RIGHT_EAR("right_ear"), + LEFT_SHOULDER("left_shoulder"), + RIGHT_SHOULDER("right_shoulder"), + LEFT_ELBOW("left_elbow"), + RIGHT_ELBOW("right_elbow"), + LEFT_WRIST("left_wrist"), + RIGHT_WRIST("right_wrist"), + LEFT_HIP("left_hip"), + RIGHT_HIP("right_hip"), + LEFT_KNEE("left_knee"), + RIGHT_KNEE("right_knee"), + LEFT_ANKLE("left_ankle"), + RIGHT_ANKLE("right_ankle"), + + // 17-22: feet + LEFT_BIG_TOE("left_big_toe"), + LEFT_SMALL_TOE("left_small_toe"), + LEFT_HEEL("left_heel"), + RIGHT_BIG_TOE("right_big_toe"), + RIGHT_SMALL_TOE("right_small_toe"), + RIGHT_HEEL("right_heel"), + + // 23-90: face (68 points) + FACE_0("face-0"), + FACE_1("face-1"), + FACE_2("face-2"), + FACE_3("face-3"), + FACE_4("face-4"), + FACE_5("face-5"), + FACE_6("face-6"), + FACE_7("face-7"), + FACE_8("face-8"), + FACE_9("face-9"), + FACE_10("face-10"), + FACE_11("face-11"), + FACE_12("face-12"), + FACE_13("face-13"), + FACE_14("face-14"), + FACE_15("face-15"), + FACE_16("face-16"), + FACE_17("face-17"), + FACE_18("face-18"), + FACE_19("face-19"), + FACE_20("face-20"), + FACE_21("face-21"), + FACE_22("face-22"), + FACE_23("face-23"), + FACE_24("face-24"), + FACE_25("face-25"), + FACE_26("face-26"), + FACE_27("face-27"), + FACE_28("face-28"), + FACE_29("face-29"), + FACE_30("face-30"), + FACE_31("face-31"), + FACE_32("face-32"), + FACE_33("face-33"), + FACE_34("face-34"), + FACE_35("face-35"), + FACE_36("face-36"), + FACE_37("face-37"), + FACE_38("face-38"), + FACE_39("face-39"), + FACE_40("face-40"), + FACE_41("face-41"), + FACE_42("face-42"), + FACE_43("face-43"), + FACE_44("face-44"), + FACE_45("face-45"), + FACE_46("face-46"), + FACE_47("face-47"), + FACE_48("face-48"), + FACE_49("face-49"), + FACE_50("face-50"), + FACE_51("face-51"), + FACE_52("face-52"), + FACE_53("face-53"), + FACE_54("face-54"), + FACE_55("face-55"), + FACE_56("face-56"), + FACE_57("face-57"), + FACE_58("face-58"), + FACE_59("face-59"), + FACE_60("face-60"), + FACE_61("face-61"), + FACE_62("face-62"), + FACE_63("face-63"), + FACE_64("face-64"), + FACE_65("face-65"), + FACE_66("face-66"), + FACE_67("face-67"), + + // 91-132: hands (42 points) + LEFT_HAND_ROOT("left_hand_root"), + LEFT_THUMB1("left_thumb1"), + LEFT_THUMB2("left_thumb2"), + LEFT_THUMB3("left_thumb3"), + LEFT_THUMB4("left_thumb4"), + LEFT_FORE_FINGER1("left_forefinger1"), + LEFT_FORE_FINGER2("left_forefinger2"), + LEFT_FORE_FINGER3("left_forefinger3"), + LEFT_FORE_FINGER4("left_forefinger4"), + LEFT_MIDDLE_FINGER1("left_middle_finger1"), + LEFT_MIDDLE_FINGER2("left_middle_finger2"), + LEFT_MIDDLE_FINGER3("left_middle_finger3"), + LEFT_MIDDLE_FINGER4("left_middle_finger4"), + LEFT_RING_FINGER1("left_ring_finger1"), + LEFT_RING_FINGER2("left_ring_finger2"), + LEFT_RING_FINGER3("left_ring_finger3"), + LEFT_RING_FINGER4("left_ring_finger4"), + LEFT_PINKY_FINGER1("left_pinky_finger1"), + LEFT_PINKY_FINGER2("left_pinky_finger2"), + LEFT_PINKY_FINGER3("left_pinky_finger3"), + LEFT_PINKY_FINGER4("left_pinky_finger4"), + RIGHT_HAND_ROOT("right_hand_root"), + RIGHT_THUMB1("right_thumb1"), + RIGHT_THUMB2("right_thumb2"), + RIGHT_THUMB3("right_thumb3"), + RIGHT_THUMB4("right_thumb4"), + RIGHT_FORE_FINGER1("right_forefinger1"), + RIGHT_FORE_FINGER2("right_forefinger2"), + RIGHT_FORE_FINGER3("right_forefinger3"), + RIGHT_FORE_FINGER4("right_forefinger4"), + RIGHT_MIDDLE_FINGER1("right_middle_finger1"), + RIGHT_MIDDLE_FINGER2("right_middle_finger2"), + RIGHT_MIDDLE_FINGER3("right_middle_finger3"), + RIGHT_MIDDLE_FINGER4("right_middle_finger4"), + RIGHT_RING_FINGER1("right_ring_finger1"), + RIGHT_RING_FINGER2("right_ring_finger2"), + RIGHT_RING_FINGER3("right_ring_finger3"), + RIGHT_RING_FINGER4("right_ring_finger4"), + RIGHT_PINKY_FINGER1("right_pinky_finger1"), + RIGHT_PINKY_FINGER2("right_pinky_finger2"), + RIGHT_PINKY_FINGER3("right_pinky_finger3"), + RIGHT_PINKY_FINGER4("right_pinky_finger4"), +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/TrackerResetOverride.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/TrackerResetOverride.kt new file mode 100644 index 0000000000..6da9b6a2ba --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/data/TrackerResetOverride.kt @@ -0,0 +1,27 @@ +package dev.slimevr.tracking.videocalibration.data + +import dev.slimevr.tracking.videocalibration.util.toEulerYZXString +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.QuaternionD + +/** + * Converts a tracker's rotation into its bone's rotation. + */ +data class TrackerResetOverride( + val globalYaw: Double, + val localRotation: QuaternionD, +) { + private val globalRotation = QuaternionD.rotationAroundYAxis(globalYaw) + + fun toBoneRotation(trackerRotation: QuaternionD): QuaternionD { + val rotation = (globalRotation * trackerRotation * localRotation).twinNearest(QuaternionD.IDENTITY) + return rotation + } + + fun toBoneRotation(trackerRotation: Quaternion): Quaternion { + val rotation = toBoneRotation(trackerRotation.toDouble()) + return rotation.toFloat() + } + + override fun toString() = "TrackerReset(global_yaw=${globalRotation.toEulerYZXString()} local=${localRotation.toEulerYZXString()})" +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/networking/MDNSRegistry.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/networking/MDNSRegistry.kt new file mode 100644 index 0000000000..837d6aa94a --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/networking/MDNSRegistry.kt @@ -0,0 +1,116 @@ +package dev.slimevr.tracking.videocalibration.networking + +import io.eiren.util.logging.LogManager +import java.net.Inet4Address +import java.net.NetworkInterface +import javax.jmdns.JmDNS +import javax.jmdns.ServiceEvent +import javax.jmdns.ServiceListener +import kotlin.collections.iterator + +/** + * Registry of published mDNS services. + */ +class MDNSRegistry { + + /** + * Services to monitor. + */ + enum class ServiceType(val mDNSServiceType: String) { + WEBCAM("_slimevr-camera._tcp.local."), + } + + /** + * An available service. + */ + data class Service( + val type: ServiceType, + val host: Inet4Address, + val port: Int, + ) + + private val lock = Any() + private val jmDNSs = mutableListOf() + private val services = mutableMapOf() + + /** + * Starts listening for mDNS services. + */ + fun start() { + LogManager.info("Setting up mDNS discovery...") + + try { + for (address in enumerateNetworks()) { + LogManager.info("Listening for mDNS on network $address...") + try { + val jmdns = JmDNS.create(address) + jmDNSs.add(jmdns) + } catch (e: Exception) { + LogManager.warning("Failed to create mDNS instance for $address: $e", e) + } + } + } catch (e: Exception) { + LogManager.warning("Failed to enumerate network instances", e) + } + + for (serviceType in ServiceType.entries) { + LogManager.info("Listening for mDNS service $serviceType...") + for (jmDNS in jmDNSs) { + jmDNS.addServiceListener( + serviceType.mDNSServiceType, + object : ServiceListener { + + override fun serviceAdded(event: ServiceEvent) { + LogManager.debug("Resolving mDNS service $serviceType on ${jmDNS.inetAddress}...") + jmDNS.requestServiceInfo(event.type, event.name) + } + + override fun serviceRemoved(event: ServiceEvent) { + synchronized(lock) { + LogManager.debug("Removed mDNS service $serviceType") + services.remove(serviceType) + } + } + + override fun serviceResolved(event: ServiceEvent) { + val info = event.info + val host = info.inetAddresses.filterIsInstance().firstOrNull() ?: return + val port = info.port + val service = Service(serviceType, host, port) + synchronized(lock) { + LogManager.debug("Added mDNS service $service") + services[service.type] = service + } + } + }, + ) + } + } + + LogManager.info("mDNS discovery started") + } + + /** + * Gets a discovered service. + */ + fun findService(serviceType: ServiceType): Service? { + synchronized(lock) { + return services[serviceType] + } + } + + companion object { + + private fun enumerateNetworks() = iterator { + for (network in NetworkInterface.getNetworkInterfaces()) { + if (network.isUp && !network.isLoopback) { + for (address in network.inetAddresses) { + if (address is Inet4Address) { + yield(address) + } + } + } + } + } + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/networking/WebRTCManager.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/networking/WebRTCManager.kt new file mode 100644 index 0000000000..191e0993c9 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/networking/WebRTCManager.kt @@ -0,0 +1,188 @@ +package dev.slimevr.tracking.videocalibration.networking + +import dev.onvoid.webrtc.CreateSessionDescriptionObserver +import dev.onvoid.webrtc.PeerConnectionFactory +import dev.onvoid.webrtc.PeerConnectionObserver +import dev.onvoid.webrtc.RTCAnswerOptions +import dev.onvoid.webrtc.RTCConfiguration +import dev.onvoid.webrtc.RTCIceCandidate +import dev.onvoid.webrtc.RTCIceConnectionState +import dev.onvoid.webrtc.RTCIceGatheringState +import dev.onvoid.webrtc.RTCPeerConnection +import dev.onvoid.webrtc.RTCPeerConnectionState +import dev.onvoid.webrtc.RTCSdpType +import dev.onvoid.webrtc.RTCSessionDescription +import dev.onvoid.webrtc.SetSessionDescriptionObserver +import dev.onvoid.webrtc.media.video.CustomVideoSource +import dev.onvoid.webrtc.media.video.VideoFrame +import io.eiren.util.logging.LogManager +import kotlinx.coroutines.CompletableDeferred +import java.lang.IllegalStateException +import kotlin.coroutines.resume +import kotlin.coroutines.resumeWithException +import kotlin.coroutines.suspendCoroutine + +/** + * Manages remote clients that are connecting to SlimeVR to receive video. + */ +class WebRTCManager { + + /** + * Types of videos that the client can request. + */ + enum class VideoProvider { + VIDEO_CALIBRATION, + } + + private class Connection( + val videoProvider: VideoProvider, + val peerConnection: RTCPeerConnection, + val videoSource: CustomVideoSource, + ) + + private val lock = Any() + private val peerConnectionFactory = PeerConnectionFactory() + private val connections = mutableListOf() + + /** + * Creates a peer connection between a client and the server. + * + * A remote client sends us an offer SDP. We set up a peer connection, do ICE + * discovery, and respond with an answer SDP. The remote client uses this answer SDP + * to connect via WebRTC to receive a video stream. + */ + suspend fun connect(provider: VideoProvider, offerSDP: String): String { + LogManager.info("Creating peer connection for incoming video request...") + + // Will be completed by the peer connection's observable + val iceGatheringComplete = CompletableDeferred() + + val peerConnection = + peerConnectionFactory.createPeerConnection( + RTCConfiguration(), + object : PeerConnectionObserver { + override fun onIceCandidate(candidate: RTCIceCandidate) { + // Nothing to do since we are waiting for all candidates to be + // gathered before replying to the remote client + } + + override fun onIceGatheringChange(state: RTCIceGatheringState) { + if (state == RTCIceGatheringState.COMPLETE) { + iceGatheringComplete.complete(Unit) + } + } + + override fun onConnectionChange(state: RTCPeerConnectionState) { + if (state == RTCPeerConnectionState.CONNECTED) { + LogManager.info("Remote client connected to WebRTC!") + } + cleanupConnections() + } + }, + ) + + val videoSource = CustomVideoSource() + val videoTrack = peerConnectionFactory.createVideoTrack("video0", videoSource) + peerConnection.addTrack(videoTrack, listOf("stream0")) + + LogManager.info("Setting remote description...") + + suspendCoroutine { cont -> + peerConnection.setRemoteDescription( + RTCSessionDescription(RTCSdpType.OFFER, offerSDP), + object : SetSessionDescriptionObserver { + override fun onSuccess() { + cont.resume(Unit) + } + override fun onFailure(error: String) { + cont.resumeWithException(IllegalStateException("Failed to set remote description: $error")) + } + }, + ) + } + + LogManager.info("Creating answer...") + + val answer = suspendCoroutine { cont -> + peerConnection.createAnswer( + RTCAnswerOptions(), + object : CreateSessionDescriptionObserver { + override fun onSuccess(description: RTCSessionDescription) { + cont.resume(description) + } + override fun onFailure(error: String) { + cont.resumeWithException(IllegalStateException("Failed to create answer: $error")) + } + }, + ) + } + + LogManager.info("Setting local description...") + + suspendCoroutine { cont -> + peerConnection.setLocalDescription( + answer, + object : SetSessionDescriptionObserver { + override fun onSuccess() { + cont.resume(Unit) + } + override fun onFailure(error: String) { + cont.resumeWithException(IllegalStateException("Failed to set local description: $error")) + } + }, + ) + } + + if (peerConnection.iceConnectionState == RTCIceConnectionState.COMPLETED) { + LogManager.info("ICE gathering already complete") + } else { + LogManager.info("Waiting for ICE gathering to complete...") + iceGatheringComplete.await() + } + + LogManager.info("Peer connection ready") + + synchronized(lock) { + val connection = Connection(provider, peerConnection, videoSource) + connections.add(connection) + } + + return peerConnection.localDescription.sdp + } + + /** + * Broadcasts a video frame to any clients requesting that video. + */ + fun broadcastVideoFrame(provider: VideoProvider, frame: VideoFrame) { + synchronized(lock) { + for (conn in connections) { + if (conn.videoProvider == provider) { + conn.videoSource.pushFrame(frame) + } + } + } + } + + /** + * Cleans up connections so that native resources (e.g. ports) are released. + */ + private fun cleanupConnections() { + synchronized(lock) { + val toClose = connections.filter { + it.peerConnection.connectionState == RTCPeerConnectionState.DISCONNECTED || + it.peerConnection.connectionState == RTCPeerConnectionState.FAILED + } + + connections.removeAll(toClose) + + // Closing the connection MUST BE done outside iterating the list, because + // this can trigger the onConnectionChange event which calls + // cleanupConnections again + toClose.forEach { it.peerConnection.close() } + + connections.removeIf { + it.peerConnection.connectionState == RTCPeerConnectionState.CLOSED + } + } + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/snapshots/HumanPoseSnapshot.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/snapshots/HumanPoseSnapshot.kt new file mode 100644 index 0000000000..4d1d9da423 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/snapshots/HumanPoseSnapshot.kt @@ -0,0 +1,14 @@ +package dev.slimevr.tracking.videocalibration.snapshots + +import dev.slimevr.tracking.videocalibration.data.Camera +import dev.slimevr.tracking.videocalibration.data.CocoWholeBodyKeypoint +import io.github.axisangles.ktmath.Vector2D +import kotlin.time.Duration +import kotlin.time.TimeSource + +class HumanPoseSnapshot( + val instant: TimeSource.Monotonic.ValueTimeMark, + val timestamp: Duration, + val joints: Map, + val camera: Camera, +) diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/snapshots/ImageSnapshot.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/snapshots/ImageSnapshot.kt new file mode 100644 index 0000000000..c957010915 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/snapshots/ImageSnapshot.kt @@ -0,0 +1,13 @@ +package dev.slimevr.tracking.videocalibration.snapshots + +import dev.slimevr.tracking.videocalibration.data.Camera +import java.awt.image.BufferedImage +import kotlin.time.Duration +import kotlin.time.TimeSource + +class ImageSnapshot( + val instant: TimeSource.Monotonic.ValueTimeMark, + val timestamp: Duration, + val image: BufferedImage, + val camera: Camera, +) diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/snapshots/TrackerSnapshot.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/snapshots/TrackerSnapshot.kt new file mode 100644 index 0000000000..5b717437a8 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/snapshots/TrackerSnapshot.kt @@ -0,0 +1,10 @@ +package dev.slimevr.tracking.videocalibration.snapshots + +import io.github.axisangles.ktmath.QuaternionD +import io.github.axisangles.ktmath.Vector3D + +class TrackerSnapshot( + val rawTrackerToWorld: QuaternionD, + val adjustedTrackerToWorld: QuaternionD, + val trackerOriginInWorld: Vector3D?, +) diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/snapshots/TrackersSnapshot.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/snapshots/TrackersSnapshot.kt new file mode 100644 index 0000000000..0cfe0f43d2 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/snapshots/TrackersSnapshot.kt @@ -0,0 +1,10 @@ +package dev.slimevr.tracking.videocalibration.snapshots + +import dev.slimevr.tracking.trackers.TrackerPosition +import kotlin.time.Duration +import kotlin.time.TimeSource + +class TrackersSnapshot( + val instant: TimeSource.Monotonic.ValueTimeMark, + val trackers: Map, +) diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/HumanPoseSource.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/HumanPoseSource.kt new file mode 100644 index 0000000000..2b92407f6b --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/HumanPoseSource.kt @@ -0,0 +1,107 @@ +package dev.slimevr.tracking.videocalibration.sources + +import RtmposeOnnxPipeline +import dev.onvoid.webrtc.media.FourCC +import dev.onvoid.webrtc.media.video.NativeI420Buffer +import dev.onvoid.webrtc.media.video.VideoBufferConverter +import dev.onvoid.webrtc.media.video.VideoFrame +import dev.slimevr.tracking.videocalibration.networking.WebRTCManager +import dev.slimevr.tracking.videocalibration.snapshots.HumanPoseSnapshot +import dev.slimevr.tracking.videocalibration.snapshots.ImageSnapshot +import dev.slimevr.tracking.videocalibration.util.DebugOutput +import io.eiren.util.logging.LogManager +import io.github.axisangles.ktmath.Vector2D +import kotlinx.coroutines.* +import kotlinx.coroutines.channels.Channel +import java.awt.image.DataBufferInt +import java.util.concurrent.atomic.AtomicReference +import kotlin.io.path.absolutePathString +import kotlin.io.path.toPath + +class HumanPoseSource( + val imagesSource: Channel, + val webRTCManager: WebRTCManager, + val debugOutput: DebugOutput, +) { + + enum class Status { + NOT_RUNNING, + RUNNING, + DONE, + } + + val status = AtomicReference(Status.NOT_RUNNING) + val humanPoseSnapshots = Channel(Channel.Factory.UNLIMITED) + + private val scope = CoroutineScope(Dispatchers.IO + SupervisorJob()) + private val pipeline = RtmposeOnnxPipeline( + "", + HumanPoseSource::class.java.protectionDomain.codeSource.location.toURI().toPath().parent.resolve("rtmpose-m_simcc-body7_pt-body7_420e-256x192-e48f03d0_20230504.onnx").absolutePathString(), + ) + + fun start() { + scope.launch { + status.set(Status.RUNNING) + pipeline.start() + try { + for (imageFrame in imagesSource) { + detectPose(imageFrame) + } + } catch (e: Exception) { + LogManager.warning("Human pose source failed", e) + scope.cancel() + } finally { + status.set(Status.DONE) + } + } + } + + fun requestStop() { + scope.cancel() + } + + private fun detectPose(imageFrame: ImageSnapshot) { + val poseResponse = pipeline.processImage(imageFrame.image) + val detection = poseResponse.detections.firstOrNull() + if (detection != null) { + val joints = detection.joints.filter { + it.visible + }.associate { + it.name to Vector2D(it.x, it.y) + } + + val humanPoseSnapshot = HumanPoseSnapshot(imageFrame.instant, imageFrame.timestamp, joints, imageFrame.camera) + humanPoseSnapshots.trySend(humanPoseSnapshot) + + val poseImage = debugOutput.drawHumanPoseImage( + imageFrame.image, + humanPoseSnapshot, + ) + + debugOutput.saveHumanPoseImage(imageFrame.timestamp, poseImage) + + val imageBytes = intRgbToRgbaBytes(poseImage.raster.dataBuffer as DataBufferInt) + val imageI420Bytes = NativeI420Buffer.allocate(poseImage.width, poseImage.height) + VideoBufferConverter.convertToI420(imageBytes, imageI420Bytes, FourCC.RGBA) + + val videoFrame = VideoFrame(imageI420Bytes, imageFrame.timestamp.inWholeNanoseconds) + + webRTCManager.broadcastVideoFrame(WebRTCManager.VideoProvider.VIDEO_CALIBRATION, videoFrame) + } + } + + private fun intRgbToRgbaBytes(buffer: DataBufferInt): ByteArray { + val src = buffer.data + val dst = ByteArray(src.size * 4) + + var di = 0 + for (pixel in src) { + dst[di++] = 0xFF.toByte() // A + dst[di++] = (pixel and 0xFF).toByte() // B + dst[di++] = ((pixel shr 8) and 0xFF).toByte() // G + dst[di++] = ((pixel shr 16) and 0xFF).toByte() // R + } + + return dst + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/PhoneWebcamSource.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/PhoneWebcamSource.kt new file mode 100644 index 0000000000..0eeb7cbb34 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/PhoneWebcamSource.kt @@ -0,0 +1,359 @@ +package dev.slimevr.tracking.videocalibration.sources + +import dev.onvoid.webrtc.CreateSessionDescriptionObserver +import dev.onvoid.webrtc.PeerConnectionFactory +import dev.onvoid.webrtc.PeerConnectionObserver +import dev.onvoid.webrtc.RTCConfiguration +import dev.onvoid.webrtc.RTCDataChannelInit +import dev.onvoid.webrtc.RTCIceCandidate +import dev.onvoid.webrtc.RTCIceConnectionState +import dev.onvoid.webrtc.RTCIceGatheringState +import dev.onvoid.webrtc.RTCOfferOptions +import dev.onvoid.webrtc.RTCPeerConnection +import dev.onvoid.webrtc.RTCPeerConnectionState +import dev.onvoid.webrtc.RTCRtpTransceiver +import dev.onvoid.webrtc.RTCSdpType +import dev.onvoid.webrtc.RTCSessionDescription +import dev.onvoid.webrtc.SetSessionDescriptionObserver +import dev.onvoid.webrtc.media.FourCC +import dev.onvoid.webrtc.media.video.CustomVideoSource +import dev.onvoid.webrtc.media.video.VideoBufferConverter +import dev.onvoid.webrtc.media.video.VideoFrame +import dev.onvoid.webrtc.media.video.VideoTrack +import dev.slimevr.tracking.videocalibration.data.Camera +import dev.slimevr.tracking.videocalibration.data.CameraExtrinsic +import dev.slimevr.tracking.videocalibration.data.CameraIntrinsic +import dev.slimevr.tracking.videocalibration.networking.MDNSRegistry +import dev.slimevr.tracking.videocalibration.snapshots.ImageSnapshot +import dev.slimevr.tracking.videocalibration.util.DebugOutput +import io.eiren.util.logging.LogManager +import io.github.axisangles.ktmath.QuaternionD +import io.github.axisangles.ktmath.Vector3D +import io.ktor.client.HttpClient +import io.ktor.client.call.body +import io.ktor.client.engine.cio.CIO +import io.ktor.client.plugins.contentnegotiation.ContentNegotiation +import io.ktor.client.request.preparePost +import io.ktor.client.request.setBody +import io.ktor.http.ContentType +import io.ktor.http.HttpStatusCode +import io.ktor.http.URLBuilder +import io.ktor.http.URLProtocol +import io.ktor.http.contentType +import io.ktor.serialization.kotlinx.json.json +import kotlinx.coroutines.CompletableDeferred +import kotlinx.coroutines.CoroutineScope +import kotlinx.coroutines.Dispatchers +import kotlinx.coroutines.awaitCancellation +import kotlinx.coroutines.cancel +import kotlinx.coroutines.channels.Channel +import kotlinx.coroutines.launch +import kotlinx.serialization.Serializable +import java.awt.Dimension +import java.awt.Transparency +import java.awt.color.ColorSpace +import java.awt.image.BufferedImage +import java.awt.image.ComponentColorModel +import java.awt.image.DataBuffer +import java.awt.image.DataBufferByte +import java.awt.image.Raster +import java.lang.IllegalStateException +import java.util.concurrent.atomic.AtomicReference +import kotlin.coroutines.resume +import kotlin.coroutines.resumeWithException +import kotlin.coroutines.suspendCoroutine +import kotlin.time.TimeSource + +/** + * Produces webcam images from a calibrated phone camera. + * + * The phone is running a WebRTC signaling server which accepts an offer SDP and + * replies with an answer SDP. We use the answer SDP to set up the actual WebRTC video + * connection. + */ +class PhoneWebcamSource( + private val webcamService: MDNSRegistry.Service, + private val debugOutput: DebugOutput, +) { + enum class Status { + NOT_STARTED, + INITIALIZING, + RUNNING, + DONE, + } + + val status = AtomicReference(Status.NOT_STARTED) + val imageSnapshots = Channel(Channel.Factory.CONFLATED) + + private var scope = CoroutineScope(Dispatchers.IO) + private var startTime = TimeSource.Monotonic.markNow() + private var camera: Camera? = null + + /** + * Starts the service. + */ + fun start() { + scope.launch { + run() + } + } + + /** + * Stops the service + */ + fun requestStop() { + scope.cancel() + } + + private suspend fun run() { + status.set(Status.INITIALIZING) + + val peerConnection: RTCPeerConnection + try { + peerConnection = connect() + } catch (e: Exception) { + LogManager.warning("Failed to start phone webcam source: $e", e) + status.set(Status.DONE) + return + } + + status.set(Status.RUNNING) + + try { + awaitCancellation() + } finally { + status.set(Status.DONE) + peerConnection.close() + } + } + + /** + * Creates a WebRTC peer connection. + */ + private suspend fun connect(): RTCPeerConnection { + LogManager.info("Creating peer connection to request video...") + + // Will be completed by the peer connection's observable + val iceGatheringComplete = CompletableDeferred() + + val peerConnectionFactory = PeerConnectionFactory() + + val peerConnection = + peerConnectionFactory.createPeerConnection( + RTCConfiguration(), + object : PeerConnectionObserver { + override fun onIceCandidate(candidate: RTCIceCandidate) { + // Nothing to do since we are waiting for all candidates to be + // gathered before replying to the remote client + } + + override fun onIceGatheringChange(state: RTCIceGatheringState) { + if (state == RTCIceGatheringState.COMPLETE) { + iceGatheringComplete.complete(Unit) + } + } + + override fun onConnectionChange(state: RTCPeerConnectionState) { + when (state) { + RTCPeerConnectionState.CONNECTED -> + LogManager.info("Connected to WebRTC!") + + RTCPeerConnectionState.DISCONNECTED, + RTCPeerConnectionState.CLOSED, + RTCPeerConnectionState.FAILED, + -> + scope.cancel() + + else -> {} + } + } + + override fun onTrack(transceiver: RTCRtpTransceiver) { + val track = transceiver.receiver.track + if (track is VideoTrack) { + track.addSink { handleVideoFrame(it) } + } + } + }, + ) + + val videoSource = CustomVideoSource() + val videoTrack = peerConnectionFactory.createVideoTrack("video0", videoSource) + peerConnection.addTrack(videoTrack, listOf("stream0")) + + // Create data channel so that both sides know when the connection is closed + peerConnection.createDataChannel("data0", RTCDataChannelInit()) + + LogManager.info("Creating offer...") + + val offer = suspendCoroutine { cont -> + peerConnection.createOffer( + RTCOfferOptions(), + object : CreateSessionDescriptionObserver { + override fun onSuccess(description: RTCSessionDescription) { + cont.resume(description) + } + override fun onFailure(error: String) { + cont.resumeWithException(IllegalStateException("Failed to create offer: $error")) + } + }, + ) + } + + LogManager.info("Setting local description...") + + suspendCoroutine { cont -> + peerConnection.setLocalDescription( + RTCSessionDescription(RTCSdpType.OFFER, offer.sdp), + object : SetSessionDescriptionObserver { + override fun onSuccess() { + cont.resume(Unit) + } + override fun onFailure(error: String) { + cont.resumeWithException(IllegalStateException("Failed to set local description: $error")) + } + }, + ) + } + + if (peerConnection.iceConnectionState == RTCIceConnectionState.COMPLETED) { + LogManager.info("ICE gathering already complete") + } else { + LogManager.info("Waiting for ICE gathering to complete...") + iceGatheringComplete.await() + } + + LogManager.info("Requesting answer SDP...") + + val answerSDP = requestAnswerSDP(peerConnection.localDescription.sdp) + + LogManager.info("Setting remote description...") + + suspendCoroutine { cont -> + peerConnection.setRemoteDescription( + RTCSessionDescription(RTCSdpType.ANSWER, answerSDP), + object : SetSessionDescriptionObserver { + override fun onSuccess() { + cont.resume(Unit) + } + override fun onFailure(error: String) { + cont.resumeWithException(IllegalStateException("Failed to set remote description: $error")) + } + }, + ) + } + + return peerConnection + } + + /** + * Connects to the phone's WebRTC signaling server and requests an answer SDP. + */ + private suspend fun requestAnswerSDP(offerSDP: String): String { + val client = HttpClient(CIO) { + install(ContentNegotiation) { + json() + } + } + + val urlBuilder = URLBuilder( + protocol = URLProtocol.HTTP, + host = webcamService.host.hostAddress, + port = webcamService.port, + pathSegments = listOf("offer"), + ) + + val url = urlBuilder.build() + LogManager.info("Sending WebRTC offer to: $url") + + val offerRequest = client.preparePost(url) { + contentType(ContentType.Application.Json) + setBody(OfferRequest(offerSDP)) + } + + val offerResponse = offerRequest.execute() + + if (offerResponse.status != HttpStatusCode.OK) { + error("Failed to get answer from signaling server") + } + + val answer = offerResponse.body() + + val cameraToWorld = answer.cameraToWorld.let { QuaternionD(it[0], it[1], it[2], it[3]) } + val intrinsic = answer.intrinsics.let { CameraIntrinsic(it.fx, it.fy, it.tx, it.ty) } + val imageSize = answer.imageSize.let { Dimension(it[0], it[1]) } + val camera = Camera(CameraExtrinsic.fromCameraPose(cameraToWorld, Vector3D.NULL), intrinsic, imageSize) + this.camera = camera + + debugOutput.saveCamera(camera) + + return answer.sdp + } + + /** + * Handles a video frame by processing it into a [BufferedImage] and sending it to + * the channel. + */ + private fun handleVideoFrame(videoFrame: VideoFrame) { + val camera = camera ?: return + + val now = TimeSource.Monotonic.markNow() + val timestamp = now - startTime + + val buffer = videoFrame.buffer + val rgbaBuffer = ByteArray(buffer.width * buffer.height * 4) + VideoBufferConverter.convertFromI420(buffer, rgbaBuffer, FourCC.RGBA) + + val raster = Raster.createInterleavedRaster( + DataBufferByte(rgbaBuffer, rgbaBuffer.size), + buffer.width, + buffer.height, + 4 * buffer.width, + 4, + intArrayOf(3, 2, 1, 0), + null, + ) + + val colorModel = ComponentColorModel( + ColorSpace.getInstance(ColorSpace.CS_sRGB), + true, + false, + Transparency.OPAQUE, + DataBuffer.TYPE_BYTE, + ) + + val image = BufferedImage(colorModel, raster, false, null) + + // TODO: Can we eliminate this? + val redrawnImage = BufferedImage(image.width, image.height, BufferedImage.TYPE_INT_RGB) + val g = redrawnImage.createGraphics() + g.drawImage(image, 0, 0, image.width, image.height, 0, 0, image.width, image.height, null) + g.dispose() + + debugOutput.saveWebcamImage(timestamp, redrawnImage) + + val imageSnapshot = ImageSnapshot(TimeSource.Monotonic.markNow(), timestamp, redrawnImage, camera) + imageSnapshots.trySend(imageSnapshot) + } + + @Serializable + private class OfferRequest( + val sdp: String, + ) + + @Serializable + private class IntrinsicsResponse( + val fx: Double, + val fy: Double, + val tx: Double, + val ty: Double, + ) + + @Serializable + private class OfferResponse( + val sdp: String, + val imageSize: List, + val intrinsics: IntrinsicsResponse, + val cameraToWorld: List, + ) +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/RtmposeOnnxPipeline.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/RtmposeOnnxPipeline.kt new file mode 100644 index 0000000000..6ca71f2446 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/RtmposeOnnxPipeline.kt @@ -0,0 +1,530 @@ +import ai.onnxruntime.OnnxTensor +import ai.onnxruntime.OrtEnvironment +import ai.onnxruntime.OrtException +import ai.onnxruntime.OrtSession +import ai.onnxruntime.TensorInfo +import dev.slimevr.tracking.videocalibration.data.CocoWholeBodyKeypoint +import java.awt.BasicStroke +import java.awt.Color +import java.awt.Graphics2D +import java.awt.RenderingHints +import java.awt.image.BufferedImage +import java.io.File +import java.nio.FloatBuffer +import java.util.concurrent.locks.ReentrantLock +import javax.imageio.ImageIO +import kotlin.concurrent.withLock +import kotlin.math.min + +data class JointResult( + val id: Int, + val name: CocoWholeBodyKeypoint, + val x: Double, + val y: Double, + val confidence: Double, + val visible: Boolean, +) + +data class PersonResult( + val person_id: Int, + val joints: List, +) + +data class PoseResponse( + val image_path: String, + val detections: List, +) + +/** + * Core RTMPose pipeline in Kotlin (pure JVM image processing): + * - decode image bytes with ImageIO + * - run YOLO detector ONNX + * - run RTMPose ONNX for each person box + * - return service-compatible JSON model + * + * Output assumptions (based on your exported models): + * - detector: dets [1,N,5] + labels [1,N] + * - pose: SimCC pair [1,K,384] and [1,K,512] + */ +class RtmposeOnnxPipeline( + private val yoloOnnxPath: String, + private val poseOnnxPath: String, + private val yoloScoreThreshold: Float = 0.3f, + private val poseScoreThreshold: Float = 0.5f, + private val useCudaIfAvailable: Boolean = false, + private val cudaDeviceId: Int = 0, + private val yoloInputWidth: Int = 640, + private val yoloInputHeight: Int = 640, + private val poseInputWidth: Int = 192, + private val poseInputHeight: Int = 256, +) : AutoCloseable { + + private val env: OrtEnvironment = OrtEnvironment.getEnvironment() + private var yoloSession: OrtSession? = null + private var poseSession: OrtSession? = null + private val inferLock = ReentrantLock() + private val cocoWholeBodyKeypoints = CocoWholeBodyKeypoint.values() + + fun start() { +// val yoloOptions = OrtSession.SessionOptions() +// yoloOptions.setOptimizationLevel(OrtSession.SessionOptions.OptLevel.ALL_OPT) +// configureExecutionProvider(yoloOptions) +// yoloSession = env.createSession(yoloOnnxPath, yoloOptions) + + val poseOptions = OrtSession.SessionOptions() + poseOptions.setOptimizationLevel(OrtSession.SessionOptions.OptLevel.ALL_OPT) + configureExecutionProvider(poseOptions) + poseSession = env.createSession(poseOnnxPath, poseOptions) + + validateSessionSchemas() + } + + private fun configureExecutionProvider(options: OrtSession.SessionOptions) { + if (!useCudaIfAvailable) return + try { + options.addCUDA(cudaDeviceId) + println("ONNX Runtime: CUDA EP enabled (device=$cudaDeviceId).") + } catch (t: Throwable) { + // Falls back to CPU EP automatically when CUDA EP cannot be added. + println("ONNX Runtime: CUDA EP unavailable, falling back to CPU. Reason: ${t.message}") + } + } + + fun processImage(image: BufferedImage, imageLabel: String = ""): PoseResponse { + val rgbImage = toRgb(image) + return inferLock.withLock { +// val personBoxes = runDetector(rgbImage) +// if (personBoxes.isNotEmpty()) { +// saveDebugBboxImage(rgbImage, personBoxes[0], File("C:\\SlimeVR\\bbox.jpg")) +// } + val personBoxes = listOf(BBox(0.0f, 0.0f, image.width.toFloat(), image.height.toFloat())) + val detections = personBoxes.mapIndexedNotNull { personId, box -> + val pose = runPoseForBox(rgbImage, box) ?: return@mapIndexedNotNull null + val joints = pose.mapIndexed { idx, kp -> + JointResult( + id = idx, + name = keypointName(idx), + x = kp.x.toDouble(), + y = kp.y.toDouble(), + confidence = kp.score.toDouble(), + visible = kp.score >= poseScoreThreshold, + ) + } + PersonResult(person_id = personId, joints = joints) + } + PoseResponse(image_path = imageLabel, detections = detections) + } + } + + private fun saveDebugBboxImage(image: BufferedImage, box: BBox, outputFile: File) { + val out = BufferedImage(image.width, image.height, BufferedImage.TYPE_INT_RGB) + val g = out.createGraphics() + g.drawImage(image, 0, 0, null) + g.color = Color(255, 0, 0) + g.stroke = BasicStroke(3f) + g.drawRect( + box.x1.toInt(), + box.y1.toInt(), + box.width().toInt().coerceAtLeast(1), + box.height().toInt().coerceAtLeast(1), + ) + g.dispose() + outputFile.parentFile?.mkdirs() + ImageIO.write(out, "jpg", outputFile) + } + + private fun runDetector(image: BufferedImage): List { + val yoloSession = yoloSession ?: return listOf() + + val prep = letterboxRgbToNchw(image, yoloInputWidth, yoloInputHeight) + val inputName = yoloSession.inputNames.iterator().next() + val shape = longArrayOf(1, 3, yoloInputHeight.toLong(), yoloInputWidth.toLong()) + val tensor = OnnxTensor.createTensor(env, FloatBuffer.wrap(prep.nchw), shape) + + yoloSession.run(mapOf(inputName to tensor)).use { output -> + val boxes = parseDetectorOutputs(output, prep.scale, prep.padX, prep.padY, image.width, image.height) + val best = boxes.maxByOrNull { it.score } ?: return emptyList() + return listOf(best.box) + } + } + + private fun runPoseForBox(image: BufferedImage, box: BBox): List? { + val poseSession = poseSession ?: return listOf() + + val clamped = clampBox(box, image.width, image.height) ?: return null + val crop = image.getSubimage(clamped.x1.toInt(), clamped.y1.toInt(), clamped.width().toInt(), clamped.height().toInt()) + val resized = resizeImage(crop, poseInputWidth, poseInputHeight) + val chw = rgbToNchw(resized) + + val inputName = poseSession.inputNames.iterator().next() + val shape = longArrayOf(1, 3, poseInputHeight.toLong(), poseInputWidth.toLong()) + val tensor = OnnxTensor.createTensor(env, FloatBuffer.wrap(chw), shape) + + poseSession.run(mapOf(inputName to tensor)).use { output -> + val keypoints = parsePoseOutputs(output) ?: return null + return keypoints.map { kp -> + val x = clamped.x1 + kp.x * clamped.width() / poseInputWidth.toFloat() + val y = clamped.y1 + kp.y * clamped.height() / poseInputHeight.toFloat() + Keypoint(x = x, y = y, score = kp.score) + } + } + } + + private fun parsePoseOutputs(output: OrtSession.Result): List? { + val simccXTensor = output[0] as? OnnxTensor + ?: throw IllegalArgumentException( + "Unexpected pose output[0] type: ${output[0]::class.java.name}. Expected OnnxTensor.", + ) + val simccYTensor = output[1] as? OnnxTensor + ?: throw IllegalArgumentException( + "Unexpected pose output[1] type: ${output[1]::class.java.name}. Expected OnnxTensor.", + ) + + val xInfo = simccXTensor.info as? TensorInfo + ?: throw IllegalArgumentException("Pose output 'simcc_x' is not tensor info.") + val yInfo = simccYTensor.info as? TensorInfo + ?: throw IllegalArgumentException("Pose output 'simcc_y' is not tensor info.") + val xShape = xInfo.shape + val yShape = yInfo.shape + require(xShape.size == 3 && yShape.size == 3) { + "Unexpected pose output shapes: simcc_x=${xShape.contentToString()}, simcc_y=${yShape.contentToString()}" + } + require(xShape[0] == yShape[0] && xShape[1] == yShape[1]) { + "Mismatched simcc batch/keypoint dims: simcc_x=${xShape.contentToString()}, simcc_y=${yShape.contentToString()}" + } + + val kCount = xShape[1].toInt() + val xBins = xShape[2].toInt() + val yBins = yShape[2].toInt() + val simccSplitRatioX = xBins.toFloat() / poseInputWidth.toFloat() + val simccSplitRatioY = yBins.toFloat() / poseInputHeight.toFloat() + + val xBuf = simccXTensor.floatBuffer.duplicate() + val yBuf = simccYTensor.floatBuffer.duplicate() + val xFlat = FloatArray(xBuf.remaining()) + val yFlat = FloatArray(yBuf.remaining()) + xBuf.get(xFlat) + yBuf.get(yFlat) + + require(xFlat.size >= kCount * xBins && yFlat.size >= kCount * yBins) { + "SimCC buffers are smaller than expected for one batch: " + + "x=${xFlat.size} need>=${kCount * xBins}, y=${yFlat.size} need>=${kCount * yBins}" + } + + val out = ArrayList(kCount) + for (i in 0 until kCount) { + val (xIdx, xScore) = argMaxSlice(xFlat, i * xBins, xBins) + val (yIdx, yScore) = argMaxSlice(yFlat, i * yBins, yBins) + val x = xIdx.toFloat() / simccSplitRatioX + val y = yIdx.toFloat() / simccSplitRatioY + out.add(Keypoint(x = x, y = y, score = min(xScore, yScore))) + } + return out + } + + private fun validateSessionSchemas() { +// val yoloSession = yoloSession ?: return + val poseSession = poseSession ?: return + +// val yoloInputNames = yoloSession.inputNames +// require(yoloInputNames.contains("input")) { +// "YOLO model must expose input named 'input'. Found: $yoloInputNames" +// } +// +// val yoloOutputNames = yoloSession.outputNames +// require(yoloOutputNames.contains("dets")) { +// "YOLO model must expose output named 'dets'. Found: $yoloOutputNames" +// } +// require(yoloOutputNames.contains("labels")) { +// "YOLO model must expose output named 'labels'. Found: $yoloOutputNames" +// } + + val poseInputNames = poseSession.inputNames + require(poseInputNames.contains("input")) { + "RTMPose model must expose input named 'input'. Found: $poseInputNames" + } + + val poseOutputNames = poseSession.outputNames + require(poseOutputNames.contains("simcc_x")) { + "RTMPose model must expose output named 'simcc_x'. Found: $poseOutputNames" + } + require(poseOutputNames.contains("simcc_y")) { + "RTMPose model must expose output named 'simcc_y'. Found: $poseOutputNames" + } + + // Soft shape checks for extra safety; fail only when the schema is clearly incompatible. +// validateExpectedRank( +// session = yoloSession, +// tensorName = "dets", +// expectedRank = 3, +// modelName = "YOLO", +// ) +// validateExpectedRank( +// session = yoloSession, +// tensorName = "labels", +// expectedRank = 2, +// modelName = "YOLO", +// ) + validateExpectedRank( + session = poseSession, + tensorName = "simcc_x", + expectedRank = 3, + modelName = "RTMPose", + ) + validateExpectedRank( + session = poseSession, + tensorName = "simcc_y", + expectedRank = 3, + modelName = "RTMPose", + ) + } + + private fun validateExpectedRank( + session: OrtSession, + tensorName: String, + expectedRank: Int, + modelName: String, + ) { + val nodeInfo = session.outputInfo[tensorName] + ?: throw IllegalArgumentException("$modelName output '$tensorName' is missing.") + val tensorInfo = nodeInfo.info as? TensorInfo + ?: throw IllegalArgumentException("$modelName output '$tensorName' is not a tensor.") + val rank = tensorInfo.shape.size + require(rank == expectedRank) { + "$modelName output '$tensorName' rank mismatch. Expected $expectedRank, got $rank " + + "(shape=${tensorInfo.shape.contentToString()})" + } + } + + private fun parseDetectorOutputs( + output: OrtSession.Result, + scale: Float, + padX: Float, + padY: Float, + imageW: Int, + imageH: Int, + ): List { + val detsTensor = output[0] as? OnnxTensor + ?: throw IllegalArgumentException( + "Unexpected detector output[0] type: ${output[0]::class.java.name}. Expected OnnxTensor.", + ) + val labelsTensor = output[1] as? OnnxTensor + ?: throw IllegalArgumentException( + "Unexpected detector output[1] type: ${output[1]::class.java.name}. Expected OnnxTensor.", + ) + return parseDetsWithLabels(detsTensor, labelsTensor, scale, padX, padY, imageW, imageH) + } + + private fun parseDetsWithLabels( + detsTensor: OnnxTensor, + labelsTensor: OnnxTensor, + scale: Float, + padX: Float, + padY: Float, + imageW: Int, + imageH: Int, + ): List { + val detInfo = detsTensor.info as? TensorInfo + ?: throw IllegalArgumentException("Detector 'dets' output is not tensor info.") + val detShape = detInfo.shape + require(detShape.size == 3 && detShape[0] == 1L && detShape[2] == 5L) { + "Unexpected detector 'dets' shape ${detShape.contentToString()}; expected [1,N,5]." + } + + val detBuf = detsTensor.floatBuffer.duplicate() + val detFlat = FloatArray(detBuf.remaining()) + detBuf.get(detFlat) + require(detFlat.size % 5 == 0) { + "Detector 'dets' flat size must be divisible by 5, got ${detFlat.size}." + } + val nByFlat = detFlat.size / 5 + val nByShape = if (detShape[1] > 0) detShape[1].toInt() else nByFlat + val labelsCount = labelsTensor.info.let { info -> + (info as? TensorInfo)?.shape?.let { shape -> + if (shape.size == 2 && shape[0] == 1L && shape[1] > 0) shape[1].toInt() else nByFlat + } ?: nByFlat + } + // Runtime buffers are authoritative; TensorInfo can be partially symbolic/static. + val n = min(nByFlat, min(nByShape, labelsCount)) + require(n > 0) { "Detector produced no candidate boxes (nByFlat=$nByFlat, nByShape=$nByShape, labelsCount=$labelsCount)." } + + val out = ArrayList(n) + for (i in 0 until n) { + val base = i * 5 + + // Match rtmlib YOLOX outputs.shape[-1] == 5 path: + // use detector score directly with score threshold. + val score = detFlat[base + 4] + if (score < yoloScoreThreshold) continue + + val box = fromLetterboxedXYXY( + x1 = detFlat[base], + y1 = detFlat[base + 1], + x2 = detFlat[base + 2], + y2 = detFlat[base + 3], + scale = scale, + padX = padX, + padY = padY, + imageW = imageW, + imageH = imageH, + ) + if (box != null) out.add(ScoredBox(box, score)) + } + return out + } + + private fun fromLetterboxedXYXY( + x1: Float, + y1: Float, + x2: Float, + y2: Float, + scale: Float, + padX: Float, + padY: Float, + imageW: Int, + imageH: Int, + ): BBox? { + val ox1 = ((x1 - padX) / scale).coerceIn(0f, (imageW - 1).toFloat()) + val oy1 = ((y1 - padY) / scale).coerceIn(0f, (imageH - 1).toFloat()) + val ox2 = ((x2 - padX) / scale).coerceIn(0f, (imageW - 1).toFloat()) + val oy2 = ((y2 - padY) / scale).coerceIn(0f, (imageH - 1).toFloat()) + val box = BBox(ox1, oy1, ox2, oy2) + return if (box.width() > 2f && box.height() > 2f) box else null + } + + private fun clampBox(box: BBox, imageW: Int, imageH: Int): BBox? { + val x1 = box.x1.coerceIn(0f, (imageW - 1).toFloat()) + val y1 = box.y1.coerceIn(0f, (imageH - 1).toFloat()) + val x2 = box.x2.coerceIn(0f, (imageW - 1).toFloat()) + val y2 = box.y2.coerceIn(0f, (imageH - 1).toFloat()) + val clamped = BBox(x1, y1, x2, y2) + return if (clamped.width() > 2f && clamped.height() > 2f) clamped else null + } + + private fun argMaxSlice(arr: FloatArray, offset: Int, length: Int): Pair { + if (length <= 0) return 0 to 0f + var idx = 0 + var best = arr[offset] + var i = 1 + while (i < length) { + val v = arr[offset + i] + if (v > best) { + best = v + idx = i + } + i += 1 + } + return idx to best + } + + private fun keypointName(index: Int): CocoWholeBodyKeypoint = cocoWholeBodyKeypoints.getOrNull(index) + ?: throw IllegalArgumentException( + "Keypoint index $index is out of range for CocoWholeBodyKeypoint " + + "(size=${cocoWholeBodyKeypoints.size}).", + ) + + private fun letterboxRgbToNchw(image: BufferedImage, dstW: Int, dstH: Int): Preprocessed { + val srcW = image.width.toFloat() + val srcH = image.height.toFloat() + val scale = min(dstW / srcW, dstH / srcH) + val newW = (srcW * scale).toInt() + val newH = (srcH * scale).toInt() + + val canvas = BufferedImage(dstW, dstH, BufferedImage.TYPE_INT_RGB) + val g = canvas.createGraphics() + g.color = Color(114, 114, 114) + g.fillRect(0, 0, dstW, dstH) + g.setRenderingHint(RenderingHints.KEY_INTERPOLATION, RenderingHints.VALUE_INTERPOLATION_BILINEAR) + // Match rtmlib YOLOX preprocess: paste resized image at top-left. + g.drawImage(image, 0, 0, newW, newH, null) + g.dispose() + + return Preprocessed(detectorToNchwYoloX(canvas), scale, 0f, 0f) + } + + private fun resizeImage(src: BufferedImage, width: Int, height: Int): BufferedImage { + val out = BufferedImage(width, height, BufferedImage.TYPE_INT_RGB) + val g: Graphics2D = out.createGraphics() + g.setRenderingHint(RenderingHints.KEY_INTERPOLATION, RenderingHints.VALUE_INTERPOLATION_BILINEAR) + g.drawImage(src, 0, 0, width, height, null) + g.dispose() + return out + } + + private fun toRgb(src: BufferedImage): BufferedImage { + if (src.type == BufferedImage.TYPE_INT_RGB) return src + val out = BufferedImage(src.width, src.height, BufferedImage.TYPE_INT_RGB) + val g = out.createGraphics() + g.drawImage(src, 0, 0, null) + g.dispose() + return out + } + + private fun rgbToNchw(image: BufferedImage): FloatArray { + val width = image.width + val height = image.height + val pixels = IntArray(width * height) + image.getRGB(0, 0, width, height, pixels, 0, width) + + val hw = width * height + val chw = FloatArray(hw * 3) + for (i in pixels.indices) { + val p = pixels[i] + val r = (p shr 16 and 0xFF) / 255.0f + val g = (p shr 8 and 0xFF) / 255.0f + val b = (p and 0xFF) / 255.0f + chw[i] = r + chw[hw + i] = g + chw[2 * hw + i] = b + } + return chw + } + + /** + * Match rtmlib YOLOX input formatting: + * - channel order: BGR + * - value range: raw 0..255 float32 + * - no mean/std normalization + */ + private fun detectorToNchwYoloX(image: BufferedImage): FloatArray { + val width = image.width + val height = image.height + val pixels = IntArray(width * height) + image.getRGB(0, 0, width, height, pixels, 0, width) + + val hw = width * height + val chw = FloatArray(hw * 3) + for (i in pixels.indices) { + val p = pixels[i] + val r = (p shr 16 and 0xFF).toFloat() + val g = (p shr 8 and 0xFF).toFloat() + val b = (p and 0xFF).toFloat() + + chw[i] = b + chw[hw + i] = g + chw[2 * hw + i] = r + } + return chw + } + + override fun close() { + val yoloSession = yoloSession ?: return + val poseSession = poseSession ?: return + try { + yoloSession.close() + } catch (_: OrtException) { + } + try { + poseSession.close() + } catch (_: OrtException) { + } + } + + private data class Keypoint(val x: Float, val y: Float, val score: Float) + private data class BBox(val x1: Float, val y1: Float, val x2: Float, val y2: Float) { + fun width(): Float = x2 - x1 + fun height(): Float = y2 - y1 + } + private data class ScoredBox(val box: BBox, val score: Float) + private data class Preprocessed(val nchw: FloatArray, val scale: Float, val padX: Float, val padY: Float) +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/SnapshotsDatabase.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/SnapshotsDatabase.kt new file mode 100644 index 0000000000..b40a02bbd1 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/SnapshotsDatabase.kt @@ -0,0 +1,96 @@ +package dev.slimevr.tracking.videocalibration.sources + +import dev.slimevr.tracking.videocalibration.snapshots.HumanPoseSnapshot +import dev.slimevr.tracking.videocalibration.snapshots.TrackersSnapshot +import kotlinx.coroutines.channels.Channel +import kotlinx.coroutines.channels.onFailure +import kotlinx.coroutines.channels.onSuccess +import kotlin.time.Duration + +/** + * Database of snapshots. + */ +class SnapshotsDatabase( + private val maxJitter: Duration, + private val humanPoseFramesSource: Channel, + private val trackersFramesSource: Channel, +) { + + val allHumanPoseSnapshots = mutableListOf() + val allTrackersSnapshots = mutableListOf() + + val recentHumanPoseSnapshots = mutableListOf() + val recentTrackersSnapshots = mutableListOf() + + /** + * Inserts all pending snapshots into the database. + */ + fun update() { + while (true) { + humanPoseFramesSource.tryReceive() + .onSuccess { + allHumanPoseSnapshots.add(it) + recentHumanPoseSnapshots.add(it) + } + .onFailure { break } + } + + while (true) { + trackersFramesSource.tryReceive() + .onSuccess { + allTrackersSnapshots.add(it) + recentTrackersSnapshots.add(it) + } + .onFailure { break } + } + } + + fun matchAll(poseDelay: Duration) = match(allHumanPoseSnapshots, allTrackersSnapshots, poseDelay) + + fun matchRecent(poseDelay: Duration) = match(recentHumanPoseSnapshots, recentTrackersSnapshots, poseDelay) + + /** + * Matches human pose snapshots with tracker snapshots, after applying a delay to + * the human pose snapshots since they may be captured at different times. + */ + private fun match( + humanPoseSnapshots: List, + trackersSnapshots: List, + humanPoseDelay: Duration, + ): List> { + val result = mutableListOf>() + + var j = 0 + for (pose in humanPoseSnapshots) { + val poseInstant = pose.instant + humanPoseDelay + + while (j + 1 < trackersSnapshots.size) { + val current = trackersSnapshots[j] + val currentDiff = (poseInstant - current.instant).absoluteValue + + val next = trackersSnapshots[j + 1] + val nextDiff = (poseInstant - next.instant).absoluteValue + + if (nextDiff < currentDiff) { + j++ + } else { + break + } + } + + if ((trackersSnapshots[j].instant - poseInstant).absoluteValue < maxJitter) { + result += Pair(trackersSnapshots[j], pose) + } + } + + return result + } + + /** + * Clears the recent snapshots. + */ + fun clearRecent() { + recentHumanPoseSnapshots.clear() + recentTrackersSnapshots.clear() + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/TrackersSource.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/TrackersSource.kt new file mode 100644 index 0000000000..eb705dc822 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/sources/TrackersSource.kt @@ -0,0 +1,124 @@ +package dev.slimevr.tracking.videocalibration.sources + +import dev.slimevr.tracking.trackers.Tracker +import dev.slimevr.tracking.trackers.TrackerPosition +import dev.slimevr.tracking.videocalibration.snapshots.TrackerSnapshot +import dev.slimevr.tracking.videocalibration.snapshots.TrackersSnapshot +import dev.slimevr.tracking.videocalibration.util.DebugOutput +import dev.slimevr.tracking.videocalibration.util.ScheduledInterval +import io.eiren.util.logging.LogManager +import io.github.axisangles.ktmath.Vector3D +import kotlinx.coroutines.channels.Channel +import kotlin.time.Duration +import kotlin.time.TimeSource + +/** + * Takes snapshots of the trackers at a fixed interval. + */ +class TrackersSource( + private val trackersToRecord: Map, + private val interval: Duration, + private val debugOutput: DebugOutput, +) { + + enum class Status { + NOT_STARTED, + RUNNING, + DONE, + } + + var status = Status.NOT_STARTED + private set + + val trackersSnapshots = Channel(Channel.Factory.UNLIMITED) + + private val scheduler = ScheduledInterval(interval) + private val allTrackerSnapshots = mutableListOf() + + init { +// val missingTrackers = REQUIRED_TRACKERS_TO_RECORD.subtract(trackersToRecord.keys) +// require(missingTrackers.isEmpty()) { "Required trackers are missing: $missingTrackers" } + } + + /** + * Starts taking snapshots. Snapshots will be published to [trackersSnapshots]. + */ + fun start() { + status = Status.RUNNING + } + + /** + * Stops taking snapshots. + */ + fun requestStop() { + status = Status.DONE + debugOutput.saveTrackerSnapshots(trackersToRecord, allTrackerSnapshots, interval) + } + + /** + * Must be called on each server tick, to take snapshots at appropriate times. + */ + fun onTick() { + if (!scheduler.shouldInvoke()) { + return + } + + val snapshot: TrackersSnapshot + try { + snapshot = makeSnapshot() + } catch (e: Exception) { + LogManager.warning("Failed to create trackers snapshot", e) + requestStop() + return + } + + allTrackerSnapshots.add(snapshot) + trackersSnapshots.trySend(snapshot) + } + + /** + * Creates a snapshot of the trackers to record. + */ + private fun makeSnapshot(): TrackersSnapshot { + val now = TimeSource.Monotonic.markNow() + + val snapshots = mutableMapOf() + for ((trackerPosition, tracker) in trackersToRecord) { + if (tracker.trackerPosition != trackerPosition) { + error("Tracker ${tracker.name} position has changed") + } + + val rawTrackerToWorld = tracker.getRawRotation().toDouble() + val adjustedTrackerToWorld = tracker.getRotation().toDouble() + + var trackerOriginInWorld: Vector3D? = null + if (TRACKERS_WITH_POSITION.contains(trackerPosition)) { + if (tracker.hasPosition) { + trackerOriginInWorld = tracker.position.toDouble() + } else { + error("Tracker $trackerPosition is missing required position") + } + } + + snapshots[trackerPosition] = + TrackerSnapshot( + rawTrackerToWorld, + adjustedTrackerToWorld, + trackerOriginInWorld, + ) + } + + return TrackersSnapshot(now, snapshots) + } + + companion object { + + private val REQUIRED_TRACKERS_TO_RECORD = setOf( + TrackerPosition.HEAD, + TrackerPosition.LEFT_HAND, + TrackerPosition.RIGHT_HAND, + ) + + private val TRACKERS_WITH_POSITION = REQUIRED_TRACKERS_TO_RECORD + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/CaptureBentOverPose.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/CaptureBentOverPose.kt new file mode 100644 index 0000000000..14fa0d7955 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/CaptureBentOverPose.kt @@ -0,0 +1,101 @@ +package dev.slimevr.tracking.videocalibration.steps + +import dev.slimevr.tracking.trackers.TrackerPosition +import dev.slimevr.tracking.videocalibration.snapshots.TrackersSnapshot +import dev.slimevr.tracking.videocalibration.util.toAngleAxisString +import io.eiren.util.logging.LogManager +import io.github.axisangles.ktmath.QuaternionD +import org.apache.commons.math3.util.FastMath +import kotlin.time.Duration.Companion.seconds + +class CaptureBentOverPose { + + class Solution( + val reference: QuaternionD, + // Snapshot of rotations of all the trackers when the bent over pose was captured + val trackerRotations: List>, + ) + + private val minDuration = 1.seconds + private val maxAngleDeviation = FastMath.toRadians(5.0) + private val minStableSnapshots = 30 + + // TODO: Same as SolveUpperBodyTrackerReset + val trackerPositions = setOf( + TrackerPosition.UPPER_CHEST, + TrackerPosition.CHEST, + TrackerPosition.WAIST, + TrackerPosition.HIP, + ) + + val minBentOverAngle = FastMath.toRadians(30.0) + + fun capture( + trackersSnapshots: List, + forwardPose: CaptureForwardPose.Solution, + ): Solution? { + if (trackersSnapshots.isEmpty()) { + return null + } + + val headRotations = trackersSnapshots.mapNotNull { + val head = it.trackers[TrackerPosition.HEAD] ?: return@mapNotNull null + it.instant to head.rawTrackerToWorld + } + + if (headRotations.isEmpty()) { + return null + } + + val (latestInstant, latestHeadRotation) = headRotations.last() + + // TODO: Upper body trackers must have at least 30 deg inclination from rotation at reference + + var numStableSnapshots = 0 + var hasMinDuration = false + for ((instant, headRotation) in headRotations.reversed()) { + if ((latestHeadRotation * headRotation.inv()).angleR() > maxAngleDeviation) { + break + } + ++numStableSnapshots + + if (latestInstant - instant > minDuration) { + hasMinDuration = true + break + } + } + + if (!hasMinDuration || numStableSnapshots < minStableSnapshots) { + return null + } + + for (trackerPosition in trackerPositions) { + val bentOverTrackerRotation = trackersSnapshots.last().trackers[trackerPosition] + val forwardPoseTrackerRotation = forwardPose.trackerRotations.last()[trackerPosition] + if (bentOverTrackerRotation != null && forwardPoseTrackerRotation != null) { + // TODO: Required tracker must have data + if (bentOverTrackerRotation.rawTrackerToWorld.angleToR( + forwardPoseTrackerRotation, + ) < minBentOverAngle + ) { + LogManager.debug("Skipping because not bent over enough") + return null + } + } + } + + // TODO: Get average over duration instead + + LogManager.info("Found bent-over pose: ${latestHeadRotation.toAngleAxisString()}") + + val trackerRotations = trackersSnapshots.takeLast(50).map { + it.trackers + .mapNotNull { (trackerPosition, trackerSnapshot) -> + trackerPosition to trackerSnapshot.rawTrackerToWorld + } + .toMap() + } + + return Solution(latestHeadRotation, trackerRotations) + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/CaptureForwardPose.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/CaptureForwardPose.kt new file mode 100644 index 0000000000..722d42e2ac --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/CaptureForwardPose.kt @@ -0,0 +1,89 @@ +package dev.slimevr.tracking.videocalibration.steps + +import dev.slimevr.tracking.trackers.TrackerPosition +import dev.slimevr.tracking.videocalibration.snapshots.TrackersSnapshot +import dev.slimevr.tracking.videocalibration.util.toAngleAxisString +import io.eiren.util.logging.LogManager +import io.github.axisangles.ktmath.Matrix3D +import io.github.axisangles.ktmath.QuaternionD +import io.github.axisangles.ktmath.Vector3D +import org.apache.commons.math3.util.FastMath +import kotlin.math.* +import kotlin.time.Duration.Companion.seconds + +class CaptureForwardPose { + + class Solution( + // Reference yaw (to align the Z-axes of all the trackers, i.e. backwards) + val reference: QuaternionD, + // Snapshot of rotations of all the trackers when the reference yaw was captured + val trackerRotations: List>, + ) + + private val minDuration = 2.seconds + private val maxAngleDeviation = FastMath.toRadians(5.0) + private val minAngleFromVertical = FastMath.toRadians(20.0) + private val minStableSnapshots = 30 + + fun capture(trackersSnapshots: List): Solution? { + if (trackersSnapshots.isEmpty()) { + return null + } + + val headRotations = trackersSnapshots.mapNotNull { + val head = it.trackers[TrackerPosition.HEAD] ?: return@mapNotNull null + it.instant to head.rawTrackerToWorld + } + + if (headRotations.isEmpty()) { + return null + } + + val (latestInstant, latestHeadRotation) = headRotations.last() + + // HEAD tracker must be level + if ( + abs(latestHeadRotation.sandwichUnitY().dot(Vector3D.POS_Y)) < cos(minAngleFromVertical) + ) { + return null + } + + var numStableSnapshots = 0 + var hasMinDuration = false + for ((instant, headRotation) in headRotations.reversed()) { + if (latestHeadRotation.angleToR(headRotation) > maxAngleDeviation) { + break + } + ++numStableSnapshots + + if (latestInstant - instant > minDuration) { + hasMinDuration = true + break + } + } + + if (!hasMinDuration || numStableSnapshots < minStableSnapshots) { + return null + } + + // TODO: Get average over duration instead + + val headZAxis = latestHeadRotation.sandwichUnitZ() + val zAxis = Vector3D(headZAxis.x, 0.0, headZAxis.z).unit() + val yAxis = Vector3D.POS_Y + val xAxis = yAxis.cross(zAxis) + val reference = Matrix3D(xAxis, yAxis, zAxis).toQuaternionAssumingOrthonormal() + + LogManager.info("Found forward pose: ${reference.toAngleAxisString()}") + + val trackerRotations = trackersSnapshots.takeLast(50).map { + it.trackers + .mapNotNull { (trackerPosition, trackerSnapshot) -> + trackerPosition to trackerSnapshot.rawTrackerToWorld + } + .toMap() + } + + return Solution(reference, trackerRotations) + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/SkeletonOffsetsSolver.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/SkeletonOffsetsSolver.kt new file mode 100644 index 0000000000..320e408183 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/SkeletonOffsetsSolver.kt @@ -0,0 +1,223 @@ +package dev.slimevr.tracking.videocalibration.steps + +import dev.slimevr.tracking.processor.Bone +import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets +import dev.slimevr.tracking.processor.skeleton.refactor.Skeleton +import dev.slimevr.tracking.processor.skeleton.refactor.SkeletonUpdater +import dev.slimevr.tracking.trackers.TrackerPosition +import dev.slimevr.tracking.videocalibration.data.Camera +import dev.slimevr.tracking.videocalibration.data.CocoWholeBodyKeypoint +import dev.slimevr.tracking.videocalibration.data.TrackerResetOverride +import dev.slimevr.tracking.videocalibration.snapshots.HumanPoseSnapshot +import dev.slimevr.tracking.videocalibration.snapshots.TrackerSnapshot +import dev.slimevr.tracking.videocalibration.snapshots.TrackersSnapshot +import dev.slimevr.tracking.videocalibration.util.DebugOutput +import dev.slimevr.tracking.videocalibration.util.numericalJacobian +import io.eiren.util.logging.LogManager +import io.github.axisangles.ktmath.Vector2D +import org.apache.commons.math3.analysis.MultivariateVectorFunction +import org.apache.commons.math3.fitting.leastsquares.LeastSquaresBuilder +import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer +import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer +import kotlin.math.min + +class SkeletonOffsetsSolver( + private val debugOutput: DebugOutput, +) { + + class Solution( + val skeletonOffsets: Map, + ) + + private val skeleton = Skeleton(false, false) + + fun solve( + frames: List>, + camera: Camera, + initialSkeletonOffsets: Map, + trackerResets: Map, + ): Solution? { + val adjustedFrames = frames.map { (trackersSnapshot, humanPoseSnapshot) -> + val fixedTrackersSnapshot = + TrackersSnapshot( + trackersSnapshot.instant, + trackersSnapshot.trackers.map { (trackerPosition, trackerSnapshot) -> + val trackerReset = trackerResets[trackerPosition] + if (trackerReset != null) { + trackerPosition to + TrackerSnapshot( + trackerSnapshot.rawTrackerToWorld, + trackerReset.toBoneRotation(trackerSnapshot.rawTrackerToWorld), + trackerSnapshot.trackerOriginInWorld, + ) + } else { + trackerPosition to trackerSnapshot + } + }.toMap(), + ) + fixedTrackersSnapshot to humanPoseSnapshot + } + + val filteredAdjustedFrames = adjustedFrames.filterIndexed { index, _ -> index % SKIP_FRAMES == 0 } + + LogManager.info("Solving skeleton offsets with ${filteredAdjustedFrames.size} frames...") + + if (filteredAdjustedFrames.size < MIN_FRAMES) { + return null + } + + val n = filteredAdjustedFrames.size * NUM_JOINTS * 2 + + val costFn = MultivariateVectorFunction { p -> + var index = 0 + val residuals = DoubleArray(n) { 0.0 } + + val config = SkeletonUpdater.HumanSkeletonConfig() + val skeletonOffsets = makeSkeletonOffsets(p, initialSkeletonOffsets) + for (frame in filteredAdjustedFrames) { + val (trackersSnapshot, humanPoseSnapshot) = frame + val trackersData = SkeletonUpdater.TrackersData.fromSnapshot(trackersSnapshot.trackers) + val joints = humanPoseSnapshot.joints + + val skeletonUpdater = SkeletonUpdater(skeleton, trackersData, config, skeletonOffsets) + skeletonUpdater.update() + + val leftShoulderJoint = joints[CocoWholeBodyKeypoint.LEFT_SHOULDER] + val rightShoulderJoint = joints[CocoWholeBodyKeypoint.RIGHT_SHOULDER] + + index = addResidual(residuals, index, camera, skeleton.leftUpperArmBone, leftShoulderJoint) + index = addResidual(residuals, index, camera, skeleton.rightUpperArmBone, rightShoulderJoint) + + if (leftShoulderJoint != null && rightShoulderJoint != null) { + index = addResidual(residuals, index, camera, skeleton.upperChestBone, (leftShoulderJoint + rightShoulderJoint) * 0.5) + } + + index = addResidual(residuals, index, camera, skeleton.leftUpperLegBone, joints[CocoWholeBodyKeypoint.LEFT_HIP]) + index = addResidual(residuals, index, camera, skeleton.leftLowerLegBone, joints[CocoWholeBodyKeypoint.LEFT_KNEE]) + index = addResidual(residuals, index, camera, skeleton.leftFootBone, joints[CocoWholeBodyKeypoint.LEFT_ANKLE]) + + index = addResidual(residuals, index, camera, skeleton.rightUpperLegBone, joints[CocoWholeBodyKeypoint.RIGHT_HIP]) + index = addResidual(residuals, index, camera, skeleton.rightLowerLegBone, joints[CocoWholeBodyKeypoint.RIGHT_KNEE]) + index = addResidual(residuals, index, camera, skeleton.rightFootBone, joints[CocoWholeBodyKeypoint.RIGHT_ANKLE]) + } + + return@MultivariateVectorFunction residuals + } + + val model = numericalJacobian(costFn) + + // TODO: Use params + val initial = doubleArrayOf( + 0.7, // TORSO + 0.35, // HIPS_WIDTH + 0.5, // UPPER_LEG + 0.5, // LOWER_LEG + 0.15, // NECK + 0.20, // HEAD + ) + + val problem = LeastSquaresBuilder() + .start(initial) + .model(model) + .target(DoubleArray(n) { 0.0 }) + .maxEvaluations(10000) + .maxIterations(10000) + .build() + + val optimizer = LevenbergMarquardtOptimizer() + + val result: LeastSquaresOptimizer.Optimum + try { + result = optimizer.optimize(problem) + } catch (e: Exception) { + LogManager.warning("Failed to solve skeleton offsets: $e", e) + return null + } + + val skeletonOffsets = makeSkeletonOffsets(result.point.toArray(), initialSkeletonOffsets) + + // Give some of the neck back to the upper chest so that head movement doesn't cause as much movement in upper body + val neck = skeletonOffsets[SkeletonConfigOffsets.NECK] + if (neck != null) { + skeletonOffsets[SkeletonConfigOffsets.NECK] = neck * 0.5f + val upperChest = skeletonOffsets[SkeletonConfigOffsets.UPPER_CHEST] + if (upperChest != null) { + skeletonOffsets[SkeletonConfigOffsets.UPPER_CHEST] = upperChest + neck * 0.5f + skeletonOffsets[SkeletonConfigOffsets.SHOULDERS_DISTANCE] = neck * 0.5f + } + } + + // Adjust ankle to the ground + val lowerLegLength = skeletonOffsets[SkeletonConfigOffsets.LOWER_LEG] + if (lowerLegLength != null) { + skeletonOffsets[SkeletonConfigOffsets.LOWER_LEG] = lowerLegLength + ANKLE_TO_HEEL_LENGTH.toFloat() + } + + fun printSkeletonOffset(skeletonOffset: SkeletonConfigOffsets) { + val formatter = "%.2f" + LogManager.info("${skeletonOffset.name.padStart(15, ' ')}: ${formatter.format(skeletonOffsets[skeletonOffset])} (was ${formatter.format(initialSkeletonOffsets[skeletonOffset])})") + } + + LogManager.info("Solved skeleton offsets:") + printSkeletonOffset(SkeletonConfigOffsets.HEAD) + printSkeletonOffset(SkeletonConfigOffsets.NECK) + printSkeletonOffset(SkeletonConfigOffsets.UPPER_CHEST) + printSkeletonOffset(SkeletonConfigOffsets.CHEST) + printSkeletonOffset(SkeletonConfigOffsets.WAIST) + printSkeletonOffset(SkeletonConfigOffsets.HIP) + printSkeletonOffset(SkeletonConfigOffsets.HIPS_WIDTH) + printSkeletonOffset(SkeletonConfigOffsets.UPPER_LEG) + printSkeletonOffset(SkeletonConfigOffsets.LOWER_LEG) + + return Solution(skeletonOffsets) + } + + private fun makeSkeletonOffsets( + p: DoubleArray, + initialSkeletonOffsets: Map, + ): MutableMap { + val torsoLength = p[0] + val offsets = initialSkeletonOffsets.toMutableMap().apply { + this[SkeletonConfigOffsets.UPPER_CHEST] = (UPPER_CHEST_RATIO * torsoLength).toFloat() + this[SkeletonConfigOffsets.CHEST] = (CHEST_RATIO * torsoLength).toFloat() + this[SkeletonConfigOffsets.WAIST] = (WAIST_RATIO * torsoLength).toFloat() + this[SkeletonConfigOffsets.HIP] = (HIP_RATIO * torsoLength).toFloat() + this[SkeletonConfigOffsets.HIPS_WIDTH] = p[1].toFloat() + this[SkeletonConfigOffsets.UPPER_LEG] = p[2].toFloat() + this[SkeletonConfigOffsets.LOWER_LEG] = p[3].toFloat() + this[SkeletonConfigOffsets.NECK] = p[4].toFloat() + this[SkeletonConfigOffsets.HEAD] = p[5].toFloat() + } + return offsets + } + + private fun addResidual(residuals: DoubleArray, index: Int, camera: Camera, bone: Bone, joint: Vector2D?): Int { + if (joint == null) { + return index + } + + val estimated = camera.project(bone.getPosition().toDouble()) + if (estimated == null) { + return index + } + + residuals[index + 0] = joint.x - estimated.x + residuals[index + 1] = joint.y - estimated.y + + return index + 2 + } + + companion object { + private const val SKIP_FRAMES = 5 + private const val MIN_FRAMES = 100 + + private const val UPPER_CHEST_RATIO = 0.25 + private const val CHEST_RATIO = 0.25 + private const val WAIST_RATIO = 0.25 + private const val HIP_RATIO = 0.25 + + private const val ANKLE_TO_HEEL_LENGTH = 0.08 + + private const val NUM_JOINTS = 9 + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/SolveCamera.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/SolveCamera.kt new file mode 100644 index 0000000000..69ec3c7400 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/SolveCamera.kt @@ -0,0 +1,255 @@ +package dev.slimevr.tracking.videocalibration.steps + +import dev.slimevr.tracking.trackers.TrackerPosition +import dev.slimevr.tracking.videocalibration.data.Camera +import dev.slimevr.tracking.videocalibration.data.CameraExtrinsic +import dev.slimevr.tracking.videocalibration.data.CameraIntrinsic +import dev.slimevr.tracking.videocalibration.data.CocoWholeBodyKeypoint +import dev.slimevr.tracking.videocalibration.snapshots.HumanPoseSnapshot +import dev.slimevr.tracking.videocalibration.snapshots.TrackersSnapshot +import dev.slimevr.tracking.videocalibration.sources.SnapshotsDatabase +import dev.slimevr.tracking.videocalibration.util.DebugOutput +import dev.slimevr.tracking.videocalibration.util.numericalJacobian +import io.eiren.util.logging.LogManager +import io.github.axisangles.ktmath.QuaternionD +import io.github.axisangles.ktmath.Vector2D +import io.github.axisangles.ktmath.Vector3D +import org.apache.commons.math3.analysis.MultivariateVectorFunction +import org.apache.commons.math3.fitting.leastsquares.LeastSquaresBuilder +import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer +import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer +import org.apache.commons.math3.util.FastMath +import org.slf4j.ext.LoggerWrapper +import kotlin.math.atan2 +import kotlin.math.cos +import kotlin.math.sin +import kotlin.time.Duration +import kotlin.time.Duration.Companion.milliseconds + +class SolveCamera( + private val debugOutput: DebugOutput, +) { + + class Solution( + val camera: Camera, + val cameraDelay: Duration, + ) + + private val minDistanceInWorld = 0.05 + private val minMatches = 100 + + fun solve(database: SnapshotsDatabase): Solution? { + val zeroFrames = database.matchRecent(Duration.ZERO) + val zeroMatches = mutableListOf() + zeroMatches += extractPath(TrackerPosition.RIGHT_HAND, CocoWholeBodyKeypoint.RIGHT_WRIST, zeroFrames) + if (zeroMatches.size < minMatches) { + return null + } + + var centroid = Vector3D.NULL + for (match in zeroMatches) { + centroid += match.trackerOriginInWorld + } + centroid /= zeroMatches.size.toDouble() + LogManager.debug("Tracker centroid: $centroid") + + var bestRMS = Double.POSITIVE_INFINITY + var bestCameraDelay: Duration? = null + var bestCamera: Camera? = null + var bestWristInTracker: Vector3D? = null + + for (cameraDelayInMs in -300..300 step 20) { + val cameraDelay = cameraDelayInMs.milliseconds + val frames = database.matchRecent(cameraDelay) + if (frames.isEmpty()) { + continue + } + + val initialCamera = frames.first().second.camera + + val matches = mutableListOf() +// matches += extractPath(TrackerPosition.LEFT_HAND, CocoWholeBodyKeypoint.LEFT_WRIST, frames) + matches += extractPath(TrackerPosition.RIGHT_HAND, CocoWholeBodyKeypoint.RIGHT_WRIST, frames) + + // TODO: Check coverage across image + +// if (matches.size < minMatches) { +// continue +// } + +// LogManager.debug("Solving camera with ${matches.size} correspondences...") + for (angleDeg in 0..360 step 20) { + val (initialExtraYaw, initialCameraOriginInWorld) = placeCamera(initialCamera, FastMath.toRadians(angleDeg.toDouble()), 3.0) + + val (camera, wristInTracker, rms) = solveCamera(initialCamera, initialExtraYaw, initialCameraOriginInWorld, matches) ?: continue + +// LogManager.debug("Solved camera $angleDeg deg: rms=$rms camera=$camera") + + if (rms >= bestRMS) { + continue + } + + val projectedMatches = + matches.map { match -> + projectWrist( + camera, + match.trackerToWorld, + match.trackerOriginInWorld, + wristInTracker, + ) to match.joint + } + + debugOutput.saveHandToControllerMatches( + projectedMatches, + initialCamera.imageSize, + ) + + bestRMS = rms + bestCameraDelay = cameraDelay + bestCamera = camera + bestWristInTracker = wristInTracker + } + } + + if (bestCamera == null || bestCameraDelay == null) { + return null + } + + LogManager.info("Solved camera: $bestCamera") + LogManager.info(" delay=$bestCameraDelay") + LogManager.info(" wristInTracker=$bestWristInTracker") + + return Solution(bestCamera, bestCameraDelay) + } + + private fun projectWrist(camera: Camera, trackerRotation: QuaternionD, trackerOriginInWorld: Vector3D, wristInTracker: Vector3D): Vector2D? { + // TODO: Select the right constant + val wrist = trackerOriginInWorld + trackerRotation.sandwich(wristInTracker) + return camera.project(wrist) + } + + private data class Correspondence( + val trackerToWorld: QuaternionD, + val trackerOriginInWorld: Vector3D, + val joint: Vector2D, + ) + + private fun extractPath( + trackerPosition: TrackerPosition, + jointPosition: CocoWholeBodyKeypoint, + frames: List>, + ): List { + val matches = mutableListOf() + + for ((trackersFrame, humanFrame) in frames.reversed()) { + val tracker = trackersFrame.trackers[trackerPosition] + val joint = humanFrame.joints[jointPosition] + if (tracker == null || joint == null) { + continue + } + + val originInWorld = tracker.trackerOriginInWorld + if (originInWorld == null) { + continue + } + + val lastMatch = matches.lastOrNull() + if (lastMatch == null || (originInWorld - lastMatch.trackerOriginInWorld).len() > minDistanceInWorld) { + matches += Correspondence(tracker.rawTrackerToWorld, originInWorld, joint) + } + } + + return matches + } + + private fun placeCamera( + initialCamera: Camera, + angleRad: Double, + distanceFromOrigin: Double, + ): Pair { + val cameraOriginInWorld = Vector3D(cos(angleRad), 0.0, sin(angleRad)) * distanceFromOrigin + + val cameraZ = initialCamera.extrinsic.cameraToWorld.sandwichUnitZ() + val cameraYaw = atan2(cameraZ.z, cameraZ.x) + val targetYaw = atan2(-cameraOriginInWorld.z, -cameraOriginInWorld.x) + val extraYaw = -(targetYaw - cameraYaw) // In an RHS coordinate system, we have to rotate in the opposite direction + + return Pair(extraYaw, cameraOriginInWorld) + } + + private fun solveCamera( + initialCamera: Camera, + initialExtraYaw: Double, + initialCameraOriginInWorld: Vector3D, + matches: List, + ): Triple? { + val n = matches.size * 2 + + val costFn = MultivariateVectorFunction { params -> + val camera = buildCamera(params, initialCamera) + val wristInTracker = Vector3D(params[4], params[5], params[6]) + + val residuals = DoubleArray(n) { 0.0 } + for ((i, match) in matches.withIndex()) { + val projected = projectWrist(camera, match.trackerToWorld, match.trackerOriginInWorld, wristInTracker) + if (projected != null) { + val dx = projected.x - match.joint.x + val dy = projected.y - match.joint.y + residuals[i * 2 + 0] = dx + residuals[i * 2 + 1] = dy + } else { + residuals[i * 2 + 0] = 1.0e6 + residuals[i * 2 + 1] = 1.0e6 + } + } + + residuals + } + + val model = numericalJacobian(costFn) + + // TODO: Should initialize relative to HEAD + val initial = doubleArrayOf( + initialExtraYaw, + initialCameraOriginInWorld.x, + initialCameraOriginInWorld.y, + initialCameraOriginInWorld.z, + 0.0, // Tracker to wrist x + 0.0, // Tracker to wrist y + 0.0, // Tracker to wrist z + ) + + val problem = LeastSquaresBuilder() + .start(initial) + .model(model) + .target(DoubleArray(n) { 0.0 }) + .maxEvaluations(10000) + .maxIterations(10000) + .build() + + val optimizer = LevenbergMarquardtOptimizer() + + val result: LeastSquaresOptimizer.Optimum + try { + result = optimizer.optimize(problem) + } catch (e: Exception) { + return null + } + + val bestCamera = buildCamera(result.point.toArray(), initialCamera) + val wristInTracker = Vector3D(result.point.getEntry(4), result.point.getEntry(5), result.point.getEntry(6)) + + return Triple(bestCamera, wristInTracker, result.rms) + } + + private fun buildCamera(params: DoubleArray, initialCamera: Camera): Camera { + val cameraToWorld = QuaternionD.rotationAroundYAxis(params[0]) * initialCamera.extrinsic.cameraToWorld + val cameraOriginInWorld = Vector3D(params[1], params[2], params[3]) + + return Camera( + CameraExtrinsic.fromCameraPose(cameraToWorld, cameraOriginInWorld), + CameraIntrinsic(initialCamera.intrinsic.fx, initialCamera.intrinsic.fy, initialCamera.intrinsic.tx, initialCamera.intrinsic.ty), + initialCamera.imageSize, + ) + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/SolveNonUpperBodyTracker.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/SolveNonUpperBodyTracker.kt new file mode 100644 index 0000000000..9639f07e63 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/SolveNonUpperBodyTracker.kt @@ -0,0 +1,342 @@ +package dev.slimevr.tracking.videocalibration.steps + +import dev.slimevr.tracking.trackers.TrackerPosition +import dev.slimevr.tracking.videocalibration.data.Camera +import dev.slimevr.tracking.videocalibration.data.CocoWholeBodyKeypoint +import dev.slimevr.tracking.videocalibration.data.TrackerResetOverride +import dev.slimevr.tracking.videocalibration.snapshots.HumanPoseSnapshot +import dev.slimevr.tracking.videocalibration.snapshots.TrackersSnapshot +import dev.slimevr.tracking.videocalibration.sources.SnapshotsDatabase +import dev.slimevr.tracking.videocalibration.util.numericalJacobian +import io.eiren.util.logging.LogManager +import io.github.axisangles.ktmath.QuaternionD +import io.github.axisangles.ktmath.Vector2D +import io.github.axisangles.ktmath.Vector3D +import org.apache.commons.math3.analysis.MultivariateVectorFunction +import org.apache.commons.math3.fitting.leastsquares.LeastSquaresBuilder +import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer +import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer +import org.apache.commons.math3.util.FastMath +import kotlin.math.PI +import kotlin.math.abs +import kotlin.time.Duration +import kotlin.time.Duration.Companion.milliseconds + +class SolveNonUpperBodyTracker { + + data class Solution( + val trackerPosition: TrackerPosition, + val trackerReset: TrackerResetOverride, + val cameraDelay: Duration, + ) + + val trackerPositionToJoints = mapOf( + TrackerPosition.LEFT_UPPER_LEG to Pair(CocoWholeBodyKeypoint.LEFT_HIP, CocoWholeBodyKeypoint.LEFT_KNEE), + TrackerPosition.LEFT_LOWER_LEG to Pair(CocoWholeBodyKeypoint.LEFT_KNEE, CocoWholeBodyKeypoint.LEFT_ANKLE), + TrackerPosition.RIGHT_UPPER_LEG to Pair(CocoWholeBodyKeypoint.RIGHT_HIP, CocoWholeBodyKeypoint.RIGHT_KNEE), + TrackerPosition.RIGHT_LOWER_LEG to Pair(CocoWholeBodyKeypoint.RIGHT_KNEE, CocoWholeBodyKeypoint.RIGHT_ANKLE), + TrackerPosition.LEFT_UPPER_ARM to Pair(CocoWholeBodyKeypoint.LEFT_SHOULDER, CocoWholeBodyKeypoint.LEFT_ELBOW), + TrackerPosition.RIGHT_UPPER_ARM to Pair(CocoWholeBodyKeypoint.RIGHT_SHOULDER, CocoWholeBodyKeypoint.RIGHT_ELBOW), + ) + + private val minMatches = 120 + + fun solve( + trackerPosition: TrackerPosition, + camera: Camera, + forwardPose: CaptureForwardPose.Solution, + snapshotsDatabase: SnapshotsDatabase, + cameraDelay: Duration, + ): Solution? { + val frames = snapshotsDatabase.matchRecent(cameraDelay) + if (frames.isEmpty()) { + return null + } + + val joints = trackerPositionToJoints[trackerPosition] + if (joints == null) { + return null + } + + val matches = buildMatches(trackerPosition, frames, joints.first, joints.second) + + val filteredMatches = filterMatches(matches) + if (filteredMatches.size < minMatches) { + return null + } + + if (!enoughRotation(filteredMatches)) { + return null + } + + LogManager.info("Trying to solve $trackerPosition...") + + var bestRMS = Double.POSITIVE_INFINITY + var bestTotalDelay: Duration? = null + var bestTrackerReset: TrackerResetOverride? = null + + for (extraDelay in -500..500 step 10) { + val totalDelay = cameraDelay + extraDelay.milliseconds + val shiftedFrames = snapshotsDatabase.matchRecent(totalDelay) + val shiftedMatches = buildMatches(trackerPosition, shiftedFrames, joints.first, joints.second) + val filteredShiftedMatches = filterMatches(shiftedMatches) + if (filteredShiftedMatches.size < minMatches) { + return null + } + + var bestInitialRMS = Double.POSITIVE_INFINITY + var bestInitialParams: DoubleArray? = null + for (initialParams in startingParams()) { + val rms = calcError( + initialParams, + trackerPosition, + filteredShiftedMatches, + camera, + forwardPose, + ) + if (rms < bestInitialRMS) { + bestInitialRMS = rms + bestInitialParams = initialParams + } + } + + if (bestInitialParams == null) { + LogManager.warning("Failed to find best initial params") + return null + } + + val result = solveLM( + trackerPosition, + filteredShiftedMatches, + camera, + forwardPose, + bestInitialParams, + ) + if (result == null) { + LogManager.warning("Failed to optimize tracker") + return null + } + + val (trackerReset, rms) = result + if (rms < bestRMS) { + bestRMS = rms + bestTotalDelay = totalDelay + bestTrackerReset = trackerReset + } + } + + if (bestTotalDelay == null || bestTrackerReset == null) { + return null + } + + LogManager.info("Found tracker resets for $trackerPosition: $bestTrackerReset delay=$bestTotalDelay") + + return Solution(trackerPosition, bestTrackerReset, bestTotalDelay) + } + + class Match( + val trackerRotation: QuaternionD, + val lowerJoint: Vector2D, + val upperJoint: Vector2D, + val boneDir: Vector2D, + ) + + private fun buildMatches( + trackerPosition: TrackerPosition, + frames: List>, + upperJoint: CocoWholeBodyKeypoint, + lowerJoint: CocoWholeBodyKeypoint, + ): List { + val matches = mutableListOf() + for ((trackersSnapshot, humanPoseSnapshot) in frames) { + val tracker = trackersSnapshot.trackers[trackerPosition] ?: continue + + val r = tracker.rawTrackerToWorld + + // Tracker Y-axis points from lower joint to upper joint + val upperJoint = humanPoseSnapshot.joints[upperJoint] ?: continue + val lowerJoint = humanPoseSnapshot.joints[lowerJoint] ?: continue + val boneDir = (upperJoint - lowerJoint).unit() + + matches += Match(r, lowerJoint, upperJoint, boneDir) + } + + return matches + } + + private val minBoneLength = 40.0 + private val minAngleDeviation = FastMath.toRadians(5.0) + + private fun filterMatches(matches: List): List { + val filtered = mutableListOf() + for (match in matches.asReversed()) { + if ((match.upperJoint - match.lowerJoint).len() < minBoneLength) { + continue + } + + val lastMatch = filtered.lastOrNull() + if ( + lastMatch == null || + lastMatch.trackerRotation.angleToR(match.trackerRotation) >= minAngleDeviation + ) { + filtered += match + } + } + + return filtered + } + + private val minMaxRotation = FastMath.toRadians(60.0) + + private fun enoughRotation(matches: List): Boolean { + for (i in matches) { + for (j in matches) { + // TODO: Should look at non-yaw rotation + if (i.trackerRotation.angleToR(j.trackerRotation) >= minMaxRotation) { + return true + } + } + } + return false + } + + private fun solveLM( + trackerPosition: TrackerPosition, + matches: List, + camera: Camera, + forwardPose: CaptureForwardPose.Solution, + initialParams: DoubleArray, + ): Pair? { + val n = forwardPose.trackerRotations.size + matches.size + + val costFn = MultivariateVectorFunction { p -> + val reset = buildTrackerReset(p) + + val residual = DoubleArray(n) { 0.0 } + + var i = 0 + + // Ensure that the bone is aligned to the reference direction + for (frame in forwardPose.trackerRotations) { + val trackerRotation = frame[trackerPosition] + if (trackerRotation != null) { + val trackerBone = reset.toBoneRotation(trackerRotation) +// residual[i++] = trackerBone.angleToR(forwardPose.reference) + val z = trackerBone.sandwichUnitZ() + val z2 = Vector3D(z.x, 0.0, z.z).unit() + residual[i++] = z2.angleTo(forwardPose.reference.sandwichUnitZ()) + } + } + + // Ensure that the after-reset tracker Y-axis is aligned to the bone + for (match in matches) { + val trackerBone = reset.toBoneRotation(match.trackerRotation) + val projectedBoneDir = + camera.project( + trackerBone.sandwichUnitY(), + (match.lowerJoint + match.upperJoint) * 0.5, + 1.0, + ) + if (projectedBoneDir != null) { + residual[i++] = projectedBoneDir.angleTo(match.boneDir) + } else { + residual[i++] = 1.0e6 + } + } + + residual + } + + val model = numericalJacobian(costFn) + + val problem = LeastSquaresBuilder() + .start(initialParams) + .model(model) + .target(DoubleArray(n) { 0.0 }) + .maxEvaluations(10000) + .maxIterations(10000) + .checker { iter, prev, current -> +// LogManager.info("$iter ${prev.rms} ${current.rms}") + abs(current.rms - prev.rms) < 1e-7 + } + .build() + + val optimizer = LevenbergMarquardtOptimizer() + + val result: LeastSquaresOptimizer.Optimum + try { + result = optimizer.optimize(problem) + } catch (e: Exception) { + LogManager.warning("Failed to optimize tracker: $e") + return null + } + + val (preRot, postRot) = buildTrackerReset(result.point.toArray()) + + val trackerReset = TrackerResetOverride(preRot, postRot) + + return Pair(trackerReset, result.rms) + } + + private fun buildTrackerReset(p: DoubleArray): TrackerResetOverride { + val override = TrackerResetOverride(p[0], QuaternionD(p[1], p[2], p[3], p[4]).unit()) + return override + } + + private fun startingParams() = iterator { + val count = 4 + for (y1 in 0 until count) { + val globalYaw = 2.0 * PI / count * y1 + for (y2 in 0 until count) { + val localYaw = 2.0 * PI / count * y2 + for (z in 0 until count) { + val localRoll = 2.0 * PI / count * z + val localRotation = QuaternionD.rotationAroundYAxis(localYaw) * QuaternionD.rotationAroundZAxis(localRoll) + yield(doubleArrayOf(globalYaw, localRotation.w, localRotation.x, localRotation.y, localRotation.z)) + } + } + } + } + + fun calcError( + params: DoubleArray, + trackerPosition: TrackerPosition, + matches: List, + camera: Camera, + forwardPose: CaptureForwardPose.Solution, + ): Double { + val reset = buildTrackerReset(params) + + var rms = 0.0 + + // Ensure that the bone is aligned to the reference direction + for (frame in forwardPose.trackerRotations) { + val trackerRotation = frame[trackerPosition] + if (trackerRotation != null) { + val trackerBone = reset.toBoneRotation(trackerRotation) + val error = trackerBone.angleToR(forwardPose.reference) + rms += error * error + } + } + + // Ensure that the after-reset tracker Y-axis is aligned to the bone + for (match in matches) { + val trackerBone = reset.toBoneRotation(match.trackerRotation) + val projectedBoneDir = + camera.project( + trackerBone.sandwichUnitY(), + (match.lowerJoint + match.upperJoint) * 0.5, + 1.0, + ) + if (projectedBoneDir != null) { + val error = projectedBoneDir.angleTo(match.boneDir) + rms += error * error + } else { + val error = 1.0e6 + rms += error * error + } + } + + return rms + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/SolveUpperBodyTracker.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/SolveUpperBodyTracker.kt new file mode 100644 index 0000000000..68a1612914 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/SolveUpperBodyTracker.kt @@ -0,0 +1,159 @@ +package dev.slimevr.tracking.videocalibration.steps + +import dev.slimevr.tracking.trackers.TrackerPosition +import dev.slimevr.tracking.videocalibration.data.TrackerResetOverride +import dev.slimevr.tracking.videocalibration.util.numericalJacobian +import io.eiren.util.logging.LogManager +import io.github.axisangles.ktmath.QuaternionD +import org.apache.commons.math3.analysis.MultivariateVectorFunction +import org.apache.commons.math3.fitting.leastsquares.LeastSquaresBuilder +import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer +import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer +import org.apache.commons.math3.util.FastMath +import kotlin.math.PI +import kotlin.math.abs +import kotlin.math.max + +class SolveUpperBodyTracker { + + fun solve( + trackerPosition: TrackerPosition, + forwardPose: CaptureForwardPose.Solution, + bentOverPose: CaptureBentOverPose.Solution, + ): TrackerResetOverride? { + val n = forwardPose.trackerRotations.size + bentOverPose.trackerRotations.size * 2 + + val costFn = MultivariateVectorFunction { p -> + val reset = buildTrackerReset(p) + + val residual = DoubleArray(n) { 0.0 } + + var i = 0 + for (frame in forwardPose.trackerRotations) { + val trackerRotation = frame[trackerPosition] + if (trackerRotation != null) { + val trackerBone = reset.toBoneRotation(trackerRotation) + residual[i++] = trackerBone.angleToR(forwardPose.reference) + } + } + for (frame in bentOverPose.trackerRotations) { + val trackerRotation = frame[trackerPosition] + if (trackerRotation != null) { + val trackerBone = reset.toBoneRotation(trackerRotation) + residual[i++] = trackerBone.sandwichUnitX().angleTo(forwardPose.reference.sandwichUnitX()) + + // y-axis should be facing forward, instead of backwards + // Rotate forwardPose forward instead of using bentOverPose because the standing forwardPose is usually more balanced + val bentOverYAxis = (forwardPose.reference * QuaternionD.rotationAroundXAxis(-PI / 4.0)).sandwichUnitY() + val bentOverAngle = trackerBone.sandwichUnitY().angleTo(bentOverYAxis) + residual[i++] = max(bentOverAngle - FastMath.toRadians(30.0), 0.0) + } + } + + residual + } + + val model = numericalJacobian(costFn) + + var bestRMS = Double.POSITIVE_INFINITY + var bestInitialParams: DoubleArray? = null + for (initialParams in startingParams()) { + val rms = calcError(initialParams, trackerPosition, forwardPose, bentOverPose) + if (rms < bestRMS) { + bestRMS = rms + bestInitialParams = initialParams + } + } + + if (bestInitialParams == null) { + LogManager.warning("Failed to find best initial params") + return null + } + + LogManager.info("Best initial params: ${bestInitialParams.toList()} rms=$bestRMS") + + val problem = LeastSquaresBuilder() + .start(bestInitialParams) + .model(model) + .target(DoubleArray(n) { 0.0 }) + .maxEvaluations(10000) + .maxIterations(10000) + .checker { iter, prev, current -> +// LogManager.info("$iter ${prev.rms} ${current.rms}") + abs(current.rms - prev.rms) < 1e-7 + } + .build() + + val optimizer = LevenbergMarquardtOptimizer() + + val result: LeastSquaresOptimizer.Optimum + try { + result = optimizer.optimize(problem) + } catch (e: Exception) { + LogManager.warning("Failed to optimize tracker: $e") + return null + } + + val trackerReset = buildTrackerReset(result.point.toArray()) + + LogManager.info("Found tracker resets for $trackerPosition: $trackerReset") + + return trackerReset + } + + private fun buildTrackerReset(p: DoubleArray): TrackerResetOverride { + val override = TrackerResetOverride(p[0], QuaternionD(p[1], p[2], p[3], p[4]).unit()) + return override + } + + private fun startingParams() = iterator { + val count = 4 + for (y1 in 0 until count) { + val globalYaw = 2.0 * PI / count * y1 + for (y2 in 0 until count) { + val localYaw = 2.0 * PI / count * y2 + for (z in 0 until count) { + val localRoll = 2.0 * PI / count * z + val localRotation = QuaternionD.rotationAroundYAxis(localYaw) * QuaternionD.rotationAroundZAxis(localRoll) + yield(doubleArrayOf(globalYaw, localRotation.w, localRotation.x, localRotation.y, localRotation.z)) + } + } + } + } + + private fun calcError( + params: DoubleArray, + trackerPosition: TrackerPosition, + forwardPose: CaptureForwardPose.Solution, + bentOverPose: CaptureBentOverPose.Solution, + ): Double { + val reset = buildTrackerReset(params) + + var rms = 0.0 + for (frame in forwardPose.trackerRotations) { + val trackerRotation = frame[trackerPosition] + if (trackerRotation != null) { + val trackerBone = reset.toBoneRotation(trackerRotation) + val error = trackerBone.angleToR(forwardPose.reference) + rms += error * error + } + } + for (frame in bentOverPose.trackerRotations) { + val trackerRotation = frame[trackerPosition] + if (trackerRotation != null) { + val trackerBone = reset.toBoneRotation(trackerRotation) + val error1 = trackerBone.sandwichUnitX().angleTo(forwardPose.reference.sandwichUnitX()) + + // y-axis should be facing forward, instead of backwards + // Rotate forwardPose forward instead of using bentOverPose because the standing forwardPose is usually more balanced + val bentOverYAxis = (forwardPose.reference * QuaternionD.rotationAroundXAxis(-PI / 4.0)).sandwichUnitY() + val bentOverAngle = trackerBone.sandwichUnitY().angleTo(bentOverYAxis) + val error2 = max(bentOverAngle - FastMath.toRadians(30.0), 0.0) + + rms += error1 * error1 + error2 * error2 + } + } + + return rms + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/Step.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/Step.kt new file mode 100644 index 0000000000..453e382099 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/Step.kt @@ -0,0 +1,12 @@ +package dev.slimevr.tracking.videocalibration.steps + +enum class Step { + NOT_STARTED, + SOLVING_CAMERA, + CAPTURING_FORWARD_POSE, + CAPTURING_BENT_OVER_POSE, + SOLVING_UPPER_BODY, + SOLVING_NON_UPPER_BODY, + SOLVING_SKELETON_OFFSETS, + DONE, +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/VideoCalibrator.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/VideoCalibrator.kt new file mode 100644 index 0000000000..e048d9d14b --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/steps/VideoCalibrator.kt @@ -0,0 +1,601 @@ +package dev.slimevr.tracking.videocalibration.steps + +import com.google.flatbuffers.FlatBufferBuilder +import dev.slimevr.protocol.GenericConnection +import dev.slimevr.tracking.processor.config.SkeletonConfigManager +import dev.slimevr.tracking.trackers.Tracker +import dev.slimevr.tracking.trackers.TrackerPosition +import dev.slimevr.tracking.videocalibration.data.Camera +import dev.slimevr.tracking.videocalibration.data.TrackerResetOverride +import dev.slimevr.tracking.videocalibration.snapshots.HumanPoseSnapshot +import dev.slimevr.tracking.videocalibration.snapshots.TrackerSnapshot +import dev.slimevr.tracking.videocalibration.sources.SnapshotsDatabase +import dev.slimevr.tracking.videocalibration.util.DebugOutput +import io.eiren.util.logging.LogManager +import kotlinx.coroutines.CoroutineScope +import kotlinx.coroutines.Dispatchers +import kotlinx.coroutines.async +import kotlinx.coroutines.awaitAll +import kotlinx.coroutines.cancel +import kotlinx.coroutines.coroutineScope +import kotlinx.coroutines.delay +import kotlinx.coroutines.ensureActive +import kotlinx.coroutines.launch +import solarxr_protocol.MessageBundle +import solarxr_protocol.datatypes.TransactionId +import solarxr_protocol.datatypes.math.Quat +import solarxr_protocol.datatypes.math.Vec3f +import solarxr_protocol.rpc.RpcMessage +import solarxr_protocol.rpc.RpcMessageHeader +import solarxr_protocol.rpc.VideoTrackerCalibrationCamera +import solarxr_protocol.rpc.VideoTrackerCalibrationProgressResponse +import solarxr_protocol.rpc.VideoTrackerCalibrationStatus +import java.util.concurrent.atomic.AtomicReference +import kotlin.collections.iterator +import kotlin.coroutines.coroutineContext +import kotlin.time.Duration +import kotlin.time.Duration.Companion.seconds + +/** + * State machine for video calibration. + */ +class VideoCalibrator( + private val trackersToReset: Map, + private val skeletonConfigManager: SkeletonConfigManager, + private val snapshotsDatabase: SnapshotsDatabase, + private val websocket: GenericConnection, + private val debugOutput: DebugOutput, +) { + val step = AtomicReference(Step.NOT_STARTED) + + private val scope = CoroutineScope(Dispatchers.Default) + + private val solveCamera = SolveCamera(debugOutput) + private val captureForwardPose = CaptureForwardPose() + private val captureBentOverPose = CaptureBentOverPose() + private val solveUpperBodyTracker = SolveUpperBodyTracker() + private val solveNonUpperBodyTracker = SolveNonUpperBodyTracker() + private val skeletonOffsetsSolver = SkeletonOffsetsSolver(debugOutput) + + private var camera: SolveCamera.Solution? = null + private var forwardPose: CaptureForwardPose.Solution? = null + private var bentOverPose: CaptureBentOverPose.Solution? = null + private val trackerResets = mutableMapOf() + private val trackerToHumanPoseDelays = mutableMapOf() + + init { +// val missingTrackersToReset = REQUIRED_TRACKERS_TO_RESET.subtract(trackersToReset.keys) +// require(missingTrackersToReset.isEmpty()) { "Missing trackers to reset: $missingTrackersToReset" } +// +// val unsupportedTrackersToReset = trackersToReset.keys.subtract(SUPPORTED_TRACKERS_TO_RESET) +// require(unsupportedTrackersToReset.isEmpty()) { "Unsupported trackers to reset: $unsupportedTrackersToReset" } + } + + /** + * Starts video calibration. + */ + fun start() { + LogManager.info("Resetting trackers: ${trackersToReset.keys}") + + scope.launch { + try { + run() + } catch (e: Exception) { + LogManager.warning("Video calibration failed", e) + step.set(Step.DONE) + } + } + } + + /** + * Stops video calibration. + */ + fun requestStop() { + scope.cancel() + } + + private suspend fun run() { + while (true) { + coroutineContext.ensureActive() + + when (step.get()) { + Step.NOT_STARTED -> begin() + Step.SOLVING_CAMERA -> solveCamera() + Step.CAPTURING_FORWARD_POSE -> captureForwardPose() + Step.CAPTURING_BENT_OVER_POSE -> captureBentOverPose() + Step.SOLVING_UPPER_BODY -> solveUpperBody() + Step.SOLVING_NON_UPPER_BODY -> solveNonUpperBodyTrackerResets() + Step.SOLVING_SKELETON_OFFSETS -> solveSkeletonOffsets() + Step.DONE -> break + } + + delay(DELAY_BETWEEN_ATTEMPTS) + } + } + + private fun begin() { + step.set(Step.SOLVING_CAMERA) + + val fbb = FlatBufferBuilder(512) + val progressOffset = + VideoTrackerCalibrationProgressResponse.createVideoTrackerCalibrationProgressResponse( + fbb, + VideoTrackerCalibrationStatus.CALIBRATE_CAMERA, + 0, + 0, + 0, + 0, + ) + val messageOffset = createRPCMessage(fbb, RpcMessage.VideoTrackerCalibrationProgressResponse, progressOffset) + fbb.finish(messageOffset) + websocket.send(fbb.dataBuffer()) + } + + private suspend fun solveCamera() { + LogManager.info("Solving camera...") + + snapshotsDatabase.update() + + val camera = solveCamera.solve(snapshotsDatabase) + if (camera == null) { + return + } + + this.camera = camera + + trackerToHumanPoseDelays[TrackerPosition.HEAD] = camera.cameraDelay + trackerToHumanPoseDelays[TrackerPosition.LEFT_HAND] = camera.cameraDelay + trackerToHumanPoseDelays[TrackerPosition.RIGHT_HAND] = camera.cameraDelay + + val fbb = FlatBufferBuilder(512) + val cameraOffset = addCamera(fbb, camera.camera) + val progressOffset = + VideoTrackerCalibrationProgressResponse.createVideoTrackerCalibrationProgressResponse( + fbb, + VideoTrackerCalibrationStatus.CAPTURE_FORWARD_POSE, + cameraOffset, + 0, + 0, + 0, + ) + val messageOffset = createRPCMessage(fbb, RpcMessage.VideoTrackerCalibrationProgressResponse, progressOffset) + fbb.finish(messageOffset) + websocket.send(fbb.dataBuffer()) + + snapshotsDatabase.clearRecent() + + step.set(Step.CAPTURING_FORWARD_POSE) + + delay(2.seconds) + } + + private fun captureForwardPose() { + LogManager.info("Capturing forward pose...") + + val camera = camera ?: error("Missing camera") + + snapshotsDatabase.update() + + forwardPose = captureForwardPose.capture(snapshotsDatabase.recentTrackersSnapshots) + if (forwardPose == null) { + return + } + + val fbb = FlatBufferBuilder(512) + val cameraOffset = addCamera(fbb, camera.camera) + val progressOffset = + VideoTrackerCalibrationProgressResponse.createVideoTrackerCalibrationProgressResponse( + fbb, + VideoTrackerCalibrationStatus.CAPTURE_BENT_OVER_POSE, + cameraOffset, + 0, + 0, + 0, + ) + val messageOffset = createRPCMessage(fbb, RpcMessage.VideoTrackerCalibrationProgressResponse, progressOffset) + fbb.finish(messageOffset) + websocket.send(fbb.dataBuffer()) + + snapshotsDatabase.clearRecent() + + step.set(Step.CAPTURING_BENT_OVER_POSE) + } + + private fun captureBentOverPose() { + LogManager.info("Capturing bent-over pose...") + + val camera = camera ?: error("Missing camera") + val forwardPoseSolution = forwardPose ?: error("Missing forward pose") + + snapshotsDatabase.update() + + bentOverPose = captureBentOverPose.capture(snapshotsDatabase.recentTrackersSnapshots, forwardPoseSolution) + if (bentOverPose == null) { + return + } + + val fbb = FlatBufferBuilder(512) + val cameraOffset = addCamera(fbb, camera.camera) + val trackersDoneOffset = addTrackersReset(fbb) + val trackersToResetOffset = addRemainingTrackersToReset(fbb) + val progressOffset = + VideoTrackerCalibrationProgressResponse.createVideoTrackerCalibrationProgressResponse( + fbb, + VideoTrackerCalibrationStatus.CALIBRATE_TRACKERS, + cameraOffset, + trackersDoneOffset, + trackersToResetOffset, + 0, + ) + val messageOffset = createRPCMessage(fbb, RpcMessage.VideoTrackerCalibrationProgressResponse, progressOffset) + fbb.finish(messageOffset) + websocket.send(fbb.dataBuffer()) + + snapshotsDatabase.clearRecent() + + step.set(Step.SOLVING_UPPER_BODY) + } + + private fun solveUpperBody() { + LogManager.info("Solving upper body trackers resets...") + + val camera = camera ?: error("Missing camera") + val forwardPose = forwardPose ?: error("Missing forward pose") + val bentOverPose = bentOverPose ?: error("Missing bent over pose") + + for (trackerPosition in UPPER_BODY_TRACKERS_TO_RESET) { + val tracker = trackersToReset[trackerPosition] ?: continue + + val trackerReset = + solveUpperBodyTracker.solve( + trackerPosition, + forwardPose, + bentOverPose, + ) + + if (trackerReset == null) { + error("Failed to solve upper body tracker $trackerPosition") + } + + trackerResets[trackerPosition] = trackerReset + tracker.resetsHandler.trackerResetOverride = trackerReset + tracker.resetsHandler.postProcessResetFull(forwardPose.reference.toFloat()) + + // TODO: This camera delay should be computed from tracker itself, and not from the camera step + trackerToHumanPoseDelays[trackerPosition] = camera.cameraDelay + + val fbb = FlatBufferBuilder(512) + val cameraOffset = addCamera(fbb, camera.camera) + val trackersDoneOffset = addTrackersReset(fbb) + val trackersToResetOffset = addRemainingTrackersToReset(fbb) + val progressOffset = + VideoTrackerCalibrationProgressResponse.createVideoTrackerCalibrationProgressResponse( + fbb, + VideoTrackerCalibrationStatus.CALIBRATE_TRACKERS, + cameraOffset, + trackersDoneOffset, + trackersToResetOffset, + 0, + ) + val messageOffset = createRPCMessage(fbb, RpcMessage.VideoTrackerCalibrationProgressResponse, progressOffset) + fbb.finish(messageOffset) + websocket.send(fbb.dataBuffer()) + } + + // No need to clear database because we are collecting movements for the + // remaining trackers while solving the upper body tracker resets. + + step.set(Step.SOLVING_NON_UPPER_BODY) + } + + private suspend fun solveNonUpperBodyTrackerResets() { + LogManager.info("Solving remaining trackers resets...") + + val camera = camera ?: error("Missing camera") + val forwardPose = forwardPose ?: error("Missing forward pose") + + snapshotsDatabase.update() + + val attemptTrackersToReset = trackersToReset.filter { !trackerResets.contains(it.key) } + val solvedTrackerResets = coroutineScope { + val jobs = attemptTrackersToReset.map { (trackerPosition, tracker) -> + async(Dispatchers.Default) { + trackerPosition to solveNonUpperBodyTracker.solve( + trackerPosition, + camera.camera, + forwardPose, + snapshotsDatabase, + camera.cameraDelay, + ) + } + } + + jobs.awaitAll() + } + + for ((trackerPosition, solution) in solvedTrackerResets) { + if (solution != null) { + val tracker = trackersToReset[trackerPosition] ?: error("Missing tracker") + trackerResets[trackerPosition] = solution.trackerReset + tracker.resetsHandler.trackerResetOverride = solution.trackerReset + tracker.resetsHandler.postProcessResetFull(forwardPose.reference.toFloat()) + + trackerToHumanPoseDelays[trackerPosition] = solution.cameraDelay + } + } + +// for ((trackerPosition, tracker) in trackersToReset) { +// if (trackerResets.containsKey(trackerPosition)) { +// continue +// } +// +// snapshotsDatabase.update() +// val frames = snapshotsDatabase.matchRecent(camera.cameraDelay) +// +// val trackerReset = solveNonUpperBodyTracker.solve( +// trackerPosition, +// frames, +// camera.camera, +// forwardPose, +// ) +// +// if (trackerReset == null) { +// continue +// } +// +// trackerResets[trackerPosition] = trackerReset +// tracker.resetsHandler.trackerResetOverride = trackerReset +// tracker.resetsHandler.postProcessResetFull(forwardPose.reference.toFloat()) +// +// val fbb = FlatBufferBuilder(512) +// val cameraOffset = addCamera(fbb, camera.camera) +// val trackersDoneOffset = addTrackersReset(fbb) +// val trackersToResetOffset = addRemainingTrackersToReset(fbb) +// val progressOffset = +// VideoTrackerCalibrationProgressResponse.createVideoTrackerCalibrationProgressResponse( +// fbb, +// VideoTrackerCalibrationStatus.CALIBRATE_TRACKERS, +// cameraOffset, +// trackersDoneOffset, +// trackersToResetOffset, +// 0, +// ) +// val messageOffset = createRPCMessage(fbb, RpcMessage.VideoTrackerCalibrationProgressResponse, progressOffset) +// fbb.finish(messageOffset) +// websocket.send(fbb.dataBuffer()) +// } + + val remainingTrackersToReset = trackersToReset.keys.subtract(trackerResets.keys) + if (remainingTrackersToReset.isNotEmpty()) { + LogManager.debug("Still need to reset $remainingTrackersToReset") + return + } + + LogManager.info("Tracker resets:") + for ((trackerPosition, trackerReset) in trackerResets) { + LogManager.info("$trackerPosition: $trackerReset") + } + + val fbb = FlatBufferBuilder(512) + val cameraOffset = addCamera(fbb, camera.camera) + val trackersDoneOffset = addTrackersReset(fbb) + val trackersToResetOffset = addRemainingTrackersToReset(fbb) + val progressOffset = + VideoTrackerCalibrationProgressResponse.createVideoTrackerCalibrationProgressResponse( + fbb, + VideoTrackerCalibrationStatus.CALIBRATE_SKELETON_OFFSETS, + cameraOffset, + trackersDoneOffset, + trackersToResetOffset, + 0, + ) + val messageOffset = createRPCMessage(fbb, RpcMessage.VideoTrackerCalibrationProgressResponse, progressOffset) + fbb.finish(messageOffset) + websocket.send(fbb.dataBuffer()) + + val alignedFrames = alignFrames(snapshotsDatabase, trackerResets, trackerToHumanPoseDelays) + + // TODO: Move to its own step? + debugOutput.saveReconstruction( + camera.camera, + alignedFrames, + trackerResets, + ) + + snapshotsDatabase.clearRecent() + + step.set(Step.SOLVING_SKELETON_OFFSETS) + } + + private suspend fun solveSkeletonOffsets() { + LogManager.info("Solving skeleton offsets...") + + val camera = camera ?: error("Missing camera") + + snapshotsDatabase.update() + val frames = snapshotsDatabase.matchAll(camera.cameraDelay) + + val initialSkeletonOffsets = skeletonConfigManager.configOffsets.toMap() + val solution = + skeletonOffsetsSolver.solve( + frames, + camera.camera, + initialSkeletonOffsets, + trackerResets, + ) + + if (solution == null) { + return + } + + val alignedFrames = alignFrames(snapshotsDatabase, trackerResets, trackerToHumanPoseDelays) + + debugOutput.saveSkeletonOffsets( + camera.camera, + alignedFrames, + initialSkeletonOffsets, + solution.skeletonOffsets, + ) + + skeletonConfigManager.setOffsets(solution.skeletonOffsets) + + // Let the user see the result + // TODO: Should continue sending webcam as long as connected + delay(10.seconds) + + val fbb = FlatBufferBuilder(512) + val cameraOffset = addCamera(fbb, camera.camera) + val trackersDoneOffset = addTrackersReset(fbb) + val trackersToResetOffset = addRemainingTrackersToReset(fbb) + val progressOffset = + VideoTrackerCalibrationProgressResponse.createVideoTrackerCalibrationProgressResponse( + fbb, + VideoTrackerCalibrationStatus.DONE, + cameraOffset, + trackersDoneOffset, + trackersToResetOffset, + 0, + ) + val messageOffset = createRPCMessage(fbb, RpcMessage.VideoTrackerCalibrationProgressResponse, progressOffset) + fbb.finish(messageOffset) + websocket.send(fbb.dataBuffer()) + + step.set(Step.DONE) + } + + private fun createRPCMessage(fbb: FlatBufferBuilder, messageType: Byte, messageOffset: Int, respondTo: RpcMessageHeader? = null): Int { + val data = IntArray(1) + + RpcMessageHeader.startRpcMessageHeader(fbb) + RpcMessageHeader.addMessage(fbb, messageOffset) + RpcMessageHeader.addMessageType(fbb, messageType) + respondTo?.txId()?.let { txId -> + RpcMessageHeader.addTxId(fbb, TransactionId.createTransactionId(fbb, txId.id())) + } + data[0] = RpcMessageHeader.endRpcMessageHeader(fbb) + + val messages = MessageBundle.createRpcMsgsVector(fbb, data) + + MessageBundle.startMessageBundle(fbb) + MessageBundle.addRpcMsgs(fbb, messages) + return MessageBundle.endMessageBundle(fbb) + } + + private fun addCamera(fbb: FlatBufferBuilder, camera: Camera): Int { + val (extrinsic, intrinsic, imageSize) = camera + + VideoTrackerCalibrationCamera.startVideoTrackerCalibrationCamera(fbb) + + val worldToCameraOffset = extrinsic.worldToCamera.toFloat().let { Quat.createQuat(fbb, it.x, it.y, it.z, it.w) } + VideoTrackerCalibrationCamera.addWorldToCamera(fbb, worldToCameraOffset) + + val worldOriginInCamera = extrinsic.worldOriginInCamera.toFloat().let { Vec3f.createVec3f(fbb, it.x, it.y, it.z) } + VideoTrackerCalibrationCamera.addWorldOriginInCamera(fbb, worldOriginInCamera) + + VideoTrackerCalibrationCamera.addFx(fbb, intrinsic.fx.toFloat()) + VideoTrackerCalibrationCamera.addFy(fbb, intrinsic.fy.toFloat()) + VideoTrackerCalibrationCamera.addTx(fbb, intrinsic.tx.toFloat()) + VideoTrackerCalibrationCamera.addTy(fbb, intrinsic.ty.toFloat()) + + VideoTrackerCalibrationCamera.addWidth(fbb, imageSize.width) + VideoTrackerCalibrationCamera.addHeight(fbb, imageSize.height) + + return VideoTrackerCalibrationCamera.endVideoTrackerCalibrationCamera(fbb) + } + + private fun addTrackersReset(fbb: FlatBufferBuilder): Int = VideoTrackerCalibrationProgressResponse.createTrackersPendingVector( + fbb, + trackerResets.keys.map { it.bodyPart.toByte() }.toByteArray(), + ) + + private fun addRemainingTrackersToReset(fbb: FlatBufferBuilder): Int { + val remaining = trackersToReset.keys.subtract(trackerResets.keys) + return VideoTrackerCalibrationProgressResponse.createTrackersPendingVector( + fbb, + remaining.map { it.bodyPart.toByte() }.toByteArray(), + ) + } + + private fun alignFrames( + snapshotsDatabase: SnapshotsDatabase, + trackerResets: Map, + trackerToHumanPoseDelays: Map, + ): List>> { + val trackerFrames: Map> = + trackerToHumanPoseDelays.map { (trackerPosition, delay) -> + val frames = snapshotsDatabase.matchAll(delay) + trackerPosition to + frames.mapNotNull { + val trackerSnapshot = it.first.trackers[trackerPosition] + if (trackerSnapshot != null) { + it.second to trackerSnapshot + } else { + null + } + }.toMap() + }.toMap() + + val humanPoseToTrackers: List>> = + snapshotsDatabase.allHumanPoseSnapshots.map { humanPoseSnapshot -> + humanPoseSnapshot to + trackerToHumanPoseDelays.keys.mapNotNull { trackerPosition -> + val hp = trackerFrames[trackerPosition] + if (hp != null) { + val ts = hp[humanPoseSnapshot] + if (ts != null) { + val tr = trackerResets[trackerPosition] + trackerPosition to + if (tr != null) { + TrackerSnapshot( + ts.rawTrackerToWorld, + tr.toBoneRotation(ts.rawTrackerToWorld), + ts.trackerOriginInWorld, + ) + } else { + ts + } + } else { + null + } + } else { + null + } + }.toMap() + } + + return humanPoseToTrackers + } + + companion object { + + private val REQUIRED_TRACKERS_TO_RESET = listOf( + TrackerPosition.CHEST, + TrackerPosition.LEFT_UPPER_LEG, + TrackerPosition.RIGHT_UPPER_LEG, + TrackerPosition.LEFT_LOWER_LEG, + TrackerPosition.RIGHT_LOWER_LEG, + ) + + private val SUPPORTED_TRACKERS_TO_RESET = setOf( + TrackerPosition.UPPER_CHEST, + TrackerPosition.CHEST, + TrackerPosition.WAIST, + TrackerPosition.HIP, + TrackerPosition.LEFT_UPPER_LEG, + TrackerPosition.LEFT_LOWER_LEG, + TrackerPosition.RIGHT_UPPER_LEG, + TrackerPosition.RIGHT_LOWER_LEG, + TrackerPosition.LEFT_UPPER_ARM, + TrackerPosition.RIGHT_UPPER_ARM, + ) + + private val UPPER_BODY_TRACKERS_TO_RESET = setOf( + TrackerPosition.UPPER_CHEST, + TrackerPosition.CHEST, + TrackerPosition.WAIST, + TrackerPosition.HIP, + ) + + private val DELAY_BETWEEN_ATTEMPTS = 1.seconds + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/util/DebugOutput.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/util/DebugOutput.kt new file mode 100644 index 0000000000..c4375e4f51 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/util/DebugOutput.kt @@ -0,0 +1,580 @@ +package dev.slimevr.tracking.videocalibration.util + +import dev.slimevr.SLIMEVR_IDENTIFIER +import dev.slimevr.poseframeformat.PfsIO +import dev.slimevr.poseframeformat.PoseFrames +import dev.slimevr.poseframeformat.trackerdata.TrackerFrame +import dev.slimevr.poseframeformat.trackerdata.TrackerFrames +import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets +import dev.slimevr.tracking.processor.skeleton.refactor.Skeleton +import dev.slimevr.tracking.processor.skeleton.refactor.SkeletonUpdater +import dev.slimevr.tracking.trackers.Tracker +import dev.slimevr.tracking.trackers.TrackerPosition +import dev.slimevr.tracking.videocalibration.data.Camera +import dev.slimevr.tracking.videocalibration.data.CocoWholeBodyKeypoint +import dev.slimevr.tracking.videocalibration.data.TrackerResetOverride +import dev.slimevr.tracking.videocalibration.snapshots.HumanPoseSnapshot +import dev.slimevr.tracking.videocalibration.snapshots.TrackerSnapshot +import dev.slimevr.tracking.videocalibration.snapshots.TrackersSnapshot +import io.eiren.util.OperatingSystem +import io.eiren.util.collections.FastList +import io.eiren.util.logging.LogManager +import io.github.axisangles.ktmath.QuaternionD +import io.github.axisangles.ktmath.Vector2D +import kotlinx.coroutines.CoroutineScope +import kotlinx.coroutines.Dispatchers +import kotlinx.coroutines.async +import kotlinx.coroutines.awaitAll +import kotlinx.coroutines.launch +import java.awt.BasicStroke +import java.awt.Color +import java.awt.Dimension +import java.awt.Graphics2D +import java.awt.Stroke +import java.awt.image.BufferedImage +import java.io.File +import java.nio.file.Path +import javax.imageio.ImageIO +import kotlin.collections.component1 +import kotlin.collections.component2 +import kotlin.collections.iterator +import kotlin.io.path.Path +import kotlin.io.path.absolutePathString +import kotlin.io.path.createDirectory +import kotlin.io.path.exists +import kotlin.io.path.writeText +import kotlin.time.Duration +import kotlin.time.Duration.Companion.seconds +import kotlin.time.DurationUnit + +class DebugOutput( + dir: Path, + private val detailed: Boolean = false, +) { + + val cameraFile: Path + val webcamDir: Path + val humanPosesDir: Path + val reconstructionDir: Path + val skeletonOffsetsDir: Path + val trackersFile: Path + val cameraAlignmentImage: Path + val reconstructionVideo: Path + val skeletonOffsetsVideo: Path + + init { + dir.toFile().deleteRecursively() + dir.createDirectory() + + cameraFile = dir.resolve("camera.txt") + + webcamDir = dir.resolve("1_webcam") + webcamDir.createDirectory() + + humanPosesDir = dir.resolve("2_poses") + humanPosesDir.createDirectory() + + reconstructionDir = dir.resolve("3_reconstruction") + reconstructionDir.createDirectory() + + skeletonOffsetsDir = dir.resolve("4_skeleton_offsets") + skeletonOffsetsDir.createDirectory() + + trackersFile = dir.resolve("trackers.pfs") + cameraAlignmentImage = dir.resolve("camera_alignment.png") + reconstructionVideo = dir.resolve("reconstruction.mp4") + skeletonOffsetsVideo = dir.resolve("skeleton_offsets.mp4") + } + + fun saveCamera(camera: Camera) { + val c2w = camera.extrinsic.cameraToWorld + val co = camera.extrinsic.cameraOriginInWorld + val i = camera.intrinsic + val s = camera.imageSize + + val lines = listOf( + c2w.w, + c2w.x, + c2w.y, + c2w.z, + co.x, + co.y, + co.z, + i.fx, + i.fy, + i.tx, + i.ty, + s.width, + s.height, + ) + + cameraFile.writeText( + lines.joinToString("\r\n") { it.toString() }, + ) + } + + fun webcamImage(timestamp: Duration): Path = webcamDir.resolve("webcam_${timestamp.inWholeMilliseconds.toString().padStart(6, '0')}.jpg") + + fun saveWebcamImage(timestamp: Duration, image: BufferedImage) { + ImageIO.write(image, "jpg", webcamImage(timestamp).toFile()) + } + + fun humanPoseImage(timestamp: Duration): Path = humanPosesDir.resolve("pose_${timestamp.inWholeMilliseconds.toString().padStart(6, '0')}.jpg") + + fun drawHumanPoseImage(image: BufferedImage, humanPoseSnapshot: HumanPoseSnapshot): BufferedImage { + val poseImage = BufferedImage(image.width, image.height, BufferedImage.TYPE_INT_RGB) + + val g = poseImage.createGraphics() + g.drawImage(image, 0, 0, image.width, image.height, 0, 0, image.width, image.height, null) + drawBones(g, humanPoseSnapshot) + g.dispose() + + return poseImage + } + + fun saveHumanPoseImage(timestamp: Duration, image: BufferedImage) { + if (!detailed) { + return + } + + ImageIO.write(image, "jpg", humanPoseImage(timestamp).toFile()) + } + + fun saveHandToControllerMatches( + correspondences: List>, + imageSize: Dimension, + ) { + val image = BufferedImage(imageSize.width, imageSize.height, BufferedImage.TYPE_INT_RGB) + + val g = image.createGraphics() + + g.background = MATCH_BACKGROUND + g.clearRect(0, 0, image.width, image.height) + + for ((controller, joint) in correspondences) { + if (controller == null) { + continue + } + + drawLine(g, controller, joint, MATCH_MATCH_COLOR, MATCH_STROKE) + + drawLine(g, controller + Vector2D(-MATCH_MARKER_SIZE, -MATCH_MARKER_SIZE), controller + Vector2D(MATCH_MARKER_SIZE, MATCH_MARKER_SIZE), MATCH_CONTROLLER_COLOR, MATCH_STROKE) + drawLine(g, controller + Vector2D(MATCH_MARKER_SIZE, -MATCH_MARKER_SIZE), controller + Vector2D(-MATCH_MARKER_SIZE, MATCH_MARKER_SIZE), MATCH_CONTROLLER_COLOR, MATCH_STROKE) + + drawCircle(g, joint, MATCH_MARKER_SIZE, MATCH_JOINT_COLOR, MATCH_STROKE) + } + + for (i in 0 until correspondences.size - 1) { + val pA = correspondences[i].second + val pB = correspondences[i + 1].second + drawLine(g, pA, pB, MATCH_PATH_COLOR, MATCH_STROKE) + } + + g.dispose() + + ImageIO.write(image, "png", cameraAlignmentImage.toFile()) + } + + /** + * Saves all the tracker snapshots as a [PfsIO] recording. + */ + fun saveTrackerSnapshots( + trackers: Map, + snapshots: List, + interval: Duration, + ) { + val poseFrames = PoseFrames(trackers.size) + poseFrames.frameInterval = interval.toDouble(DurationUnit.SECONDS).toFloat() + + for ((trackerPosition, tracker) in trackers) { + val frames = FastList(snapshots.size) + for (snapshot in snapshots) { + val trackerSnapshot = snapshot.trackers[trackerPosition] + val frame = + if (trackerSnapshot != null) { + TrackerFrame( + trackerPosition, + trackerSnapshot.adjustedTrackerToWorld.toFloat(), + trackerSnapshot.trackerOriginInWorld?.toFloat(), + null, + trackerSnapshot.rawTrackerToWorld.toFloat(), + ) + } else { + TrackerFrame( + trackerPosition, + null, + null, + null, + null, + ) + } + frames.add(frame) + } + + poseFrames.frameHolders.add(TrackerFrames(tracker.name, frames)) + } + + PfsIO.writeToFile(trackersFile.toFile(), poseFrames) + } + + fun reconstructionImage(index: Int): File = reconstructionDir.resolve("reconstruction_${index.toString().padStart(6, '0')}.jpg").toFile() + + fun saveReconstruction( + camera: Camera, + frames: List>>, + trackerResets: Map, + ) { + if (!detailed) { + return + } + + LogManager.info("Saving video calibration reconstruction...") + + CoroutineScope(Dispatchers.Default).launch { + val jobs = frames.withIndex().map { (i, frame) -> + async { + val (humanSnapshot, trackersSnapshot) = frame + + val image = + ImageIO.read(humanPoseImage(humanSnapshot.timestamp).toFile()) + + val g = image.createGraphics() + drawBones(g, humanSnapshot) + + val leftShoulder = + humanSnapshot.joints[CocoWholeBodyKeypoint.LEFT_SHOULDER] + val rightShoulder = + humanSnapshot.joints[CocoWholeBodyKeypoint.RIGHT_SHOULDER] + val leftHip = + humanSnapshot.joints[CocoWholeBodyKeypoint.LEFT_HIP] + val rightHip = + humanSnapshot.joints[CocoWholeBodyKeypoint.RIGHT_HIP] + if (leftShoulder != null && rightShoulder != null && leftHip != null && rightHip != null) { + val midShoulder = (leftShoulder + rightShoulder) * 0.5 + val midHip = (leftHip + rightHip) * 0.5 + drawLine(g, midShoulder, midHip, BONE_COLOR, BONE_STROKE) + + for ((i, trackerPosition) in UPPER_BODY_TRACKERS.withIndex()) { + val trackerReset = + trackerResets[trackerPosition] ?: continue + val tracker = trackersSnapshot[trackerPosition] + ?: continue + + val t = (1.0 + i) / (UPPER_BODY_TRACKERS.size + 2) + val root = midShoulder * (1.0 - t) + midHip * t + + val r = + trackerReset.toBoneRotation(tracker.rawTrackerToWorld) + drawAxis(g, r, root, camera, AXIS_SCALE) + } + } + + val headTracker = + trackersSnapshot[TrackerPosition.HEAD] + if (headTracker != null) { + val nose = humanSnapshot.joints[CocoWholeBodyKeypoint.NOSE] + if (nose != null) { + val r = headTracker.rawTrackerToWorld + drawAxis(g, r, nose, camera, AXIS_SCALE) + } + } + + for ((trackerPosition, trackerSnapshot) in trackersSnapshot) { + val (j1, j2) = TRACKER_TO_BONE[trackerPosition] ?: continue + val trackerReset = + trackerResets[trackerPosition] ?: continue + + val joint1 = humanSnapshot.joints[j1] ?: continue + val joint2 = humanSnapshot.joints[j2] ?: continue + val boneCenter = (joint1 + joint2) * 0.5 + + val r = + trackerReset.toBoneRotation(trackerSnapshot.rawTrackerToWorld) + drawAxis(g, r, boneCenter, camera, AXIS_SCALE) + } + + g.dispose() + + ImageIO.write(image, "jpg", reconstructionImage(i)) + } + } + + jobs.awaitAll() + + val ffmpeg = System.getenv("FFMPEG_PATH") + if (ffmpeg != null && Path(ffmpeg).exists()) { + LogManager.info("Generating reconstruction video...") + + var sumInterval = Duration.ZERO + for (i in 1 until frames.size) { + val f1 = frames[i - 1] + val f2 = frames[i] + val interval = f2.first.timestamp - f1.first.timestamp + sumInterval += interval + } + + val avgInterval = sumInterval / frames.size + val fps = 1.seconds / avgInterval + + val inputPattern = reconstructionDir.resolve("reconstruction_%06d.jpg") + + val command = listOf( + ffmpeg, + "-y", // overwrite output + "-framerate", fps.toString(), + "-i", inputPattern.absolutePathString(), + "-c:v", "libx264", + "-pix_fmt", "yuv420p", + reconstructionVideo.absolutePathString(), + ) + + val process = ProcessBuilder(command) + .redirectErrorStream(true) + .start() + + process.inputStream.bufferedReader().useLines { lines -> + lines.forEach { println(it) } + } + + val exitCode = process.waitFor() + LogManager.info("ffmpeg exited with $exitCode") + } + + LogManager.info("Video calibration reconstruction complete") + } + } + + fun skeletonOffsetImage(index: Int): File = skeletonOffsetsDir.resolve("skeleton_offset_${index.toString().padStart(6, '0')}.jpg").toFile() + + fun saveSkeletonOffsets( + camera: Camera, + frames: List>>, + initialSkeletonOffsets: Map, + optimizedSkeletonOffsets: Map, + ) { + if (!detailed) { + return + } + + val config = SkeletonUpdater.HumanSkeletonConfig() + + CoroutineScope(Dispatchers.Default).launch { + val jobs = frames.withIndex().map { (i, frame) -> + async { + val (humanSnapshot, trackersSnapshot) = frame + + val baseImage = + ImageIO.read(humanPoseImage(humanSnapshot.timestamp).toFile()) + + val image = BufferedImage( + baseImage.width * 2, + baseImage.height, + BufferedImage.TYPE_INT_RGB, + ) + + val g = image.createGraphics() + g.drawImage( + baseImage, + 0, + 0, + baseImage.width, + baseImage.height, + 0, + 0, + baseImage.width, + baseImage.height, + null, + ) + g.drawImage( + baseImage, + baseImage.width, + 0, + baseImage.width * 2, + baseImage.height, + 0, + 0, + baseImage.width, + baseImage.height, + null, + ) + + val skeleton = Skeleton(false, false) + val trackersData = + SkeletonUpdater.TrackersData.fromSnapshot(trackersSnapshot) + + // Draw initial + SkeletonUpdater( + skeleton, + trackersData, + config, + initialSkeletonOffsets, + ).update() + drawSkeleton(g, skeleton, camera, Vector2D.NULL) + + // Draw optimized + SkeletonUpdater( + skeleton, + trackersData, + config, + optimizedSkeletonOffsets, + ).update() + drawSkeleton( + g, + skeleton, + camera, + Vector2D(baseImage.width.toDouble(), 0.0), + ) + + g.dispose() + + ImageIO.write(image, "jpg", skeletonOffsetImage(i)) + } + } + + jobs.awaitAll() + + val ffmpeg = System.getenv("FFMPEG_PATH") + if (ffmpeg != null && Path(ffmpeg).exists()) { + LogManager.info("Generating reconstruction video...") + + var sumInterval = Duration.ZERO + for (i in 1 until frames.size) { + val f1 = frames[i - 1] + val f2 = frames[i] + val interval = f2.first.timestamp - f1.first.timestamp + sumInterval += interval + } + + val avgInterval = sumInterval / frames.size + val fps = 1.seconds / avgInterval + + val inputPattern = skeletonOffsetsDir.resolve("skeleton_offset_%06d.jpg") + + val command = listOf( + ffmpeg, + "-y", // overwrite output + "-framerate", fps.toString(), + "-i", inputPattern.absolutePathString(), + "-c:v", "libx264", + "-pix_fmt", "yuv420p", + skeletonOffsetsVideo.absolutePathString(), + ) + + val process = ProcessBuilder(command) + .redirectErrorStream(true) + .start() + + process.inputStream.bufferedReader().useLines { lines -> + lines.forEach { println(it) } + } + + val exitCode = process.waitFor() + LogManager.info("ffmpeg exited with $exitCode") + } + } + } + + private fun drawSkeleton(g: Graphics2D, skeleton: Skeleton, camera: Camera, offset: Vector2D) { + val bones = mutableListOf(skeleton.headBone) + while (bones.isNotEmpty()) { + val bone = bones.removeLast() + val head = camera.project(bone.getPosition().toDouble()) + val tail = camera.project(bone.getTailPosition().toDouble()) + if (head != null && tail != null) { + drawLine(g, head + offset, tail + offset, SKELETON_BONE_COLOR, BONE_STROKE) + } + bones.addAll(bone.children) + } + } + + private fun drawAxis(g: Graphics2D, rotation: QuaternionD, imagePoint: Vector2D, camera: Camera, scale: Double) { + val x = camera.project(rotation.sandwichUnitX() * scale, imagePoint, 1.0) + val y = camera.project(rotation.sandwichUnitY() * scale, imagePoint, 1.0) + val z = camera.project(rotation.sandwichUnitZ() * scale, imagePoint, 1.0) + if (x != null && y != null && z != null) { + drawLine(g, imagePoint, imagePoint + x, AXIS_X_COLOR, AXIS_STROKE) + drawLine(g, imagePoint, imagePoint + y, AXIS_Y_COLOR, AXIS_STROKE) + drawLine(g, imagePoint, imagePoint + z, AXIS_Z_COLOR, AXIS_STROKE) + } + } + + private fun drawBones(g: Graphics2D, humanPoseSnapshot: HumanPoseSnapshot) { + for ((j1, j2) in BONES) { + val joint1 = humanPoseSnapshot.joints[j1] ?: continue + val joint2 = humanPoseSnapshot.joints[j2] ?: continue + drawLine(g, joint1, joint2, BONE_COLOR, BONE_STROKE) + } + } + + private fun drawLine(g: Graphics2D, p1: Vector2D, p2: Vector2D, color: Color, stroke: Stroke) { + g.color = color + g.stroke = stroke + g.drawLine(p1.x.toInt(), p1.y.toInt(), p2.x.toInt(), p2.y.toInt()) + } + + private fun drawCircle(g: Graphics2D, p: Vector2D, r: Double, color: Color, stroke: Stroke) { + g.color = color + g.stroke = stroke + g.drawOval((p.x - r).toInt(), (p.y - r).toInt(), (2.0 * r).toInt(), (2.0 * r).toInt()) + } + + companion object { + + private const val VIDEO_CALIBRATION_FOLDER = "VideoCalibration" + + private val BONES = listOf( + CocoWholeBodyKeypoint.LEFT_SHOULDER to CocoWholeBodyKeypoint.RIGHT_SHOULDER, + CocoWholeBodyKeypoint.LEFT_SHOULDER to CocoWholeBodyKeypoint.LEFT_ELBOW, + CocoWholeBodyKeypoint.LEFT_ELBOW to CocoWholeBodyKeypoint.LEFT_WRIST, + CocoWholeBodyKeypoint.RIGHT_SHOULDER to CocoWholeBodyKeypoint.RIGHT_ELBOW, + CocoWholeBodyKeypoint.RIGHT_ELBOW to CocoWholeBodyKeypoint.RIGHT_WRIST, + CocoWholeBodyKeypoint.LEFT_HIP to CocoWholeBodyKeypoint.RIGHT_HIP, + CocoWholeBodyKeypoint.LEFT_HIP to CocoWholeBodyKeypoint.LEFT_KNEE, + CocoWholeBodyKeypoint.LEFT_KNEE to CocoWholeBodyKeypoint.LEFT_ANKLE, + CocoWholeBodyKeypoint.RIGHT_HIP to CocoWholeBodyKeypoint.RIGHT_KNEE, + CocoWholeBodyKeypoint.RIGHT_KNEE to CocoWholeBodyKeypoint.RIGHT_ANKLE, + CocoWholeBodyKeypoint.LEFT_SHOULDER to CocoWholeBodyKeypoint.LEFT_HIP, + CocoWholeBodyKeypoint.RIGHT_SHOULDER to CocoWholeBodyKeypoint.RIGHT_HIP, + ) + + private val TRACKER_TO_BONE = mapOf( + TrackerPosition.LEFT_UPPER_ARM to (CocoWholeBodyKeypoint.LEFT_SHOULDER to CocoWholeBodyKeypoint.LEFT_ELBOW), + TrackerPosition.RIGHT_UPPER_ARM to (CocoWholeBodyKeypoint.RIGHT_SHOULDER to CocoWholeBodyKeypoint.RIGHT_ELBOW), + TrackerPosition.LEFT_UPPER_LEG to (CocoWholeBodyKeypoint.LEFT_HIP to CocoWholeBodyKeypoint.LEFT_KNEE), + TrackerPosition.LEFT_LOWER_LEG to (CocoWholeBodyKeypoint.LEFT_KNEE to CocoWholeBodyKeypoint.LEFT_ANKLE), + TrackerPosition.RIGHT_UPPER_LEG to (CocoWholeBodyKeypoint.RIGHT_HIP to CocoWholeBodyKeypoint.RIGHT_KNEE), + TrackerPosition.RIGHT_LOWER_LEG to (CocoWholeBodyKeypoint.RIGHT_KNEE to CocoWholeBodyKeypoint.RIGHT_ANKLE), + ) + + private val UPPER_BODY_TRACKERS = listOf( + TrackerPosition.UPPER_CHEST, + TrackerPosition.CHEST, + TrackerPosition.WAIST, + TrackerPosition.HIP, + ) + + private val MATCH_BACKGROUND = Color.WHITE + private val MATCH_STROKE = BasicStroke(2.0f) + private val MATCH_PATH_COLOR = Color.GRAY + private val MATCH_MATCH_COLOR = Color.GRAY + private val MATCH_MARKER_SIZE = 5.0 + private val MATCH_JOINT_COLOR = Color.RED + private val MATCH_CONTROLLER_COLOR = Color.BLUE + + private val BONE_COLOR = Color.GRAY + private val BONE_STROKE = BasicStroke(4.0f) + + private val AXIS_STROKE = BasicStroke(6.0f) + private val AXIS_X_COLOR = Color.RED + private val AXIS_Y_COLOR = Color.GREEN + private val AXIS_Z_COLOR = Color.BLUE + private const val AXIS_SCALE = 0.05 + + private val SKELETON_BONE_COLOR = Color.GREEN + + val DEFAULT_DIR: Path = + OperatingSystem + .resolveConfigDirectory(SLIMEVR_IDENTIFIER)!! + .resolve(VIDEO_CALIBRATION_FOLDER) + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/util/KTMathExtensions.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/util/KTMathExtensions.kt new file mode 100644 index 0000000000..5847c92d4f --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/util/KTMathExtensions.kt @@ -0,0 +1,31 @@ +package dev.slimevr.tracking.videocalibration.util + +import com.jme3.math.FastMath +import io.github.axisangles.ktmath.EulerOrder +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.QuaternionD +import io.github.axisangles.ktmath.Vector3D + +fun Quaternion.toEulerYZXString(): String { + val yzx = toEulerAngles(EulerOrder.YZX) + return "(y=${"%.1f".format(yzx.y * FastMath.RAD_TO_DEG)}, z=${"%.1f".format(yzx.z * FastMath.RAD_TO_DEG)} x=${"%.1f".format(yzx.x * FastMath.RAD_TO_DEG)})" +} + +fun QuaternionD.toEulerYZXString(): String { + val yzx = toEulerAngles(EulerOrder.YZX) + return "(y=${"%.1f".format(yzx.y * FastMath.RAD_TO_DEG)}, z=${"%.1f".format(yzx.z * FastMath.RAD_TO_DEG)} x=${"%.1f".format(yzx.x * FastMath.RAD_TO_DEG)})" +} + +fun Quaternion.toAngleAxisString(): String { + val axisAngle = toRotationVector() + val angle = axisAngle.len() + val axis = if (angle >= 1e-12f) axisAngle.unit() else Vector3D.POS_Y + return "($axis, ${"%.1f".format(angle * FastMath.RAD_TO_DEG)})" +} + +fun QuaternionD.toAngleAxisString(): String { + val axisAngle = toRotationVector() + val angle = axisAngle.len() + val axis = if (angle >= 1e-12) axisAngle.unit() else Vector3D.POS_Y + return "($axis, ${"%.1f".format(angle * FastMath.RAD_TO_DEG)})" +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/util/NumericalJacobian.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/util/NumericalJacobian.kt new file mode 100644 index 0000000000..c24adff996 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/util/NumericalJacobian.kt @@ -0,0 +1,33 @@ +package dev.slimevr.tracking.videocalibration.util + +import org.apache.commons.math3.analysis.MultivariateVectorFunction +import org.apache.commons.math3.fitting.leastsquares.MultivariateJacobianFunction +import org.apache.commons.math3.linear.Array2DRowRealMatrix +import org.apache.commons.math3.linear.ArrayRealVector +import org.apache.commons.math3.util.Pair + +fun numericalJacobian( + f: MultivariateVectorFunction, + eps: Double = 1e-6, +): MultivariateJacobianFunction = MultivariateJacobianFunction { point -> + val x = point.toArray() + val y = f.value(x) + + val m = y.size + val n = x.size + + val jacobian = Array2DRowRealMatrix(m, n) + + for (j in 0 until n) { + val x2 = x.clone() + x2[j] += eps + + val y2 = f.value(x2) + + for (i in 0 until m) { + jacobian.setEntry(i, j, (y2[i] - y[i]) / eps) + } + } + + Pair(ArrayRealVector(y), jacobian) +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/videocalibration/util/ScheduledInterval.kt b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/util/ScheduledInterval.kt new file mode 100644 index 0000000000..64b2035d92 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/tracking/videocalibration/util/ScheduledInterval.kt @@ -0,0 +1,29 @@ +package dev.slimevr.tracking.videocalibration.util + +import kotlin.time.Duration +import kotlin.time.TimeSource + +/** + * Helper to track when to run a regularly scheduled task. + * + * Useful when run in a single-threaded loop like VRServer::onTick. + */ +class ScheduledInterval(val interval: Duration) { + + private var nextInvoke = TimeSource.Monotonic.markNow() + + /** + * Checks if the task should be invoked. + */ + fun shouldInvoke(): Boolean { + val now = TimeSource.Monotonic.markNow() + if (now >= nextInvoke) { + while (nextInvoke < now) { + nextInvoke += interval + } + return true + } else { + return false + } + } +} diff --git a/server/core/src/main/java/io/eiren/util/logging/LogManager.java b/server/core/src/main/java/io/eiren/util/logging/LogManager.java index bd1fe0b76d..909f9afa65 100644 --- a/server/core/src/main/java/io/eiren/util/logging/LogManager.java +++ b/server/core/src/main/java/io/eiren/util/logging/LogManager.java @@ -120,7 +120,7 @@ public static void closeLogger() { handler = new ConsoleHandler(); global.addHandler(handler); } - handler.setFormatter(new ShortConsoleLogFormatter()); + handler.setFormatter(new PreciseConsoleLogFormatter()); System.setOut(new PrintStream(new LoggerOutputStream(log, Level.INFO), true)); System.setErr(new PrintStream(new LoggerOutputStream(log, Level.SEVERE), true)); diff --git a/server/core/src/main/java/io/github/axisangles/ktmath/EulerAnglesD.kt b/server/core/src/main/java/io/github/axisangles/ktmath/EulerAnglesD.kt new file mode 100644 index 0000000000..f8120af3bf --- /dev/null +++ b/server/core/src/main/java/io/github/axisangles/ktmath/EulerAnglesD.kt @@ -0,0 +1,123 @@ +@file:Suppress("unused") + +package io.github.axisangles.ktmath + +import kotlinx.serialization.Serializable +import kotlin.math.cos +import kotlin.math.sin + +@JvmInline +@Serializable +value class EulerAnglesD(val order: EulerOrder, val x: Double, val y: Double, val z: Double) { + operator fun component1(): EulerOrder = order + operator fun component2(): Double = x + operator fun component3(): Double = y + operator fun component4(): Double = z + + /** + * creates a quaternion which represents the same rotation as this eulerAngles + * @return the quaternion + */ + fun toQuaternion(): QuaternionD { + val cX = cos(x / 2f) + val cY = cos(y / 2f) + val cZ = cos(z / 2f) + val sX = sin(x / 2f) + val sY = sin(y / 2f) + val sZ = sin(z / 2f) + + return when (order) { + EulerOrder.XYZ -> QuaternionD( + cX * cY * cZ - sX * sY * sZ, + cY * cZ * sX + cX * sY * sZ, + cX * cZ * sY - cY * sX * sZ, + cZ * sX * sY + cX * cY * sZ, + ) + + EulerOrder.YZX -> QuaternionD( + cX * cY * cZ - sX * sY * sZ, + cY * cZ * sX + cX * sY * sZ, + cX * cZ * sY + cY * sX * sZ, + cX * cY * sZ - cZ * sX * sY, + ) + + EulerOrder.ZXY -> QuaternionD( + cX * cY * cZ - sX * sY * sZ, + cY * cZ * sX - cX * sY * sZ, + cX * cZ * sY + cY * sX * sZ, + cZ * sX * sY + cX * cY * sZ, + ) + + EulerOrder.ZYX -> QuaternionD( + cX * cY * cZ + sX * sY * sZ, + cY * cZ * sX - cX * sY * sZ, + cX * cZ * sY + cY * sX * sZ, + cX * cY * sZ - cZ * sX * sY, + ) + + EulerOrder.YXZ -> QuaternionD( + cX * cY * cZ + sX * sY * sZ, + cY * cZ * sX + cX * sY * sZ, + cX * cZ * sY - cY * sX * sZ, + cX * cY * sZ - cZ * sX * sY, + ) + + EulerOrder.XZY -> QuaternionD( + cX * cY * cZ + sX * sY * sZ, + cY * cZ * sX - cX * sY * sZ, + cX * cZ * sY - cY * sX * sZ, + cZ * sX * sY + cX * cY * sZ, + ) + } + } + + // temp, replace with direct conversion later + // fun toMatrix(): Matrix3D = this.toQuaternion().toMatrix() + + /** + * creates a matrix which represents the same rotation as this eulerAngles + * @return the matrix + */ + fun toMatrix(): Matrix3D { + val cX = cos(x) + val cY = cos(y) + val cZ = cos(z) + val sX = sin(x) + val sY = sin(y) + val sZ = sin(z) + + @Suppress("ktlint") + return when (order) { + // ktlint ruining spacing + EulerOrder.XYZ -> Matrix3D( + cY*cZ , -cY*sZ , sY , + cZ*sX*sY + cX*sZ , cX*cZ - sX*sY*sZ , -cY*sX , + sX*sZ - cX*cZ*sY , cZ*sX + cX*sY*sZ , cX*cY ) + + EulerOrder.YZX -> Matrix3D( + cY*cZ , sX*sY - cX*cY*sZ , cX*sY + cY*sX*sZ , + sZ , cX*cZ , -cZ*sX , + -cZ*sY , cY*sX + cX*sY*sZ , cX*cY - sX*sY*sZ ) + + EulerOrder.ZXY -> Matrix3D( + cY*cZ - sX*sY*sZ , -cX*sZ , cZ*sY + cY*sX*sZ , + cZ*sX*sY + cY*sZ , cX*cZ , sY*sZ - cY*cZ*sX , + -cX*sY , sX , cX*cY ) + + EulerOrder.ZYX -> Matrix3D( + cY*cZ , cZ*sX*sY - cX*sZ , cX*cZ*sY + sX*sZ , + cY*sZ , cX*cZ + sX*sY*sZ , cX*sY*sZ - cZ*sX , + -sY , cY*sX , cX*cY ) + + EulerOrder.YXZ -> Matrix3D( + cY*cZ + sX*sY*sZ , cZ*sX*sY - cY*sZ , cX*sY , + cX*sZ , cX*cZ , -sX , + cY*sX*sZ - cZ*sY , cY*cZ*sX + sY*sZ , cX*cY ) + + EulerOrder.XZY -> Matrix3D( + cY*cZ , -sZ , cZ*sY , + sX*sY + cX*cY*sZ , cX*cZ , cX*sY*sZ - cY*sX , + cY*sX*sZ - cX*sY , cZ*sX , cX*cY + sX*sY*sZ ) + } + } +} diff --git a/server/core/src/main/java/io/github/axisangles/ktmath/Matrix3D.kt b/server/core/src/main/java/io/github/axisangles/ktmath/Matrix3D.kt new file mode 100644 index 0000000000..0dcd87d101 --- /dev/null +++ b/server/core/src/main/java/io/github/axisangles/ktmath/Matrix3D.kt @@ -0,0 +1,477 @@ +@file:Suppress("ktlint", "unused") + +package io.github.axisangles.ktmath + +import kotlinx.serialization.Serializable +import kotlin.math.* + +@JvmInline +@Serializable +value class Matrix3D +@Suppress("ktlint") constructor( + val xx: Double, val yx: Double, val zx: Double, + val xy: Double, val yy: Double, val zy: Double, + val xz: Double, val yz: Double, val zz: Double +) { + companion object { + val NULL = Matrix3D( + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0 + ) + val IDENTITY = Matrix3D( + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 + ) + } + + /** + * creates a new matrix from x y and z column vectors + */ + constructor(x: Vector3D, y: Vector3D, z: Vector3D) : this( + x.x, y.x, z.x, + x.y, y.y, z.y, + x.z, y.z, z.z + ) + + // column getters + val x get() = Vector3D(xx, xy, xz) + val y get() = Vector3D(yx, yy, yz) + val z get() = Vector3D(zx, zy, zz) + + // row getters + val xRow get() = Vector3D(xx, yx, zx) + val yRow get() = Vector3D(xy, yy, zy) + val zRow get() = Vector3D(xz, yz, zz) + + operator fun component1(): Double = xx + operator fun component2(): Double = yx + operator fun component3(): Double = zx + operator fun component4(): Double = xy + operator fun component5(): Double = yy + operator fun component6(): Double = zy + operator fun component7(): Double = xz + operator fun component8(): Double = yz + operator fun component9(): Double = zz + + operator fun unaryMinus(): Matrix3D = Matrix3D( + -xx, -yx, -zx, + -xy, -yy, -zy, + -xz, -yz, -zz + ) + + operator fun plus(that: Matrix3D): Matrix3D = Matrix3D( + this.xx + that.xx, this.yx + that.yx, this.zx + that.zx, + this.xy + that.xy, this.yy + that.yy, this.zy + that.zy, + this.xz + that.xz, this.yz + that.yz, this.zz + that.zz + ) + + operator fun minus(that: Matrix3D): Matrix3D = Matrix3D( + this.xx - that.xx, this.yx - that.yx, this.zx - that.zx, + this.xy - that.xy, this.yy - that.yy, this.zy - that.zy, + this.xz - that.xz, this.yz - that.yz, this.zz - that.zz + ) + + operator fun times(that: Double): Matrix3D = Matrix3D( + this.xx * that, this.yx * that, this.zx * that, + this.xy * that, this.yy * that, this.zy * that, + this.xz * that, this.yz * that, this.zz * that + ) + + operator fun times(that: Vector3D): Vector3D = Vector3D( + this.xx * that.x + this.yx * that.y + this.zx * that.z, + this.xy * that.x + this.yy * that.y + this.zy * that.z, + this.xz * that.x + this.yz * that.y + this.zz * that.z + ) + + operator fun times(that: Matrix3D): Matrix3D = Matrix3D( + this.xx * that.xx + this.yx * that.xy + this.zx * that.xz, + this.xx * that.yx + this.yx * that.yy + this.zx * that.yz, + this.xx * that.zx + this.yx * that.zy + this.zx * that.zz, + this.xy * that.xx + this.yy * that.xy + this.zy * that.xz, + this.xy * that.yx + this.yy * that.yy + this.zy * that.yz, + this.xy * that.zx + this.yy * that.zy + this.zy * that.zz, + this.xz * that.xx + this.yz * that.xy + this.zz * that.xz, + this.xz * that.yx + this.yz * that.yy + this.zz * that.yz, + this.xz * that.zx + this.yz * that.zy + this.zz * that.zz + ) + + /** + * computes the square of the frobenius norm of this matrix + * @return the frobenius norm squared + */ + fun normSq(): Double = + xx * xx + yx * yx + zx * zx + + xy * xy + yy * yy + zy * zy + + xz * xz + yz * yz + zz * zz + + /** + * computes the frobenius norm of this matrix + * @return the frobenius norm + */ + fun norm(): Double = sqrt(normSq()) + + /** + * computes the determinant of this matrix + * @return the determinant + */ + fun det(): Double = + (xz * yx - xx * yz) * zy + + (xx * yy - xy * yx) * zz + + (xy * yz - xz * yy) * zx + + /** + * computes the trace of this matrix + * @return the trace + */ + fun trace(): Double = xx + yy + zz + + /** + * computes the transpose of this matrix + * @return the transpose matrix + */ + fun transpose(): Matrix3D = Matrix3D( + xx, xy, xz, + yx, yy, yz, + zx, zy, zz + ) + + /** + * computes the inverse of this matrix + * @return the inverse matrix + */ + fun inv(): Matrix3D { + val det = det() + return Matrix3D( + (yy * zz - yz * zy) / det, (yz * zx - yx * zz) / det, (yx * zy - yy * zx) / det, + (xz * zy - xy * zz) / det, (xx * zz - xz * zx) / det, (xy * zx - xx * zy) / det, + (xy * yz - xz * yy) / det, (xz * yx - xx * yz) / det, (xx * yy - xy * yx) / det + ) + } + + operator fun div(that: Double): Matrix3D = this * (1.0 / that) + + /** + * computes the right division, this * that^-1 + */ + operator fun div(that: Matrix3D): Matrix3D = this * that.inv() + + /** + * computes the inverse transpose of this matrix + * @return the inverse transpose matrix + */ + fun invTranspose(): Matrix3D { + val det = det() + return Matrix3D( + (yy * zz - yz * zy) / det, (xz * zy - xy * zz) / det, (xy * yz - xz * yy) / det, + (yz * zx - yx * zz) / det, (xx * zz - xz * zx) / det, (xz * yx - xx * yz) / det, + (yx * zy - yy * zx) / det, (xy * zx - xx * zy) / det, (xx * yy - xy * yx) / det + ) + } + + /* + The following method returns the best guess rotation matrix. + In general, a square matrix can be represented as an + orthogonal matrix * symmetric matrix. + M = O*S + A symmetric matrix's transpose is itself. + An orthogonal matrix's transpose is its inverse. + S^T = S + O^T = O^-1 + If we perform the following process, we can factor out O. + M + M^-T + = O*S + (O*S)^-T + = O*S + O^-T*S^-T + = O*S + O*S^-T + = O*(S + S^-T) + So we see if we perform M + M^-T, the rotation, O, remains unchanged. + Iterating M = (M + M^-T)/2, we converge the symmetric part to identity. + + This converges exponentially (one digit per iteration) when it is far from a + rotation matrix, and quadratically (double the digits each iteration) when it + is close to a rotation matrix. + */ + /** + * computes the nearest orthonormal matrix to this matrix + * @return the rotation matrix + */ + fun orthonormalize(): Matrix3D { + if (this.det() <= 0.0) { // maybe this doesn't have to be so + throw Exception("Attempt to convert non-positive determinant matrix to rotation matrix") + } + + var curMat = this + var curDet = Double.POSITIVE_INFINITY + + for (i in 1..100) { + val newMat = (curMat + curMat.invTranspose()) / 2.0 + val newDet = abs(newMat.det()) + // should almost always exit immediately + if (newDet >= curDet) return curMat + if (newDet <= 1.0000001f) return newMat + curMat = newMat + curDet = newDet + } + + return curMat + } + + /** + * finds the rotation matrix closest to all given rotation matrices. + * multiply input matrices by a weight for weighted averaging. + * WARNING: NOT ANGULAR + * @param others a variable number of additional boxed matrices to average + * @return the average rotation matrix + */ + fun average(vararg others: ObjectMatrix3D): Matrix3D { + var count = 1.0 + var sum = this + others.forEach { + count += 1.0 + sum += it.toValue() + } + return (sum / count).orthonormalize() + } + + /** + * linearly interpolates this matrix to that matrix by t + * @param that the matrix towards which to interpolate + * @param t the amount by which to interpolate + * @return the interpolated matrix + */ + fun lerp(that: Matrix3D, t: Double): Matrix3D = (1.0 - t) * this + t * that + + // assumes this matrix is orthonormal and converts this to a quaternion + /** + * creates a quaternion representing the same rotation as this matrix, + * assuming the matrix is a rotation matrix + * @return the quaternion + */ + fun toQuaternionAssumingOrthonormal(): QuaternionD { + return if (yy > -zz && zz > -xx && xx > -yy) { + QuaternionD(1.0 + xx + yy + zz, yz - zy, zx - xz, xy - yx).unit() + } else if (xx > yy && xx > zz) { + QuaternionD(yz - zy, 1.0 + xx - yy - zz, xy + yx, xz + zx).unit() + } else if (yy > zz) { + QuaternionD(zx - xz, xy + yx, 1.0 - xx + yy - zz, yz + zy).unit() + } else { + QuaternionD(xy - yx, xz + zx, yz + zy, 1.0 - xx - yy + zz).unit() + } + } + + // orthogonalizes the matrix then returns the quaternion + /** + * creates a quaternion representing the same rotation as this matrix + * @return the quaternion + */ + fun toQuaternion(): QuaternionD = orthonormalize().toQuaternionAssumingOrthonormal() + + /* + the standard algorithm: + + yAng = asin(clamp(zx, -1, 1)) + if (abs(zx) < 0.9999999f) { + xAng = atan2(-zy, zz) + zAng = atan2(-yx, xx) + } else { + xAng = atan2(yz, yy) + zAng = 0 + } + + + + problems with the standard algorithm: + + 1) + yAng = asin(clamp(zx, -1, 1)) + + FIX: + yAng = atan2(zx, sqrt(zy*zy + zz*zz)) + + this loses many bits of accuracy when near the singularity, zx = +-1 and + can cause the algorithm to return completely inaccurate results with only + small floating point errors in the matrix. this happens because zx is + NOT sin(pitch), but rather errorTerm*sin(pitch), and small changes in zx + when zx is near +-1 make large changes in asin(zx). + + + + 2) + if (abs(zx) < 0.9999999f) { + + FIX: + if (zy*zy + zz*zz > 0.0) { + + this clause, meant to reduce the inaccuracy of the code following does + not actually test for the condition that makes the following atans unstable. + that is, when (zy, zz) and (yx, xx) are near 0. + after several matrix multiplications, the error term is expected to be + larger than 0.0000001. Often times, this clause will not catch the conditions + it is trying to catch. + + + + 3) + zAng = atan2(-yx, xx) + + FIX: + zAng = atan2(xy*zz - xz*zy, yy*zz - yz*zy) + + xAng and zAng are being computed separately. In the case of near singularity + the angles of xAng and zAng are effectively added together as they represent + the same operation (a rotation about the global y-axis). When computed + separately, it is not guaranteed that the xAng + zAng add together to give + the actual final rotation about the global y-axis. + + + + 4) + after many matrix operations are performed, without orthonormalization + the matrix will contain floating point errors that will throw off the + accuracy of any euler angles algorithm. orthonormalization should be + built into the prerequisites for this function + */ + + /** + * creates an eulerAngles representing the same rotation as this matrix, + * assuming the matrix is a rotation matrix + * @return the eulerAngles + */ + fun toEulerAnglesAssumingOrthonormal(order: EulerOrder): EulerAnglesD { + val ETA = 1.5707964 + when (order) { + EulerOrder.XYZ -> { + val kc = sqrt(zy * zy + zz * zz) + if (kc < 1e-7f) { + return EulerAnglesD( + EulerOrder.XYZ, + atan2(yz, yy), + ETA.withSign(zx), + 0.0 + ) + } + + return EulerAnglesD( + EulerOrder.XYZ, + atan2(-zy, zz), + atan2(zx, kc), + atan2(xy * zz - xz * zy, yy * zz - yz * zy) + ) + } + EulerOrder.YZX -> { + val kc = sqrt(xx * xx + xz * xz) + if (kc < 1e-7f) { + return EulerAnglesD( + EulerOrder.YZX, + 0.0, + atan2(zx, zz), + ETA.withSign(xy) + ) + } + + return EulerAnglesD( + EulerOrder.YZX, + atan2(xx * yz - xz * yx, xx * zz - xz * zx), + atan2(-xz, xx), + atan2(xy, kc) + ) + } + EulerOrder.ZXY -> { + val kc = sqrt(yy * yy + yx * yx) + if (kc < 1e-7f) { + return EulerAnglesD( + EulerOrder.ZXY, + ETA.withSign(yz), + 0.0, + atan2(xy, xx) + ) + } + + return EulerAnglesD( + EulerOrder.ZXY, + atan2(yz, kc), + atan2(yy * zx - yx * zy, yy * xx - yx * xy), + atan2(-yx, yy) + ) + } + EulerOrder.ZYX -> { + val kc = sqrt(xy * xy + xx * xx) + if (kc < 1e-7f) { + return EulerAnglesD( + EulerOrder.ZYX, + 0.0, + ETA.withSign(-xz), + atan2(-yx, yy) + ) + } + + return EulerAnglesD( + EulerOrder.ZYX, + atan2(zx * xy - zy * xx, yy * xx - yx * xy), + atan2(-xz, kc), + atan2(xy, xx) + ) + } + + EulerOrder.YXZ -> { + val kc = sqrt(zx * zx + zz * zz) + if (kc < 1e-7f) { + return EulerAnglesD( + EulerOrder.YXZ, + ETA.withSign(-zy), + atan2(-xz, xx), + 0.0 + ) + } + + return EulerAnglesD( + EulerOrder.YXZ, + atan2(-zy, kc), + atan2(zx, zz), + atan2(yz * zx - yx * zz, xx * zz - xz * zx) + ) + } + EulerOrder.XZY -> { + val kc = sqrt(yz * yz + yy * yy) + if (kc < 1e-7f) { + return EulerAnglesD( + EulerOrder.XZY, + atan2(-zy, zz), + 0.0, + ETA.withSign(-yx) + ) + } + + return EulerAnglesD( + EulerOrder.XZY, + atan2(yz, yy), + atan2(xy * yz - xz * yy, zz * yy - zy * yz), + atan2(-yx, kc) + ) + } + } + } + + // orthogonalizes the matrix then returns the euler angles + /** + * creates an eulerAngles representing the same rotation as this matrix + * @return the eulerAngles + */ + fun toEulerAngles(order: EulerOrder): EulerAnglesD = + orthonormalize().toEulerAnglesAssumingOrthonormal(order) + + fun toObject() = ObjectMatrix3D(xx, yx, zx, xy, yy, zy, xz, yz, zz) +} + +data class ObjectMatrix3D( + val xx: Double, val yx: Double, val zx: Double, + val xy: Double, val yy: Double, val zy: Double, + val xz: Double, val yz: Double, val zz: Double +) { + fun toValue() = Matrix3D(xx, yx, zx, xy, yy, zy, xz, yz, zz) +} + +operator fun Double.times(that: Matrix3D): Matrix3D = that * this + +operator fun Double.div(that: Matrix3D): Matrix3D = that.inv() * this diff --git a/server/core/src/main/java/io/github/axisangles/ktmath/Quaternion.kt b/server/core/src/main/java/io/github/axisangles/ktmath/Quaternion.kt index 1f4cccb398..ac639d6891 100644 --- a/server/core/src/main/java/io/github/axisangles/ktmath/Quaternion.kt +++ b/server/core/src/main/java/io/github/axisangles/ktmath/Quaternion.kt @@ -493,6 +493,8 @@ value class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { fun toEulerAngles(order: EulerOrder): EulerAngles = this.toMatrix().toEulerAnglesAssumingOrthonormal(order) fun toObject() = ObjectQuaternion(w, x, y, z) + + fun toDouble() = QuaternionD(w.toDouble(), x.toDouble(), y.toDouble(), z.toDouble()) } data class ObjectQuaternion(val w: Float, val x: Float, val y: Float, val z: Float) { diff --git a/server/core/src/main/java/io/github/axisangles/ktmath/QuaternionD.kt b/server/core/src/main/java/io/github/axisangles/ktmath/QuaternionD.kt new file mode 100644 index 0000000000..7964b16bb3 --- /dev/null +++ b/server/core/src/main/java/io/github/axisangles/ktmath/QuaternionD.kt @@ -0,0 +1,507 @@ +@file:Suppress("unused") + +package io.github.axisangles.ktmath + +import kotlinx.serialization.Serializable +import kotlin.math.* + +@JvmInline +@Serializable +value class QuaternionD(val w: Double, val x: Double, val y: Double, val z: Double) { + companion object { + val NULL = QuaternionD(0.0, 0.0, 0.0, 0.0) + val IDENTITY = QuaternionD(1.0, 0.0, 0.0, 0.0) + val I = QuaternionD(0.0, 1.0, 0.0, 0.0) + val J = QuaternionD(0.0, 0.0, 1.0, 0.0) + val K = QuaternionD(0.0, 0.0, 0.0, 1.0) + + /** + * SlimeVR-specific constants and utils + */ + val SLIMEVR: SlimeVR = SlimeVR + + /** + * Used to rotate an identity quaternion to face upwards for [twinExtendedBack]. + */ + private val UP_ADJ = QuaternionD(0.707, -0.707, 0.0, 0.0) + + /** + * creates a new quaternion representing the rotation about v's axis + * by an angle of v's length + * @param v the rotation vector + * @return the new quaternion + **/ + fun fromRotationVector(v: Vector3D): QuaternionD = QuaternionD(0.0, v / 2.0).exp() + + /** + * creates a new quaternion representing the rotation about axis v + * by an angle of v's length + * @param vx the rotation vector's x component + * @param vy the rotation vector's y component + * @param vz the rotation vector's z component + * @return the new quaternion + **/ + fun fromRotationVector(vx: Double, vy: Double, vz: Double): QuaternionD = fromRotationVector(Vector3D(vx, vy, vz)) + + /** + * finds Q, the smallest-angled quaternion whose local u direction aligns with + * the global v direction. + * @param u the local direction + * @param v the global direction + * @return Q + **/ + fun fromTo(u: Vector3D, v: Vector3D): QuaternionD { + val u = QuaternionD(0.0, u) + val v = QuaternionD(0.0, v) + val d = v / u + + return (d + d.len()).unit() + } + + /** + * Rotation around X-axis + * + * Derived from the axis-angle representation in + * https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Unit_quaternions + */ + fun rotationAroundXAxis(angle: Double): QuaternionD = QuaternionD(cos(angle / 2.0), sin(angle / 2.0), 0.0, 0.0) + + /** + * Rotation around Y-axis + * + * Derived from the axis-angle representation in + * https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Unit_quaternions + */ + fun rotationAroundYAxis(angle: Double): QuaternionD = QuaternionD(cos(angle / 2.0), 0.0, sin(angle / 2.0), 0.0) + + /** + * Rotation around Z-axis + * + * Derived from the axis-angle representation in + * https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Unit_quaternions + */ + fun rotationAroundZAxis(angle: Double): QuaternionD = QuaternionD(cos(angle / 2.0), 0.0, 0.0, sin(angle / 2.0)) + + /** + * SlimeVR-specific constants and utils + */ + object SlimeVR { + val FRONT = QuaternionD(0.0, 0.0, 1.0, 0.0) + val FRONT_LEFT = QuaternionD(0.383, 0.0, 0.924, 0.0) + val LEFT = QuaternionD(0.707, 0.0, 0.707, 0.0) + val BACK_LEFT = QuaternionD(0.924, 0.0, 0.383, 0.0) + val FRONT_RIGHT = QuaternionD(0.383, 0.0, -0.924, 0.0) + val RIGHT = QuaternionD(0.707, 0.0, -0.707, 0.0) + val BACK_RIGHT = QuaternionD(0.924, 0.0, -0.383, 0.0) + val BACK = QuaternionD(1.0, 0.0, 0.0, 0.0) + } + } + + /** + * @return the quaternion with w real component and xyz imaginary components + */ + constructor(w: Double, xyz: Vector3D) : this(w, xyz.x, xyz.y, xyz.z) + + /** + * @return the imaginary components as a vector3 + **/ + val xyz get(): Vector3D = Vector3D(x, y, z) + + /** + * @return the quaternion with only the w component + **/ + val re get(): QuaternionD = QuaternionD(w, 0.0, 0.0, 0.0) + + /** + * @return the quaternion with only x y z components + **/ + val im get(): QuaternionD = QuaternionD(0.0, x, y, z) + + operator fun unaryMinus(): QuaternionD = QuaternionD(-w, -x, -y, -z) + + operator fun plus(that: QuaternionD): QuaternionD = QuaternionD( + this.w + that.w, + this.x + that.x, + this.y + that.y, + this.z + that.z, + ) + + operator fun plus(that: Double): QuaternionD = QuaternionD(this.w + that, this.x, this.y, this.z) + + operator fun minus(that: QuaternionD): QuaternionD = QuaternionD( + this.w - that.w, + this.x - that.x, + this.y - that.y, + this.z - that.z, + ) + + operator fun minus(that: Double): QuaternionD = QuaternionD(this.w - that, this.x, this.y, this.z) + + /** + * computes the dot product of this quaternion with that quaternion + * @param that the quaternion with which to be dotted + * @return the dot product between quaternions + **/ + fun dot(that: QuaternionD): Double = this.w * that.w + this.x * that.x + this.y * that.y + this.z * that.z + + /** + * computes the square of the length of this quaternion + * @return the length squared + **/ + fun lenSq(): Double = w * w + x * x + y * y + z * z + + /** + * computes the length of this quaternion + * @return the length + **/ + fun len(): Double = sqrt(w * w + x * x + y * y + z * z) + + /** + * @return the normalized quaternion + **/ + fun unit(): QuaternionD { + val m = len() + return if (m == 0.0) NULL else (this / m) + } + + operator fun times(that: Double): QuaternionD = QuaternionD( + this.w * that, + this.x * that, + this.y * that, + this.z * that, + ) + + operator fun times(that: QuaternionD): QuaternionD = QuaternionD( + this.w * that.w - this.x * that.x - this.y * that.y - this.z * that.z, + this.x * that.w + this.w * that.x - this.z * that.y + this.y * that.z, + this.y * that.w + this.z * that.x + this.w * that.y - this.x * that.z, + this.z * that.w - this.y * that.x + this.x * that.y + this.w * that.z, + ) + + /** + * computes the inverse of this quaternion + * @return the inverse quaternion + **/ + fun inv(): QuaternionD { + val lenSq = lenSq() + return QuaternionD( + w / lenSq, + -x / lenSq, + -y / lenSq, + -z / lenSq, + ) + } + + operator fun div(that: Double): QuaternionD = this * (1f / that) + + /** + * computes right division, this * that^-1 + **/ + operator fun div(that: QuaternionD): QuaternionD = this * that.inv() + + operator fun component1(): Double = w + operator fun component2(): Double = x + operator fun component3(): Double = y + operator fun component4(): Double = z + + /** + * @return the conjugate of this quaternion + **/ + fun conj(): QuaternionD = QuaternionD(w, -x, -y, -z) + + /** + * computes the logarithm of this quaternion + * @return the log of this quaternion + **/ + fun log(): QuaternionD { + val co = w + val si = xyz.len() + val len = len() + + if (si == 0.0) { + return QuaternionD(ln(len), xyz / w) + } + + val ang = atan2(si, co) + return QuaternionD(ln(len), ang / si * xyz) + } + + /** + * raises e to the power of this quaternion + * @return the exponentiated quaternion + **/ + fun exp(): QuaternionD { + val ang = xyz.len() + val len = exp(w) + + if (ang == 0.0) { + return QuaternionD(len, len * xyz) + } + + val co = cos(ang) + val si = sin(ang) + return QuaternionD(len * co, len * si / ang * xyz) + } + + /** + * raises this quaternion to the power of t + * @param t the power by which to raise this quaternion + * @return the powered quaternion + **/ + fun pow(t: Double): QuaternionD = (log() * t).exp() + + /** + * between this and -this, picks the one nearest to that quaternion + * @param that the quaternion to be nearest to + * @return nearest quaternion + **/ + fun twinNearest(that: QuaternionD): QuaternionD = if (this.dot(that) < 0.0) -this else this + + /** + * between this and -this, picks the one furthest from that quaternion + * @param that the quaternion to be furthest from + * @return furthest quaternion + **/ + fun twinFurthest(that: QuaternionD): QuaternionD = if (this.dot(that) < 0.0) this else -this + + /** + * Similar to [twinNearest], but offset so the lower back quadrant is the furthest + * rotation relative to [that]. This is useful for joints that have limited forward + * rotation, but extensive backward rotation. + * @param that The reference quaternion to be nearest to or furthest from. + * @return The furthest quaternion if in the lower back quadrant, otherwise the + * nearest quaternion. + **/ + fun twinExtendedBack(that: QuaternionD): QuaternionD { + /* + * This handles the thigh extending behind the torso to face downwards, and the + * hip extending behind the chest. The thigh cannot bend to the back away from + * the torso and the spine hopefully can't bend back that far, so we can fairly + * safely assume the rotation is towards the torso. + */ + return this.twinNearest(that * UP_ADJ) + } + + /** + * interpolates from this quaternion to that quaternion by t in quaternion space + * @param that the quaternion to interpolate to + * @param t the amount to interpolate + * @return interpolated quaternion + **/ + fun interpQ(that: QuaternionD, t: Double) = if (t == 0.0) { + this + } else if (t == 1.0) { + that + } else if (t < 0.5f) { + (that / this).pow(t) * this + } else { + (this / that).pow(1f - t) * that + } + + /** + * interpolates from this quaternion to that quaternion by t in rotation space + * @param that the quaternion to interpolate to + * @param t the amount to interpolate + * @return interpolated quaternion + **/ + fun interpR(that: QuaternionD, t: Double) = this.interpQ(that.twinNearest(this), t) + + /** + * linearly interpolates from this quaternion to that quaternion by t in + * quaternion space + * @param that the quaternion to interpolate to + * @param t the amount to interpolate + * @return interpolated quaternion + **/ + fun lerpQ(that: QuaternionD, t: Double): QuaternionD = (1f - t) * this + t * that + + /** + * linearly interpolates from this quaternion to that quaternion by t in + * rotation space + * @param that the quaternion to interpolate to + * @param t the amount to interpolate + * @return interpolated quaternion + **/ + fun lerpR(that: QuaternionD, t: Double) = this.lerpQ(that.twinNearest(this), t) + + /** + * computes this quaternion's angle to identity in quaternion space + * @return angle + **/ + fun angleQ(): Double = atan2(xyz.len(), w) + + /** + * computes this quaternion's angle to identity in rotation space + * @return angle + **/ + fun angleR(): Double = 2.0 * atan2(xyz.len(), abs(w)) + + /** + * computes the angle between this quaternion and that quaternion in quaternion space + * @param that the other quaternion + * @return angle + **/ + fun angleToQ(that: QuaternionD): Double = (this / that).angleQ() + + /** + * computes the angle between this quaternion and that quaternion in rotation space + * @param that the other quaternion + * @return angle + **/ + fun angleToR(that: QuaternionD): Double = (this / that).angleR() + + /** + * computes the angle this quaternion rotates about the u axis in quaternion space + * @param u the axis + * @return angle + **/ + fun angleAboutQ(u: Vector3D): Double { + val si = u.dot(xyz) + val co = u.len() * w + return atan2(si, co) + } + + /** + * computes the angle this quaternion rotates about the u axis in rotation space + * @param u the axis + * @return angle + **/ + fun angleAboutR(u: Vector3D): Double = 2.0 * twinNearest(IDENTITY).angleAboutQ(u) + + /** + * finds Q, the quaternion nearest to this quaternion representing a rotation purely + * about the global u axis. Q is NOT unitized + * @param v the global axis + * @return Q + **/ + fun project(v: Vector3D) = QuaternionD(w, xyz.dot(v) / v.lenSq() * v) + + /** + * finds Q, the quaternion nearest to this quaternion representing a rotation NOT + * on the global u axis. Q is NOT unitized + * @param v the global axis + * @return Q + **/ + fun reject(v: Vector3D) = QuaternionD(w, v.cross(xyz).cross(v) / v.lenSq()) + + /** + * finds Q, the quaternion nearest to this quaternion whose local u direction aligns + * with the global v direction. Q is NOT unitized + * @param u the local direction + * @param v the global direction + * @return Q + **/ + fun align(u: Vector3D, v: Vector3D): QuaternionD { + val u = QuaternionD(0.0, u) + val v = QuaternionD(0.0, v) + + return (v * this / u + (v / u).len() * this) / 2.0 + } + + /** + * Produces angles such that + * QuaternionD.fromRotationVector(angles[0]*axisA.unit()) * QuaternionD.fromRotationVector(angles[1]*axisB.unit()) + * is as close to rot as possible + */ + fun biAlign(rot: QuaternionD, axisA: Vector3D, axisB: Vector3D): DoubleArray { + val a = axisA.unit() + val b = axisB.unit() + + val aQ = a.dot(rot.xyz) + val bQ = b.dot(rot.xyz) + val abQ = a.cross(b).dot(rot.xyz) - a.dot(b) * rot.w + + val angleA = atan2(2.0 * (abQ * bQ + aQ * rot.w), rot.w * rot.w - aQ * aQ + bQ * bQ - abQ * abQ) + val angleB = atan2(2.0 * (abQ * aQ + bQ * rot.w), rot.w * rot.w + aQ * aQ - bQ * bQ - abQ * abQ) + + return doubleArrayOf(angleA, angleB) + } + + /** + * applies this quaternion's rotation to that vector + * @param that the vector to be transformed + * @return that vector transformed by this quaternion + **/ + fun sandwich(that: Vector3D): Vector3D = (this * QuaternionD(0.0, that) / this).xyz + + /** + * Sandwiches the unit X vector + * + * First column of rotation matrix in + * https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Conversion_to_and_from_the_matrix_representation + */ + fun sandwichUnitX(): Vector3D = Vector3D( + w * w + x * x - y * y - z * z, + 2.0 * (x * y + w * z), + 2.0 * (x * z - w * y), + ) + + /** + * Sandwiches the unit Y vector + * + * Second column of rotation matrix in + * https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Conversion_to_and_from_the_matrix_representation + */ + fun sandwichUnitY(): Vector3D = Vector3D( + 2.0 * (x * y - w * z), + w * w - x * x + y * y - z * z, + 2.0 * (y * z + w * x), + ) + + /** + * Sandwiches the unit Z vector + * + * Third column of rotation matrix in + * https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Conversion_to_and_from_the_matrix_representation + */ + fun sandwichUnitZ(): Vector3D = Vector3D( + 2.0 * (x * z + w * y), + 2.0 * (y * z - w * x), + w * w - x * x - y * y + z * z, + ) + + /** + * computes this quaternion's unit length rotation axis + * @return rotation axis + **/ + fun axis(): Vector3D = xyz.unit() + + /** + * computes the rotation vector representing this quaternion's rotation + * @return rotation vector + **/ + fun toRotationVector(): Vector3D = 2.0 * twinNearest(IDENTITY).log().xyz + + @Suppress("ktlint") + /** + * computes the matrix representing this quaternion's rotation + * @return rotation matrix + **/ + fun toMatrix(): Matrix3D { + val d = lenSq() + return Matrix3D( + (w*w + x*x - y*y - z*z)/d , 2.0*(x*y - w*z)/d , 2.0*(w*y + x*z)/d , + 2.0*(x*y + w*z)/d , (w*w - x*x + y*y - z*z)/d , 2.0*(y*z - w*x)/d , + 2.0*(x*z - w*y)/d , 2.0*(w*x + y*z)/d , (w*w - x*x - y*y + z*z)/d ) + } + + /** + * computes the euler angles representing this quaternion's rotation + * @param order the order in which to decompose this quaternion into euler angles + * @return euler angles + **/ + fun toEulerAngles(order: EulerOrder): EulerAnglesD = this.toMatrix().toEulerAnglesAssumingOrthonormal(order) + + fun toObject() = ObjectQuaternionD(w, x, y, z) + + fun toFloat() = Quaternion(w.toFloat(), x.toFloat(), y.toFloat(), z.toFloat()) +} + +data class ObjectQuaternionD(val w: Double, val x: Double, val y: Double, val z: Double) { + fun toValue() = QuaternionD(w, x, y, z) +} + +operator fun Double.plus(that: QuaternionD): QuaternionD = that + this +operator fun Double.minus(that: QuaternionD): QuaternionD = -that + this +operator fun Double.times(that: QuaternionD): QuaternionD = that * this +operator fun Double.div(that: QuaternionD): QuaternionD = that.inv() * this diff --git a/server/core/src/main/java/io/github/axisangles/ktmath/Vector2.kt b/server/core/src/main/java/io/github/axisangles/ktmath/Vector2.kt new file mode 100644 index 0000000000..8ef4be6535 --- /dev/null +++ b/server/core/src/main/java/io/github/axisangles/ktmath/Vector2.kt @@ -0,0 +1,75 @@ +@file:Suppress("unused") + +package io.github.axisangles.ktmath + +import kotlinx.serialization.Serializable +import kotlin.math.* + +@JvmInline +@Serializable +value class Vector2(val x: Float, val y: Float) { + companion object { + val NULL = Vector2(0f, 0f) + val POS_X = Vector2(1f, 0f) + val POS_Y = Vector2(0f, 1f) + val NEG_X = Vector2(-1f, 0f) + val NEG_Y = Vector2(0f, -1f) + } + + operator fun component1() = x + operator fun component2() = y + + operator fun unaryMinus() = Vector2(-x, -y) + + operator fun plus(that: Vector2) = Vector2( + this.x + that.x, + this.y + that.y, + ) + + operator fun minus(that: Vector2) = Vector2( + this.x - that.x, + this.y - that.y, + ) + + /** + * computes the dot product of this vector with that vector + * @param that the vector with which to be dotted + * @return the dot product + **/ + infix fun dot(that: Vector2) = this.x * that.x + this.y * that.y + + /** + * computes the square of the length of this vector + * @return the length squared + **/ + fun lenSq() = x * x + y * y + + /** + * computes the length of this vector + * @return the length + **/ + fun len() = sqrt(x * x + y * y) + + /** + * @return the normalized vector + **/ + fun unit(): Vector2 { + val m = len() + return if (m == 0f) NULL else this / m + } + + operator fun times(that: Float) = Vector2( + this.x * that, + this.y * that, + ) + + // computes division of this Vector2 by a float + operator fun div(that: Float) = Vector2( + this.x / that, + this.y / that, + ) + + fun isNear(other: Vector2, maxError: Float = 1e-6f) = abs(x - other.x) <= maxError && abs(y - other.y) <= maxError +} + +operator fun Float.times(that: Vector2): Vector2 = that * this diff --git a/server/core/src/main/java/io/github/axisangles/ktmath/Vector2D.kt b/server/core/src/main/java/io/github/axisangles/ktmath/Vector2D.kt new file mode 100644 index 0000000000..a2da36fc14 --- /dev/null +++ b/server/core/src/main/java/io/github/axisangles/ktmath/Vector2D.kt @@ -0,0 +1,77 @@ +@file:Suppress("unused") + +package io.github.axisangles.ktmath + +import kotlinx.serialization.Serializable +import kotlin.math.* + +@JvmInline +@Serializable +value class Vector2D(val x: Double, val y: Double) { + companion object { + val NULL = Vector2D(0.0, 0.0) + val POS_X = Vector2D(1.0, 0.0) + val POS_Y = Vector2D(0.0, 1.0) + val NEG_X = Vector2D(-1.0, 0.0) + val NEG_Y = Vector2D(0.0, -1.0) + } + + operator fun component1() = x + operator fun component2() = y + + operator fun unaryMinus() = Vector2D(-x, -y) + + operator fun plus(that: Vector2D) = Vector2D( + this.x + that.x, + this.y + that.y, + ) + + operator fun minus(that: Vector2D) = Vector2D( + this.x - that.x, + this.y - that.y, + ) + + /** + * computes the dot product of this vector with that vector + * @param that the vector with which to be dotted + * @return the dot product + **/ + infix fun dot(that: Vector2D) = this.x * that.x + this.y * that.y + + /** + * computes the square of the length of this vector + * @return the length squared + **/ + fun lenSq() = x * x + y * y + + /** + * computes the length of this vector + * @return the length + **/ + fun len() = sqrt(x * x + y * y) + + /** + * @return the normalized vector + **/ + fun unit(): Vector2D { + val m = len() + return if (m == 0.0) NULL else this / m + } + + operator fun times(that: Double) = Vector2D( + this.x * that, + this.y * that, + ) + + // computes division of this Vector2D by a Double + operator fun div(that: Double) = Vector2D( + this.x / that, + this.y / that, + ) + + fun angleTo(that: Vector2D): Double = atan2(that.y, that.x) - atan2(y, x) + + fun isNear(other: Vector2D, maxError: Double = 1e-6) = abs(x - other.x) <= maxError && abs(y - other.y) <= maxError +} + +operator fun Double.times(that: Vector2D): Vector2D = that * this diff --git a/server/core/src/main/java/io/github/axisangles/ktmath/Vector3.kt b/server/core/src/main/java/io/github/axisangles/ktmath/Vector3.kt index 1b1d8481f3..bc7a55c497 100644 --- a/server/core/src/main/java/io/github/axisangles/ktmath/Vector3.kt +++ b/server/core/src/main/java/io/github/axisangles/ktmath/Vector3.kt @@ -101,6 +101,8 @@ value class Vector3(val x: Float, val y: Float, val z: Float) { fun angleTo(that: Vector3): Float = atan2(this.cross(that).len(), this.dot(that)) fun isNear(other: Vector3, maxError: Float = 1e-6f) = abs(x - other.x) <= maxError && abs(y - other.y) <= maxError && abs(z - other.z) <= maxError + + fun toDouble() = Vector3D(x.toDouble(), y.toDouble(), z.toDouble()) } operator fun Float.times(that: Vector3): Vector3 = that * this diff --git a/server/core/src/main/java/io/github/axisangles/ktmath/Vector3D.kt b/server/core/src/main/java/io/github/axisangles/ktmath/Vector3D.kt new file mode 100644 index 0000000000..bceccdb916 --- /dev/null +++ b/server/core/src/main/java/io/github/axisangles/ktmath/Vector3D.kt @@ -0,0 +1,110 @@ +@file:Suppress("unused") + +package io.github.axisangles.ktmath + +import kotlinx.serialization.Serializable +import kotlin.math.* + +@JvmInline +@Serializable +value class Vector3D(val x: Double, val y: Double, val z: Double) { + companion object { + val NULL = Vector3D(0.0, 0.0, 0.0) + val POS_X = Vector3D(1.0, 0.0, 0.0) + val POS_Y = Vector3D(0.0, 1.0, 0.0) + val POS_Z = Vector3D(0.0, 0.0, 1.0) + val NEG_X = Vector3D(-1.0, 0.0, 0.0) + val NEG_Y = Vector3D(0.0, -1.0, 0.0) + val NEG_Z = Vector3D(0.0, 0.0, -1.0) + } + + operator fun component1() = x + operator fun component2() = y + operator fun component3() = z + + operator fun unaryMinus() = Vector3D(-x, -y, -z) + + operator fun plus(that: Vector3D) = Vector3D( + this.x + that.x, + this.y + that.y, + this.z + that.z, + ) + + operator fun minus(that: Vector3D) = Vector3D( + this.x - that.x, + this.y - that.y, + this.z - that.z, + ) + + /** + * computes the dot product of this vector with that vector + * @param that the vector with which to be dotted + * @return the dot product + **/ + infix fun dot(that: Vector3D) = this.x * that.x + this.y * that.y + this.z * that.z + + /** + * computes the cross product of this vector with that vector + * @param that the vector with which to be crossed + * @return the cross product + **/ + infix fun cross(that: Vector3D) = Vector3D( + this.y * that.z - this.z * that.y, + this.z * that.x - this.x * that.z, + this.x * that.y - this.y * that.x, + ) + + infix fun hadamard(that: Vector3D) = Vector3D( + this.x * that.x, + this.y * that.y, + this.z * that.z, + ) + + /** + * computes the square of the length of this vector + * @return the length squared + **/ + fun lenSq() = x * x + y * y + z * z + + /** + * computes the length of this vector + * @return the length + **/ + fun len() = sqrt(x * x + y * y + z * z) + + /** + * @return the normalized vector + **/ + fun unit(): Vector3D { + val m = len() + return if (m == 0.0) NULL else this / m + } + + operator fun times(that: Double) = Vector3D( + this.x * that, + this.y * that, + this.z * that, + ) + + // computes division of this Vector3D by a Double + operator fun div(that: Double) = Vector3D( + this.x / that, + this.y / that, + this.z / that, + ) + + /** + * computes the angle between this vector with that vector + * @param that the vector to which the angle is computed + * @return the angle + **/ + fun angleTo(that: Vector3D): Double = atan2(this.cross(that).len(), this.dot(that)) + + fun isNear(other: Vector3D, maxError: Double = 1e-6) = abs(x - other.x) <= maxError && abs(y - other.y) <= maxError && abs(z - other.z) <= maxError + + fun toFloat() = Vector3(x.toFloat(), y.toFloat(), z.toFloat()) + + override fun toString(): String = "Vector3D(x=$x y=$y z=$z)" +} + +operator fun Double.times(that: Vector3D): Vector3D = that * this diff --git a/server/core/src/test/java/dev/slimevr/unit/CameraTests.kt b/server/core/src/test/java/dev/slimevr/unit/CameraTests.kt new file mode 100644 index 0000000000..027cad6f1a --- /dev/null +++ b/server/core/src/test/java/dev/slimevr/unit/CameraTests.kt @@ -0,0 +1,33 @@ +package dev.slimevr.unit + +import dev.slimevr.tracking.videocalibration.data.Camera +import dev.slimevr.tracking.videocalibration.data.CameraExtrinsic +import dev.slimevr.tracking.videocalibration.data.CameraIntrinsic +import io.github.axisangles.ktmath.QuaternionD +import io.github.axisangles.ktmath.Vector2D +import io.github.axisangles.ktmath.Vector3D +import java.awt.Dimension +import kotlin.test.Test +import kotlin.test.assertTrue + +class CameraTests { + + @Test + fun testProject() { + val camera = + Camera( + CameraExtrinsic.fromCameraPose(QuaternionD.IDENTITY, Vector3D(0.0, 0.0, -3.0)), + CameraIntrinsic(1000.0, 1000.0, 500.0, 500.0), + Dimension(1000, 1000), + ) + + val p0 = Vector3D(0.0, 0.0, 0.0) + assertTrue(camera.project(p0)!!.isNear(Vector2D(500.0, 500.0))) + + val p1 = Vector3D(1.0, 0.0, 0.0) + assertTrue(camera.project(p1)!!.isNear(Vector2D(500.0 + 1000.0 / 3.0, 500.0))) + + val p2 = Vector3D(1.0, 1.0, 1.0) + assertTrue(camera.project(p2)!!.isNear(Vector2D(500.0 + 1000.0 / 4.0, 500.0 + 1000.0 / 4.0))) + } +} diff --git a/server/core/src/test/java/dev/slimevr/unit/VideoCalibrationServiceTests.kt b/server/core/src/test/java/dev/slimevr/unit/VideoCalibrationServiceTests.kt new file mode 100644 index 0000000000..7fa1d8101b --- /dev/null +++ b/server/core/src/test/java/dev/slimevr/unit/VideoCalibrationServiceTests.kt @@ -0,0 +1,220 @@ +package dev.slimevr.unit + +import dev.slimevr.poseframeformat.PfsIO +import dev.slimevr.poseframeformat.PoseFrames +import dev.slimevr.protocol.ConnectionContext +import dev.slimevr.protocol.GenericConnection +import dev.slimevr.tracking.processor.config.SkeletonConfigManager +import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets +import dev.slimevr.tracking.trackers.Tracker +import dev.slimevr.tracking.trackers.TrackerPosition +import dev.slimevr.tracking.videocalibration.data.Camera +import dev.slimevr.tracking.videocalibration.data.CameraExtrinsic +import dev.slimevr.tracking.videocalibration.data.CameraIntrinsic +import dev.slimevr.tracking.videocalibration.networking.WebRTCManager +import dev.slimevr.tracking.videocalibration.snapshots.ImageSnapshot +import dev.slimevr.tracking.videocalibration.snapshots.TrackerSnapshot +import dev.slimevr.tracking.videocalibration.snapshots.TrackersSnapshot +import dev.slimevr.tracking.videocalibration.sources.HumanPoseSource +import dev.slimevr.tracking.videocalibration.sources.SnapshotsDatabase +import dev.slimevr.tracking.videocalibration.steps.Step +import dev.slimevr.tracking.videocalibration.steps.VideoCalibrator +import dev.slimevr.tracking.videocalibration.util.DebugOutput +import io.eiren.util.logging.LogManager +import io.github.axisangles.ktmath.QuaternionD +import io.github.axisangles.ktmath.Vector3D +import kotlinx.coroutines.CoroutineScope +import kotlinx.coroutines.Dispatchers +import kotlinx.coroutines.channels.Channel +import kotlinx.coroutines.delay +import kotlinx.coroutines.launch +import java.awt.Dimension +import java.nio.ByteBuffer +import java.util.UUID +import javax.imageio.ImageIO +import kotlin.io.path.Path +import kotlin.io.path.listDirectoryEntries +import kotlin.test.Test +import kotlin.test.assertEquals +import kotlin.time.Duration.Companion.milliseconds +import kotlin.time.Duration.Companion.seconds +import kotlin.time.TimeSource + +class VideoCalibrationServiceTests { + + @Test + fun e2eCalibration() { + val dataFolder = Path("C:\\Users\\yilan\\AppData\\Roaming\\dev.slimevr.SlimeVR\\VideoCalibrationTest19") + + val imageSnapshots = Channel(Channel.CONFLATED) + val trackersSnapshots = Channel(Channel.UNLIMITED) + + val trackersFPS = 120 + + val debugOutput = DebugOutput(Path("C:\\SlimeVR\\VideoCalibrationTest"), detailed = true) + + val webRTCManager = WebRTCManager() + + val humanPoseSource = HumanPoseSource(imageSnapshots, webRTCManager, debugOutput) + humanPoseSource.start() + + val database = + SnapshotsDatabase( + 2.seconds / trackersFPS.toDouble(), + humanPoseSource.humanPoseSnapshots, + trackersSnapshots, + ) + + val trackerPositions = setOf( + TrackerPosition.CHEST, + TrackerPosition.HIP, + TrackerPosition.LEFT_UPPER_LEG, + TrackerPosition.LEFT_LOWER_LEG, + TrackerPosition.RIGHT_UPPER_LEG, + TrackerPosition.RIGHT_LOWER_LEG, +// TrackerPosition.LEFT_UPPER_ARM, +// TrackerPosition.RIGHT_UPPER_ARM, // FIXME: Data may not contain these positions + ) + + val trackersToReset = + trackerPositions + .associateWith { + Tracker( + device = null, + id = 0, + name = it.toString(), + displayName = it.toString(), + trackerPosition = it, + ) + } + + val websocket = object : GenericConnection { + override val connectionId = UUID.randomUUID() + + override val context: ConnectionContext + get() = TODO("Not yet implemented") + + override fun send(bytes: ByteBuffer) { + LogManager.debug("Sending progress update...") + } + } + + val skeletonConfigManager = SkeletonConfigManager(false, null) + skeletonConfigManager.setOffset(SkeletonConfigOffsets.HIPS_WIDTH, 0.32f) + skeletonConfigManager.setOffset(SkeletonConfigOffsets.UPPER_LEG, 0.52f) + skeletonConfigManager.setOffset(SkeletonConfigOffsets.LOWER_LEG, 0.50f) + + val webcamFolder = dataFolder.resolve("1_webcam") + webcamFolder.toFile().copyRecursively(debugOutput.webcamDir.toFile(), true) + + val videoCalibrator = + VideoCalibrator( + trackersToReset, + skeletonConfigManager, + database, + websocket, + debugOutput, + ) + + videoCalibrator.start() + + val cameraParams = dataFolder.resolve("camera.txt").toFile().readLines() + + val camera = Camera( + CameraExtrinsic.fromCameraPose( + QuaternionD(cameraParams[0].toDouble(), cameraParams[1].toDouble(), cameraParams[2].toDouble(), cameraParams[3].toDouble()), + Vector3D(cameraParams[4].toDouble(), cameraParams[5].toDouble(), cameraParams[6].toDouble()), + ), + CameraIntrinsic(cameraParams[7].toDouble(), cameraParams[8].toDouble(), cameraParams[9].toDouble(), cameraParams[10].toDouble()), + Dimension(cameraParams[11].toInt(), cameraParams[12].toInt()), + ) + + CoroutineScope(Dispatchers.IO).launch { + val startTime = TimeSource.Monotonic.markNow() + + val imagePaths = webcamFolder.listDirectoryEntries().sorted().toList() + val regex = Regex("""webcam_(\d+)\.jpg""") + + for (imagePath in imagePaths) { + val timeOffset = regex.matchEntire(imagePath.fileName.toString()) + ?.groupValues?.get(1) + ?.toInt() + ?.milliseconds + + if (timeOffset == null) { + continue + } + + val now = TimeSource.Monotonic.markNow() + val toDelay = timeOffset - (now - startTime) + if (toDelay.isPositive()) { + delay(toDelay) + } + + val image = ImageIO.read(imagePath.toFile()) + val snapshot = ImageSnapshot(now, timeOffset, image, camera) + imageSnapshots.trySend(snapshot) + } + } + + CoroutineScope(Dispatchers.IO).launch { + val startTime = TimeSource.Monotonic.markNow() + val trackersInterval = 1.seconds / trackersFPS + + val poseFrames = PfsIO.readFromFile(dataFolder.resolve("trackers.pfs").toFile()) + + var frameIndex = 0 + while (true) { + val trackers = buildTrackersSnapshot(poseFrames, frameIndex) + if (trackers.isEmpty()) { + break + } + + val now = TimeSource.Monotonic.markNow() + + val trackersSnapshot = TrackersSnapshot(now, trackers) + trackersSnapshots.trySend(trackersSnapshot) + + val nextTrackersTime = startTime + trackersInterval * frameIndex + val toDelay = nextTrackersTime - now + ++frameIndex + + if (toDelay.isPositive()) { + delay(toDelay) + } + } + } + + while (true) { + val step = videoCalibrator.step.get() + if (step == Step.DONE) { + break + } + + Thread.sleep(1000) + } + + assertEquals(Step.DONE, videoCalibrator.step.get()) + } + + private fun buildTrackersSnapshot(poseFrames: PoseFrames, frameIndex: Int): Map { + val map = mutableMapOf() + + for (holder in poseFrames.frameHolders) { + if (frameIndex >= holder.frames.size) continue + val trackerFrame = holder.frames[frameIndex] ?: continue + val position = trackerFrame.trackerPosition ?: continue + + map[position] = + TrackerSnapshot( + trackerFrame.rawRotation?.toDouble() + ?: error("Missing raw rotation"), + trackerFrame.rotation?.toDouble() + ?: error("Missing adjusted rotation"), + trackerFrame.position?.toDouble(), + ) + } + + return map + } +} diff --git a/server/desktop/build.gradle.kts b/server/desktop/build.gradle.kts index 1950bdbcc3..34aef5bab9 100644 --- a/server/desktop/build.gradle.kts +++ b/server/desktop/build.gradle.kts @@ -74,6 +74,9 @@ tasks.shadowJar { exclude(dependency("com.fazecast:jSerialComm:.*")) exclude(dependency("net.java.dev.jna:.*:.*")) exclude(dependency("com.google.flatbuffers:flatbuffers-java:.*")) + exclude(dependency("dev.onvoid.webrtc:webrtc-java:.*")) + exclude(dependency("io.ktor:ktor-serialization-kotlinx-.*:.*")) + exclude(dependency("org.jetbrains.kotlinx:kotlinx-serialization-.*:.*")) exclude(project(":solarxr-protocol")) } diff --git a/solarxr-protocol b/solarxr-protocol index fa2895b19a..6cd5a5908c 160000 --- a/solarxr-protocol +++ b/solarxr-protocol @@ -1 +1 @@ -Subproject commit fa2895b19a53d9b1686de8c2a6efe2b3e9ca4fc6 +Subproject commit 6cd5a5908c4433d1e673529be4b112a0ecdb503f