Mimir/src/Mimir.Runtime/Synchronization/MimirMoveFusion.cs
2026-07-14 00:35:47 +01:00

310 lines
13 KiB
C#

using System.Numerics;
using MessagePack;
namespace Mimir.Runtime.Synchronization;
[MessagePackObject]
public sealed record MimirMoveFusionCameraCalibration(
[property: Key(0)] string CameraId,
[property: Key(1)] ulong WitnessIdHash,
[property: Key(2)] MimirVector3Snapshot PositionMeters,
[property: Key(3)] MimirQuaternionSnapshot Orientation,
[property: Key(4)] double FocalLengthXPx,
[property: Key(5)] double FocalLengthYPx,
[property: Key(6)] double PrincipalPointXPx,
[property: Key(7)] double PrincipalPointYPx);
[MessagePackObject]
public sealed record MimirMoveFusionRigCalibration(
[property: Key(0)] string CalibrationId,
[property: Key(1)] string TrackingSpaceId,
[property: Key(2)] IReadOnlyList<MimirMoveFusionCameraCalibration> Cameras,
[property: Key(3)] double GyroUnitsPerRadianPerSecond = 1.0,
[property: Key(4)] double MaximumAssociationSkewMilliseconds = 20.0,
[property: Key(5)] double SingleRayFallbackDepthMeters = 1.5);
public sealed record MimirMoveFusionResult(
IReadOnlyList<MimirMoveControllerPoseDocument> Poses,
int ControllerEvidenceCount,
int OpticalEvidenceCount,
int CalibratedOpticalEvidenceCount);
public static class MimirMoveFusion
{
public static MimirMoveFusionResult Fuse(
IReadOnlyList<MimirNativeMoveEvidenceSample> samples,
MimirMoveFusionRigCalibration calibration,
string fusionAuthorityId = "mimir.runtime.move-fusion",
string consumerContract = "fensalir.move-controller-input")
{
ArgumentNullException.ThrowIfNull(samples);
ArgumentNullException.ThrowIfNull(calibration);
ArgumentException.ThrowIfNullOrWhiteSpace(calibration.CalibrationId);
ArgumentException.ThrowIfNullOrWhiteSpace(calibration.TrackingSpaceId);
var controllerStates = samples
.Where(sample => sample.EvidenceKind == (uint)MimirNativeMoveEvidenceKind.ControllerState &&
sample.ControllerIdHash != 0)
.OrderBy(sample => sample.SourceTimestampNs)
.ThenBy(sample => sample.Sequence)
.ToArray();
var opticalMarkers = samples
.Where(sample => sample.EvidenceKind == (uint)MimirNativeMoveEvidenceKind.OpticalMarker)
.OrderBy(sample => sample.SourceTimestampNs)
.ThenBy(sample => sample.Sequence)
.ToArray();
var cameras = calibration.Cameras
.Where(IsCameraUsable)
.ToDictionary(camera => camera.WitnessIdHash, camera => camera);
var maxSkewNs = (ulong)Math.Max(0.0, calibration.MaximumAssociationSkewMilliseconds * 1_000_000.0);
var poses = new List<MimirMoveControllerPoseDocument>();
foreach (var controller in controllerStates)
{
var associatedMarkers = opticalMarkers
.Where(marker => IsAssociated(marker, controller, controllerStates.Length, maxSkewNs))
.Select(marker => (Marker: marker, Camera: cameras.GetValueOrDefault(marker.WitnessIdHash)))
.Where(pair => pair.Camera is not null)
.ToArray();
if (associatedMarkers.Length == 0)
{
continue;
}
var rays = associatedMarkers
.Select(pair => BuildRay(pair.Camera!, pair.Marker))
.Where(ray => ray.HasValue)
.Select(ray => ray!.Value)
.ToArray();
if (rays.Length == 0)
{
continue;
}
var position = rays.Length >= 2
? ClosestPoint(rays)
: rays[0].Origin + rays[0].Direction * (float)Math.Max(0.05, calibration.SingleRayFallbackDepthMeters);
var opticalConfidence = Math.Clamp(
associatedMarkers.Average(pair => pair.Marker.Confidence) * (rays.Length >= 2 ? 1.0 : 0.35),
0.0,
1.0);
var confidence = Math.Clamp(opticalConfidence * ControllerConfidence(controller), 0.0, 1.0);
var sourceNs = Math.Max(controller.SourceTimestampNs, associatedMarkers.Max(pair => pair.Marker.SourceTimestampNs));
var arrivalNs = Math.Max(controller.ArrivalNs, associatedMarkers.Max(pair => pair.Marker.ArrivalNs));
var evidenceStreamIds = associatedMarkers
.Select(pair => $"witness:0x{pair.Marker.WitnessIdHash:X16}")
.Append($"controller:0x{controller.ControllerIdHash:X16}")
.Distinct(StringComparer.Ordinal)
.Order(StringComparer.Ordinal)
.ToArray();
var evidenceKinds = new[]
{
rays.Length >= 2 ? "optical-marker:triangulated" : "optical-marker:single-ray-depth-prior",
"controller-state:buttons-imu",
"orientation:imu-unresolved"
};
poses.Add(new MimirMoveControllerPoseDocument(
PoseId: $"move:0x{controller.ControllerIdHash:X16}:{controller.Sequence}",
WandId: $"move:0x{controller.ControllerIdHash:X16}",
TrackingSpaceId: calibration.TrackingSpaceId,
CalibrationId: calibration.CalibrationId,
FusionAuthorityId: fusionAuthorityId,
SourceTimestampNs: checked((long)Math.Min(sourceNs, (ulong)long.MaxValue)),
EstimatedAtNs: checked((long)Math.Min(arrivalNs, (ulong)long.MaxValue)),
Sequence: controller.Sequence,
PositionMeters: ToSnapshot(position),
Orientation: new MimirQuaternionSnapshot(0.0, 0.0, 0.0, 1.0),
LinearVelocityMetersPerSecond: new MimirVector3Snapshot(0.0, 0.0, 0.0),
AngularVelocityRadiansPerSecond: new MimirVector3Snapshot(
ScaleGyro(controller.GyroX, calibration),
ScaleGyro(controller.GyroY, calibration),
ScaleGyro(controller.GyroZ, calibration)),
Confidence: confidence,
LatencyMilliseconds: arrivalNs >= sourceNs ? (arrivalNs - sourceNs) / 1_000_000.0 : 0.0,
Battery01: float.IsFinite(controller.Battery01) ? Math.Clamp(controller.Battery01, 0.0f, 1.0f) : double.NaN,
Buttons: ButtonsFromMask(controller.ButtonsMask, controller.Trigger),
EvidenceStreamIds: evidenceStreamIds,
EvidenceKinds: evidenceKinds,
ConsumerContract: consumerContract));
}
return new MimirMoveFusionResult(
poses,
controllerStates.Length,
opticalMarkers.Length,
opticalMarkers.Count(marker => cameras.ContainsKey(marker.WitnessIdHash)));
}
private static bool IsAssociated(
MimirNativeMoveEvidenceSample marker,
MimirNativeMoveEvidenceSample controller,
int controllerCount,
ulong maxSkewNs)
{
if (marker.ControllerIdHash != 0 && marker.ControllerIdHash != controller.ControllerIdHash)
{
return false;
}
if (marker.ControllerIdHash == 0 && controllerCount != 1)
{
return false;
}
return TimestampDelta(marker.SourceTimestampNs, controller.SourceTimestampNs) <= maxSkewNs;
}
private static bool IsCameraUsable(MimirMoveFusionCameraCalibration camera) =>
camera.WitnessIdHash != 0 &&
camera.FocalLengthXPx > 0.0 &&
camera.FocalLengthYPx > 0.0;
private static MimirRay? BuildRay(MimirMoveFusionCameraCalibration camera, MimirNativeMoveEvidenceSample marker)
{
if (!float.IsFinite(marker.ImageX) || !float.IsFinite(marker.ImageY))
{
return null;
}
var directionCamera = Vector3.Normalize(new Vector3(
(float)((marker.ImageX - camera.PrincipalPointXPx) / camera.FocalLengthXPx),
(float)((camera.PrincipalPointYPx - marker.ImageY) / camera.FocalLengthYPx),
1.0f));
var orientation = ToQuaternion(camera.Orientation);
return new MimirRay(
ToVector(camera.PositionMeters),
Vector3.Normalize(Vector3.Transform(directionCamera, orientation)));
}
private static Vector3 ClosestPoint(IReadOnlyList<MimirRay> rays)
{
var matrix = new Matrix3();
var rhs = Vector3.Zero;
foreach (var ray in rays)
{
var d = Vector3.Normalize(ray.Direction);
var xx = 1.0f - d.X * d.X;
var yy = 1.0f - d.Y * d.Y;
var zz = 1.0f - d.Z * d.Z;
var xy = -d.X * d.Y;
var xz = -d.X * d.Z;
var yz = -d.Y * d.Z;
matrix.M11 += xx;
matrix.M12 += xy;
matrix.M13 += xz;
matrix.M21 += xy;
matrix.M22 += yy;
matrix.M23 += yz;
matrix.M31 += xz;
matrix.M32 += yz;
matrix.M33 += zz;
rhs += new Vector3(
xx * ray.Origin.X + xy * ray.Origin.Y + xz * ray.Origin.Z,
xy * ray.Origin.X + yy * ray.Origin.Y + yz * ray.Origin.Z,
xz * ray.Origin.X + yz * ray.Origin.Y + zz * ray.Origin.Z);
}
return matrix.TrySolve(rhs, out var solved)
? solved
: rays[0].Origin + rays[0].Direction;
}
private static double ControllerConfidence(MimirNativeMoveEvidenceSample controller) =>
Math.Clamp(
0.75 +
(float.IsFinite(controller.Battery01) ? 0.10 : 0.0) +
(controller.ButtonsMask != 0 ? 0.05 : 0.0) +
(Math.Abs(controller.AccelX) + Math.Abs(controller.AccelY) + Math.Abs(controller.AccelZ) > 0.001 ? 0.10 : 0.0),
0.0,
1.0);
private static MimirTrackingButtonSnapshot[] ButtonsFromMask(uint mask, float trigger)
{
var names = new[]
{
"select", "l3", "r3", "start", "up", "right", "down", "left",
"l2", "r2", "l1", "r1", "triangle", "circle", "cross", "square",
"ps", "move", "trigger"
};
var buttons = new List<MimirTrackingButtonSnapshot>();
for (var bit = 0; bit < names.Length; bit++)
{
var pressed = (mask & (1u << bit)) != 0;
if (pressed || names[bit] == "trigger")
{
buttons.Add(new MimirTrackingButtonSnapshot(
names[bit],
pressed,
names[bit] == "trigger" ? Math.Clamp(trigger, 0.0f, 1.0f) : pressed ? 1.0 : 0.0));
}
}
return buttons.ToArray();
}
private static ulong TimestampDelta(ulong left, ulong right) => left >= right ? left - right : right - left;
private static double ScaleGyro(float value, MimirMoveFusionRigCalibration calibration) =>
calibration.GyroUnitsPerRadianPerSecond <= 0.0
? 0.0
: value / calibration.GyroUnitsPerRadianPerSecond;
private static Vector3 ToVector(MimirVector3Snapshot value) => new((float)value.X, (float)value.Y, (float)value.Z);
private static MimirVector3Snapshot ToSnapshot(Vector3 value) => new(value.X, value.Y, value.Z);
private static Quaternion ToQuaternion(MimirQuaternionSnapshot value)
{
var q = new Quaternion((float)value.X, (float)value.Y, (float)value.Z, (float)value.W);
return q.LengthSquared() <= 1.0e-12f ? Quaternion.Identity : Quaternion.Normalize(q);
}
private readonly record struct MimirRay(Vector3 Origin, Vector3 Direction);
private struct Matrix3
{
public float M11;
public float M12;
public float M13;
public float M21;
public float M22;
public float M23;
public float M31;
public float M32;
public float M33;
public readonly bool TrySolve(Vector3 rhs, out Vector3 value)
{
var det = Determinant(
M11, M12, M13,
M21, M22, M23,
M31, M32, M33);
if (Math.Abs(det) <= 1.0e-8f)
{
value = default;
return false;
}
value = new Vector3(
Determinant(rhs.X, M12, M13, rhs.Y, M22, M23, rhs.Z, M32, M33) / det,
Determinant(M11, rhs.X, M13, M21, rhs.Y, M23, M31, rhs.Z, M33) / det,
Determinant(M11, M12, rhs.X, M21, M22, rhs.Y, M31, M32, rhs.Z) / det);
return true;
}
private static float Determinant(
float m11,
float m12,
float m13,
float m21,
float m22,
float m23,
float m31,
float m32,
float m33) =>
m11 * (m22 * m33 - m23 * m32) -
m12 * (m21 * m33 - m23 * m31) +
m13 * (m21 * m32 - m22 * m31);
}
}