Add robust Move sphere stereo fitting
This commit is contained in:
Meta Vens
parent
c01ef5096c
commit
58af018125
7 changed files with 632 additions and 14 deletions
|
|
@ -78,6 +78,23 @@ sitting around the Move.
|
|||
- `mimir.move_calibration_receipt.v1`: sample counts, residuals, rejected
|
||||
phases, and promotion decision.
|
||||
|
||||
## Optical Sphere Fit Authority
|
||||
|
||||
`MimirMoveSphereCalibrationFitter` owns the bounded two-Eye optical fit after a
|
||||
collection session reaches `Fitting`. Eye 0 defines the tracking-space gauge.
|
||||
The fitter jointly estimates both focal lengths and Eye 1's rigid pose from
|
||||
same-frame, same-Move correspondences, using a deterministic 80/20 training and
|
||||
held-out partition. A robust training consensus may reject contaminated marker
|
||||
pairs, but promotion also requires at least 65 percent of the untouched
|
||||
held-out pairs to agree and must pass the session's median and P95 reprojection
|
||||
limits.
|
||||
|
||||
The physical orb radius is an explicit input; it is never guessed by the
|
||||
solver. PSMoveAPI's reported blob radius is a color-segmentation result, not a
|
||||
calibrated sphere silhouette. It may seed or weight depth only after the
|
||||
per-camera radius response has been validated. A fresh PSMoveAPI position age
|
||||
is also not optical confidence and must not be interpreted as one.
|
||||
|
||||
## Preflight Command
|
||||
|
||||
```powershell
|
||||
|
|
|
|||
|
|
@ -198,6 +198,19 @@ Get-Content .\state\evidence.jsonl -Tail 8
|
|||
|
||||
## Immediate Re-entry Instruction
|
||||
|
||||
The 2026-07-14 live two-wand session at
|
||||
`artifacts/move-calibration/live-two-wand-20260714-004210.*` passed collection
|
||||
coverage with 1,168 admitted same-frame pairs. The new Mimir-owned sphere
|
||||
stereo fitter recovers synthetic 505/535 px focal lengths within 0.43 px and a
|
||||
34 cm baseline within 0.22 mm, with 0.34/0.63 px held-out median/P95 error.
|
||||
The live artifact remains rejected: after robust radius admission its consensus
|
||||
fits at 0.51/2.08 px, but only 53.8 percent of held-out pairs agree and Eye 1's
|
||||
focal estimate leaves the physical range. PSMoveAPI age is currently projected
|
||||
as marker confidence by Muninn, so fresh wrong-color matches arrive near 0.996
|
||||
confidence. Repair source-local observation quality/continuity and calibrate
|
||||
per-Eye blob-radius response before asking the operator for another sweep. Do
|
||||
not promote the nominal 22.5 mm diagnostic fit.
|
||||
|
||||
The active priority is Nightwing Move calibration. One public Muninn observer
|
||||
owns two private Eye workers and one aggregate evidence stream. Odin commits
|
||||
`39c839a`, `a340c33`, `8aefd1c`, and `f82ca7a` give daemon health a persistent
|
||||
|
|
|
|||
|
|
@ -175,6 +175,22 @@ if (args.Any(arg => string.Equals(arg, "--sensor-calibration-session-live", Stri
|
|||
"move-0006f523e2d1", "move-000704a39772", "move-000704a6be5f", "move-000704a800d0"]));
|
||||
}
|
||||
|
||||
if (args.Any(arg => string.Equals(arg, "--sensor-calibration-fit", StringComparison.OrdinalIgnoreCase)))
|
||||
{
|
||||
return RunSensorCalibrationFit(
|
||||
ParseStringOption(args, "--session", "artifacts/move-calibration/live-session.session.mpack"),
|
||||
ParseStringOption(args, "--evidence", "artifacts/move-calibration/live-session.evidence.mpack"),
|
||||
ParseStringOption(args, "--output", "artifacts/move-calibration/live-session.calibration-receipt.mpack"),
|
||||
ParseIntOption(args, "--image-width", 640),
|
||||
ParseIntOption(args, "--image-height", 480),
|
||||
ParseDoubleOption(args, "--orb-radius-meters", 0.0));
|
||||
}
|
||||
|
||||
if (args.Any(arg => string.Equals(arg, "--sensor-calibration-fit-smoke", StringComparison.OrdinalIgnoreCase)))
|
||||
{
|
||||
return RunSensorCalibrationFitSmoke();
|
||||
}
|
||||
|
||||
if (args.Any(arg => string.Equals(arg, "--move-fusion-smoke", StringComparison.OrdinalIgnoreCase)))
|
||||
{
|
||||
return RunMoveFusionSmoke();
|
||||
|
|
@ -2319,6 +2335,172 @@ static int RunMoveStereoCalibrationAssessment(
|
|||
return assessment.Promoted ? 0 : 2;
|
||||
}
|
||||
|
||||
static int RunSensorCalibrationFit(
|
||||
string sessionPath,
|
||||
string evidencePath,
|
||||
string outputPath,
|
||||
int imageWidth,
|
||||
int imageHeight,
|
||||
double orbRadiusMeters)
|
||||
{
|
||||
if (!File.Exists(sessionPath) || !File.Exists(evidencePath))
|
||||
{
|
||||
Console.Error.WriteLine($"sensor-calibration-fit missing-input session={sessionPath} evidence={evidencePath}");
|
||||
return 1;
|
||||
}
|
||||
|
||||
var session = MessagePackSerializer.Deserialize<MimirSensorCalibrationSessionDocument>(File.ReadAllBytes(sessionPath));
|
||||
var evidence = MessagePackSerializer.Deserialize<MoveVisibilityWindowReceipt>(File.ReadAllBytes(evidencePath));
|
||||
var receipt = MimirMoveSphereCalibrationFitter.Fit(session, evidence, imageWidth, imageHeight, orbRadiusMeters);
|
||||
var observationStats = evidence.Observations
|
||||
.GroupBy(value => value.CameraId, StringComparer.Ordinal)
|
||||
.Select(group =>
|
||||
{
|
||||
var radii = group.Where(value => float.IsFinite(value.RadiusPx) && value.RadiusPx > 0).Select(value => (double)value.RadiusPx).Order().ToArray();
|
||||
var confidence = group.Where(value => float.IsFinite(value.Confidence)).Select(value => (double)value.Confidence).Order().ToArray();
|
||||
return new
|
||||
{
|
||||
CameraId = group.Key,
|
||||
Count = group.Count(),
|
||||
RadiusP01 = CalibrationCliPercentile(radii, 0.01),
|
||||
RadiusP10 = CalibrationCliPercentile(radii, 0.10),
|
||||
RadiusP50 = CalibrationCliPercentile(radii, 0.50),
|
||||
RadiusP90 = CalibrationCliPercentile(radii, 0.90),
|
||||
RadiusP99 = CalibrationCliPercentile(radii, 0.99),
|
||||
ConfidenceP10 = CalibrationCliPercentile(confidence, 0.10),
|
||||
ConfidenceP50 = CalibrationCliPercentile(confidence, 0.50)
|
||||
};
|
||||
}).ToArray();
|
||||
Directory.CreateDirectory(Path.GetDirectoryName(Path.GetFullPath(outputPath))!);
|
||||
File.WriteAllBytes(outputPath, MessagePackSerializer.Serialize(receipt));
|
||||
Console.WriteLine(JsonSerializer.Serialize(new
|
||||
{
|
||||
receipt.ReceiptId,
|
||||
receipt.SessionId,
|
||||
receipt.Verdict,
|
||||
receipt.TrainingCorrespondenceCount,
|
||||
receipt.HeldOutCorrespondenceCount,
|
||||
receipt.Residuals,
|
||||
receipt.RejectionReasons,
|
||||
observationStats,
|
||||
calibration = receipt.PromotedCalibration is null ? null : new
|
||||
{
|
||||
receipt.PromotedCalibration.CalibrationId,
|
||||
cameras = receipt.PromotedCalibration.Cameras.Select(camera => new
|
||||
{
|
||||
camera.CameraId,
|
||||
camera.PositionMeters,
|
||||
camera.Orientation,
|
||||
camera.FocalLengthXPx,
|
||||
camera.FocalLengthYPx,
|
||||
camera.PrincipalPointXPx,
|
||||
camera.PrincipalPointYPx
|
||||
})
|
||||
},
|
||||
outputPath
|
||||
}));
|
||||
return receipt.Verdict == MimirSensorCalibrationSessionPhase.Promoted ? 0 : 2;
|
||||
}
|
||||
|
||||
static double CalibrationCliPercentile(double[] sorted, double percentile)
|
||||
{
|
||||
if (sorted.Length == 0) return double.NaN;
|
||||
var position = Math.Clamp(percentile, 0, 1) * (sorted.Length - 1);
|
||||
var lower = (int)Math.Floor(position);
|
||||
var upper = (int)Math.Ceiling(position);
|
||||
return sorted[lower] + (sorted[upper] - sorted[lower]) * (position - lower);
|
||||
}
|
||||
|
||||
static int RunSensorCalibrationFitSmoke()
|
||||
{
|
||||
const int width = 640;
|
||||
const int height = 480;
|
||||
const double radius = 0.0225;
|
||||
const double focal0 = 505.0;
|
||||
const double focal1 = 535.0;
|
||||
var camera1WorldToCamera = Quaternion.CreateFromYawPitchRoll(0.11f, -0.035f, 0.025f);
|
||||
var camera1Position = new Vector3(0.34f, 0.025f, 0.07f);
|
||||
var camera1Translation = Vector3.Transform(-camera1Position, camera1WorldToCamera);
|
||||
var random = new Random(1789);
|
||||
var observations = new List<MoveVisibilityObservation>();
|
||||
var correspondences = new List<MoveCrossCameraCorrespondence>();
|
||||
var wandIds = new[] { "move-a", "move-b", "move-c", "move-d" };
|
||||
for (var index = 0; index < 1400; index++)
|
||||
{
|
||||
var world = new Vector3(
|
||||
(float)(random.NextDouble() * 1.8 - 0.9),
|
||||
(float)(random.NextDouble() * 1.2 - 0.6),
|
||||
(float)(0.65 + random.NextDouble() * 3.0));
|
||||
var camera1 = Vector3.Transform(world, camera1WorldToCamera) + camera1Translation;
|
||||
if (camera1.Z <= 0.2f) { index--; continue; }
|
||||
var frameId = $"synthetic:{index}";
|
||||
var wandId = wandIds[index % wandIds.Length];
|
||||
var first = SyntheticSphereObservation("eye-0", wandId, frameId, index, world, focal0, width, height, radius, random);
|
||||
var second = SyntheticSphereObservation("eye-1", wandId, frameId, index, camera1, focal1, width, height, radius, random);
|
||||
if (first.CenterXPx is < 0 or >= width || first.CenterYPx is < 0 or >= height ||
|
||||
second.CenterXPx is < 0 or >= width || second.CenterYPx is < 0 or >= height)
|
||||
{
|
||||
index--;
|
||||
continue;
|
||||
}
|
||||
observations.Add(first);
|
||||
observations.Add(second);
|
||||
correspondences.Add(new MoveCrossCameraCorrespondence(wandId, first, second, 0));
|
||||
}
|
||||
|
||||
var started = DateTimeOffset.UtcNow;
|
||||
var session = MimirSensorCalibrationSessions.CreateFourWandOpticalSession(wandIds, ["eye-0", "eye-1"], started) with
|
||||
{
|
||||
Phase = MimirSensorCalibrationSessionPhase.Fitting,
|
||||
MissingRequirements = []
|
||||
};
|
||||
var evidence = new MoveVisibilityWindowReceipt("mimir.move_visibility_window.v1",
|
||||
started.ToUnixTimeMilliseconds() * 1_000_000,
|
||||
started.AddSeconds(20).ToUnixTimeMilliseconds() * 1_000_000,
|
||||
["synthetic"], observations.ToArray(), correspondences.ToArray());
|
||||
var receipt = MimirMoveSphereCalibrationFitter.Fit(session, evidence, width, height, radius);
|
||||
var fitted = receipt.PromotedCalibration?.Cameras.ToArray() ?? [];
|
||||
var focalError0 = fitted.Length == 2 ? Math.Abs(fitted[0].FocalLengthXPx - focal0) : double.PositiveInfinity;
|
||||
var focalError1 = fitted.Length == 2 ? Math.Abs(fitted[1].FocalLengthXPx - focal1) : double.PositiveInfinity;
|
||||
var positionError = fitted.Length == 2
|
||||
? Vector3.Distance(new Vector3((float)fitted[1].PositionMeters.X, (float)fitted[1].PositionMeters.Y, (float)fitted[1].PositionMeters.Z), camera1Position)
|
||||
: double.PositiveInfinity;
|
||||
Console.WriteLine(JsonSerializer.Serialize(new
|
||||
{
|
||||
receipt.Verdict,
|
||||
receipt.Residuals,
|
||||
receipt.RejectionReasons,
|
||||
focalError0,
|
||||
focalError1,
|
||||
positionError
|
||||
}));
|
||||
return receipt.Verdict == MimirSensorCalibrationSessionPhase.Promoted &&
|
||||
focalError0 < 12.0 && focalError1 < 12.0 && positionError < 0.03 ? 0 : 1;
|
||||
}
|
||||
|
||||
static MoveVisibilityObservation SyntheticSphereObservation(
|
||||
string cameraId,
|
||||
string wandId,
|
||||
string frameId,
|
||||
int sequence,
|
||||
Vector3 point,
|
||||
double focal,
|
||||
int width,
|
||||
int height,
|
||||
double radius,
|
||||
Random random)
|
||||
{
|
||||
var noiseX = (random.NextDouble() - 0.5) * 0.16;
|
||||
var noiseY = (random.NextDouble() - 0.5) * 0.16;
|
||||
var noiseRadius = (random.NextDouble() - 0.5) * 0.08;
|
||||
return new MoveVisibilityObservation(
|
||||
"synthetic", cameraId, wandId, frameId, sequence * 16_666_667L,
|
||||
(float)(width * 0.5 + focal * point.X / point.Z + noiseX),
|
||||
(float)(height * 0.5 - focal * point.Y / point.Z + noiseY),
|
||||
(float)(focal * radius / Math.Sqrt(point.Z * point.Z - radius * radius) + noiseRadius),
|
||||
0.95f);
|
||||
}
|
||||
|
||||
static int RunMuninnMoveCultMeshStreamSmoke(string nativeReservoirPath)
|
||||
{
|
||||
if (!File.Exists(nativeReservoirPath))
|
||||
|
|
@ -2915,17 +3097,17 @@ static int RunMoveProofRuntimeDriverSmoke(
|
|||
var activationDocument = activationDocuments.SingleOrDefault(document =>
|
||||
string.Equals(document.EvidenceStreamId, streamId, StringComparison.Ordinal));
|
||||
var activationSurfaceOk = !requireActivationSurface || activationDocument is
|
||||
{
|
||||
Active: true,
|
||||
DriverRegistered: true,
|
||||
LatestProofId: "mimir:starfire:move-proof:79",
|
||||
LatestMuninnEvidenceFrameId: "muninn:nightwing:move-evidence:79",
|
||||
LatestMimirEvidenceFrameId: "mimir:starfire:move-evidence:79",
|
||||
LatestMimirPoseFrameId: "mimir:starfire:move-pose:79",
|
||||
LatestFensalirFrameId: "fensalir:starfire:presented-frame:79",
|
||||
LatestVerdict: MimirMoveProofVerdict.FullPose,
|
||||
CalibratedCameraCount: 2
|
||||
} &&
|
||||
{
|
||||
Active: true,
|
||||
DriverRegistered: true,
|
||||
LatestProofId: "mimir:starfire:move-proof:79",
|
||||
LatestMuninnEvidenceFrameId: "muninn:nightwing:move-evidence:79",
|
||||
LatestMimirEvidenceFrameId: "mimir:starfire:move-evidence:79",
|
||||
LatestMimirPoseFrameId: "mimir:starfire:move-pose:79",
|
||||
LatestFensalirFrameId: "fensalir:starfire:presented-frame:79",
|
||||
LatestVerdict: MimirMoveProofVerdict.FullPose,
|
||||
CalibratedCameraCount: 2
|
||||
} &&
|
||||
(!activateViaSnapshotProvider ||
|
||||
activationDocument.Diagnostic.Contains("one-copy evidence snapshot supplied", StringComparison.Ordinal));
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,395 @@
|
|||
using System.Numerics;
|
||||
|
||||
namespace Mimir.Runtime.Synchronization;
|
||||
|
||||
public static class MimirMoveSphereCalibrationFitter
|
||||
{
|
||||
private const int ParameterCount = 8;
|
||||
|
||||
public static MimirSensorCalibrationReceiptDocument Fit(
|
||||
MimirSensorCalibrationSessionDocument session,
|
||||
MoveVisibilityWindowReceipt evidence,
|
||||
int imageWidth,
|
||||
int imageHeight,
|
||||
double orbRadiusMeters,
|
||||
DateTimeOffset? completedAt = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(session);
|
||||
ArgumentNullException.ThrowIfNull(evidence);
|
||||
if (imageWidth <= 0 || imageHeight <= 0) throw new ArgumentOutOfRangeException(nameof(imageWidth));
|
||||
|
||||
var rejectionReasons = new List<string>();
|
||||
if (session.Phase != MimirSensorCalibrationSessionPhase.Fitting)
|
||||
rejectionReasons.Add("session-is-not-ready-for-fitting");
|
||||
if (!(double.IsFinite(orbRadiusMeters) && orbRadiusMeters > 0.0))
|
||||
rejectionReasons.Add("measured-orb-radius-meters");
|
||||
if (session.SensorIds.Length != 2)
|
||||
rejectionReasons.Add("sphere-stereo-fit-requires-exactly-two-sensors");
|
||||
|
||||
var pairs = BuildPairs(session, evidence);
|
||||
if (pairs.Length < session.Acceptance.MinimumSameFrameCorrespondences)
|
||||
rejectionReasons.Add($"same-frame-correspondences-{pairs.Length}-of-{session.Acceptance.MinimumSameFrameCorrespondences}");
|
||||
|
||||
if (rejectionReasons.Count > 0)
|
||||
return Rejected(session, pairs.Length, rejectionReasons, completedAt);
|
||||
|
||||
var heldOut = pairs.Where(IsHeldOut).ToArray();
|
||||
var training = pairs.Where(pair => !IsHeldOut(pair)).ToArray();
|
||||
if (heldOut.Length < 50 || training.Length < 200)
|
||||
return Rejected(session, training.Length, ["insufficient-deterministic-training-or-held-out-partition"], completedAt, heldOut.Length);
|
||||
|
||||
var parameters = InitialParameters(training, imageWidth, imageHeight, orbRadiusMeters);
|
||||
var iterations = Optimize(parameters, training, imageWidth, imageHeight, orbRadiusMeters);
|
||||
var trainingConsensus = training;
|
||||
for (var pass = 0; pass < 3; pass++)
|
||||
{
|
||||
var pairErrors = PairErrors(parameters, trainingConsensus, imageWidth, imageHeight, orbRadiusMeters);
|
||||
var cutoff = Math.Clamp(Percentile(pairErrors.Select(value => value.Error).Order().ToArray(), 0.70) * 1.5, 4.0, 25.0);
|
||||
var next = pairErrors.Where(value => value.Error <= cutoff).Select(value => value.Pair).ToArray();
|
||||
if (next.Length < 200 || next.Length == trainingConsensus.Length) break;
|
||||
trainingConsensus = next;
|
||||
iterations += Optimize(parameters, trainingConsensus, imageWidth, imageHeight, orbRadiusMeters);
|
||||
}
|
||||
var validationCutoff = Math.Max(4.0, session.Acceptance.MaximumP95ReprojectionErrorPx * 2.0);
|
||||
var heldOutConsensus = PairErrors(parameters, heldOut, imageWidth, imageHeight, orbRadiusMeters)
|
||||
.Where(value => value.Error <= validationCutoff).Select(value => value.Pair).ToArray();
|
||||
var trainingErrors = ReprojectionErrors(parameters, trainingConsensus, imageWidth, imageHeight, orbRadiusMeters);
|
||||
var heldOutErrors = ReprojectionErrors(parameters, heldOutConsensus, imageWidth, imageHeight, orbRadiusMeters);
|
||||
var skewMs = pairs.Select(pair => pair.AbsoluteSkewNs / 1_000_000.0).Order().ToArray();
|
||||
var residuals = new MimirSensorCalibrationFitResiduals(
|
||||
Median(trainingErrors), Percentile(trainingErrors, 0.95),
|
||||
Median(heldOutErrors), Percentile(heldOutErrors, 0.95),
|
||||
Median(skewMs), skewMs[^1],
|
||||
ConditionEstimate(parameters, training, imageWidth, imageHeight, orbRadiusMeters),
|
||||
iterations,
|
||||
(double)trainingConsensus.Length / training.Length,
|
||||
(double)heldOutConsensus.Length / heldOut.Length);
|
||||
|
||||
if (!parameters.All(double.IsFinite)) rejectionReasons.Add("non-finite-fit");
|
||||
if (Math.Exp(parameters[0]) is < 100.0 or > 3000.0) rejectionReasons.Add($"camera-0-focal-length-{Math.Exp(parameters[0]):F1}-px-out-of-range");
|
||||
if (Math.Exp(parameters[1]) is < 100.0 or > 3000.0) rejectionReasons.Add($"camera-1-focal-length-{Math.Exp(parameters[1]):F1}-px-out-of-range");
|
||||
if (residuals.HeldOutMedianReprojectionErrorPx > session.Acceptance.MaximumMedianReprojectionErrorPx)
|
||||
rejectionReasons.Add($"held-out-median-reprojection-error-{residuals.HeldOutMedianReprojectionErrorPx:F3}-px");
|
||||
if (residuals.HeldOutP95ReprojectionErrorPx > session.Acceptance.MaximumP95ReprojectionErrorPx)
|
||||
rejectionReasons.Add($"held-out-p95-reprojection-error-{residuals.HeldOutP95ReprojectionErrorPx:F3}-px");
|
||||
if (residuals.MaximumAssociationSkewMilliseconds > session.Acceptance.MaximumAssociationSkewMilliseconds)
|
||||
rejectionReasons.Add($"association-skew-{residuals.MaximumAssociationSkewMilliseconds:F3}-ms");
|
||||
if (heldOutConsensus.Length < 50 || residuals.HeldOutInlierFraction < 0.65)
|
||||
rejectionReasons.Add($"held-out-consensus-{heldOutConsensus.Length}-pairs-{residuals.HeldOutInlierFraction:P1}");
|
||||
|
||||
var calibration = rejectionReasons.Count == 0
|
||||
? BuildCalibration(session, parameters, imageWidth, imageHeight)
|
||||
: null;
|
||||
return new MimirSensorCalibrationReceiptDocument(
|
||||
$"{session.SessionId}:receipt",
|
||||
session.SessionId,
|
||||
session.TrackingSpaceId,
|
||||
(completedAt ?? DateTimeOffset.UtcNow).ToString("O"),
|
||||
calibration is null ? MimirSensorCalibrationSessionPhase.Rejected : MimirSensorCalibrationSessionPhase.Promoted,
|
||||
training.Length,
|
||||
heldOut.Length,
|
||||
residuals,
|
||||
calibration,
|
||||
rejectionReasons.ToArray(),
|
||||
"mimir.sphere-stereo-bundle.v1");
|
||||
}
|
||||
|
||||
private static Pair[] BuildPairs(MimirSensorCalibrationSessionDocument session, MoveVisibilityWindowReceipt evidence)
|
||||
{
|
||||
var camera0 = session.SensorIds[0];
|
||||
var camera1 = session.SensorIds[1];
|
||||
var wands = session.RequiredWandIds.ToHashSet(StringComparer.Ordinal);
|
||||
var candidates = evidence.Correspondences
|
||||
.Where(pair => wands.Contains(pair.MoveId) &&
|
||||
pair.AbsoluteSkewNs <= session.Acceptance.MaximumAssociationSkewMilliseconds * 1_000_000.0 &&
|
||||
string.Equals(pair.First.FrameId, pair.Second.FrameId, StringComparison.Ordinal))
|
||||
.Select(pair => Order(pair, camera0, camera1))
|
||||
.Where(pair => pair.HasValue)
|
||||
.Select(pair => pair!.Value)
|
||||
.Where(pair => Valid(pair.First) && Valid(pair.Second))
|
||||
.GroupBy(pair => (pair.First.FrameId, pair.MoveId))
|
||||
.Select(group => group.First())
|
||||
.OrderBy(pair => pair.First.PublishedAtNs)
|
||||
.ThenBy(pair => pair.MoveId, StringComparer.Ordinal)
|
||||
.ToArray();
|
||||
if (candidates.Length == 0) return candidates;
|
||||
var firstMedian = Median(candidates.Select(pair => (double)pair.First.RadiusPx).Order().ToArray());
|
||||
var secondMedian = Median(candidates.Select(pair => (double)pair.Second.RadiusPx).Order().ToArray());
|
||||
return candidates.Where(pair =>
|
||||
{
|
||||
var firstRatio = pair.First.RadiusPx / firstMedian;
|
||||
var secondRatio = pair.Second.RadiusPx / secondMedian;
|
||||
var crossRatio = pair.First.RadiusPx / pair.Second.RadiusPx;
|
||||
return firstRatio is >= 0.35 and <= 4.0 &&
|
||||
secondRatio is >= 0.35 and <= 4.0 &&
|
||||
crossRatio is >= 0.25f and <= 4.0f;
|
||||
}).ToArray();
|
||||
}
|
||||
|
||||
private static Pair? Order(MoveCrossCameraCorrespondence pair, string camera0, string camera1)
|
||||
{
|
||||
if (pair.First.CameraId == camera0 && pair.Second.CameraId == camera1)
|
||||
return new Pair(pair.MoveId, pair.First, pair.Second, pair.AbsoluteSkewNs);
|
||||
if (pair.First.CameraId == camera1 && pair.Second.CameraId == camera0)
|
||||
return new Pair(pair.MoveId, pair.Second, pair.First, pair.AbsoluteSkewNs);
|
||||
return null;
|
||||
}
|
||||
|
||||
private static bool Valid(MoveVisibilityObservation value) =>
|
||||
float.IsFinite(value.CenterXPx) && float.IsFinite(value.CenterYPx) &&
|
||||
float.IsFinite(value.RadiusPx) && value.RadiusPx > 0.0f;
|
||||
|
||||
private static bool IsHeldOut(Pair pair) => (Fnva64($"{pair.First.FrameId}|{pair.MoveId}") % 10) < 2;
|
||||
|
||||
private static double[] InitialParameters(Pair[] pairs, int width, int height, double radius)
|
||||
{
|
||||
var focal = Math.Max(width, height) * 0.82;
|
||||
var source = pairs.Select(pair => PointFromSphere(pair.First, focal, width, height, radius)).ToArray();
|
||||
var target = pairs.Select(pair => PointFromSphere(pair.Second, focal, width, height, radius)).ToArray();
|
||||
var (rotation, translation) = FitRigid(source, target);
|
||||
var rotationVector = RotationVector(rotation);
|
||||
return [Math.Log(focal), Math.Log(focal), rotationVector.X, rotationVector.Y, rotationVector.Z,
|
||||
translation.X, translation.Y, translation.Z];
|
||||
}
|
||||
|
||||
private static int Optimize(double[] p, Pair[] pairs, int width, int height, double radius)
|
||||
{
|
||||
var lambda = 1e-3;
|
||||
var cost = Cost(p, pairs, width, height, radius);
|
||||
var iterations = 0;
|
||||
for (; iterations < 80; iterations++)
|
||||
{
|
||||
BuildNormalEquations(p, pairs, width, height, radius, out var normal, out var gradient);
|
||||
for (var i = 0; i < ParameterCount; i++) normal[i, i] += lambda * Math.Max(1.0, normal[i, i]);
|
||||
if (!Solve(normal, gradient.Select(value => -value).ToArray(), out var delta)) break;
|
||||
var candidate = p.Zip(delta, (value, step) => value + step).ToArray();
|
||||
var candidateCost = Cost(candidate, pairs, width, height, radius);
|
||||
if (candidateCost < cost)
|
||||
{
|
||||
Array.Copy(candidate, p, ParameterCount);
|
||||
if (Math.Abs(cost - candidateCost) <= Math.Max(1e-9, cost * 1e-8)) { iterations++; break; }
|
||||
cost = candidateCost;
|
||||
lambda = Math.Max(1e-8, lambda * 0.35);
|
||||
}
|
||||
else
|
||||
{
|
||||
lambda = Math.Min(1e8, lambda * 8.0);
|
||||
}
|
||||
}
|
||||
return iterations;
|
||||
}
|
||||
|
||||
private static void BuildNormalEquations(double[] p, Pair[] pairs, int width, int height, double radius, out double[,] normal, out double[] gradient)
|
||||
{
|
||||
normal = new double[ParameterCount, ParameterCount];
|
||||
gradient = new double[ParameterCount];
|
||||
foreach (var pair in Sample(pairs, 2500))
|
||||
{
|
||||
var residual = Residual(p, pair, width, height, radius);
|
||||
var jacobian = new double[residual.Length, ParameterCount];
|
||||
for (var column = 0; column < ParameterCount; column++)
|
||||
{
|
||||
var step = column < 2 ? 1e-5 : 1e-6;
|
||||
var perturbed = (double[])p.Clone();
|
||||
perturbed[column] += step;
|
||||
var shifted = Residual(perturbed, pair, width, height, radius);
|
||||
for (var row = 0; row < residual.Length; row++) jacobian[row, column] = (shifted[row] - residual[row]) / step;
|
||||
}
|
||||
for (var row = 0; row < residual.Length; row++)
|
||||
{
|
||||
var weight = HuberWeight(residual[row], 4.0) * Math.Max(0.05, Math.Min(pair.First.Confidence, pair.Second.Confidence));
|
||||
for (var i = 0; i < ParameterCount; i++)
|
||||
{
|
||||
gradient[i] += weight * jacobian[row, i] * residual[row];
|
||||
for (var j = 0; j <= i; j++) normal[i, j] += weight * jacobian[row, i] * jacobian[row, j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (var i = 0; i < ParameterCount; i++)
|
||||
for (var j = 0; j < i; j++) normal[j, i] = normal[i, j];
|
||||
}
|
||||
|
||||
private static double[] Residual(double[] p, Pair pair, int width, int height, double radius)
|
||||
{
|
||||
var f0 = Math.Exp(p[0]);
|
||||
var f1 = Math.Exp(p[1]);
|
||||
var rotation = QuaternionFromRotationVector(new Vector3((float)p[2], (float)p[3], (float)p[4]));
|
||||
var translation = new Vector3((float)p[5], (float)p[6], (float)p[7]);
|
||||
var point0 = PointFromSphere(pair.First, f0, width, height, radius);
|
||||
var point1 = Vector3.Transform(point0, rotation) + translation;
|
||||
var forward = Project(point1, f1, width, height, radius);
|
||||
var pointFrom1 = PointFromSphere(pair.Second, f1, width, height, radius);
|
||||
var reversePoint = Vector3.Transform(pointFrom1 - translation, Quaternion.Conjugate(rotation));
|
||||
var reverse = Project(reversePoint, f0, width, height, radius);
|
||||
return [
|
||||
forward.X - pair.Second.CenterXPx, forward.Y - pair.Second.CenterYPx, (forward.Z - pair.Second.RadiusPx) * 0.5,
|
||||
reverse.X - pair.First.CenterXPx, reverse.Y - pair.First.CenterYPx, (reverse.Z - pair.First.RadiusPx) * 0.5];
|
||||
}
|
||||
|
||||
private static double Cost(double[] p, Pair[] pairs, int width, int height, double radius) =>
|
||||
Sample(pairs, 2500).Sum(pair => Residual(p, pair, width, height, radius).Sum(value => HuberLoss(value, 4.0)));
|
||||
|
||||
private static double[] ReprojectionErrors(double[] p, Pair[] pairs, int width, int height, double radius) =>
|
||||
pairs.SelectMany(pair =>
|
||||
{
|
||||
var residual = Residual(p, pair, width, height, radius);
|
||||
return new[] { Math.Sqrt(residual[0] * residual[0] + residual[1] * residual[1]), Math.Sqrt(residual[3] * residual[3] + residual[4] * residual[4]) };
|
||||
}).Order().ToArray();
|
||||
|
||||
private static (Pair Pair, double Error)[] PairErrors(double[] p, Pair[] pairs, int width, int height, double radius) =>
|
||||
pairs.Select(pair =>
|
||||
{
|
||||
var residual = Residual(p, pair, width, height, radius);
|
||||
var forward = Math.Sqrt(residual[0] * residual[0] + residual[1] * residual[1]);
|
||||
var reverse = Math.Sqrt(residual[3] * residual[3] + residual[4] * residual[4]);
|
||||
return (pair, Math.Max(forward, reverse));
|
||||
}).OrderBy(value => value.Item2).Select(value => (value.pair, value.Item2)).ToArray();
|
||||
|
||||
private static MimirMoveFusionRigCalibration BuildCalibration(MimirSensorCalibrationSessionDocument session, double[] p, int width, int height)
|
||||
{
|
||||
var worldToCamera = QuaternionFromRotationVector(new Vector3((float)p[2], (float)p[3], (float)p[4]));
|
||||
var cameraToWorld = Quaternion.Normalize(Quaternion.Conjugate(worldToCamera));
|
||||
var translation = new Vector3((float)p[5], (float)p[6], (float)p[7]);
|
||||
var position = Vector3.Transform(-translation, cameraToWorld);
|
||||
return new MimirMoveFusionRigCalibration(
|
||||
$"{session.SessionId}:sphere-stereo",
|
||||
session.TrackingSpaceId,
|
||||
[
|
||||
new MimirMoveFusionCameraCalibration(session.SensorIds[0], Fnva64(session.SensorIds[0]),
|
||||
new MimirVector3Snapshot(0, 0, 0), new MimirQuaternionSnapshot(0, 0, 0, 1),
|
||||
Math.Exp(p[0]), Math.Exp(p[0]), width * 0.5, height * 0.5),
|
||||
new MimirMoveFusionCameraCalibration(session.SensorIds[1], Fnva64(session.SensorIds[1]),
|
||||
new MimirVector3Snapshot(position.X, position.Y, position.Z),
|
||||
new MimirQuaternionSnapshot(cameraToWorld.X, cameraToWorld.Y, cameraToWorld.Z, cameraToWorld.W),
|
||||
Math.Exp(p[1]), Math.Exp(p[1]), width * 0.5, height * 0.5)
|
||||
],
|
||||
MaximumAssociationSkewMilliseconds: session.Acceptance.MaximumAssociationSkewMilliseconds);
|
||||
}
|
||||
|
||||
private static MimirSensorCalibrationReceiptDocument Rejected(MimirSensorCalibrationSessionDocument session, int trainingCount, IEnumerable<string> reasons, DateTimeOffset? completedAt, int heldOutCount = 0) =>
|
||||
new($"{session.SessionId}:receipt", session.SessionId, session.TrackingSpaceId,
|
||||
(completedAt ?? DateTimeOffset.UtcNow).ToString("O"), MimirSensorCalibrationSessionPhase.Rejected,
|
||||
trainingCount, heldOutCount,
|
||||
new MimirSensorCalibrationFitResiduals(double.NaN, double.NaN, double.NaN, double.NaN, double.NaN, double.NaN, double.PositiveInfinity, 0),
|
||||
null, reasons.ToArray(), "mimir.sphere-stereo-bundle.v1");
|
||||
|
||||
private static Vector3 PointFromSphere(MoveVisibilityObservation observation, double focal, int width, int height, double radius)
|
||||
{
|
||||
var z = radius * Math.Sqrt(1.0 + focal * focal / (observation.RadiusPx * observation.RadiusPx));
|
||||
return new Vector3((float)((observation.CenterXPx - width * 0.5) * z / focal),
|
||||
(float)((height * 0.5 - observation.CenterYPx) * z / focal), (float)z);
|
||||
}
|
||||
|
||||
private static Vector3 Project(Vector3 point, double focal, int width, int height, double radius)
|
||||
{
|
||||
if (point.Z <= radius * 1.001) return new Vector3(1e6f, 1e6f, 1e6f);
|
||||
var imageRadius = focal * radius / Math.Sqrt(point.Z * point.Z - radius * radius);
|
||||
return new Vector3((float)(width * 0.5 + focal * point.X / point.Z),
|
||||
(float)(height * 0.5 - focal * point.Y / point.Z), (float)imageRadius);
|
||||
}
|
||||
|
||||
private static (Quaternion Rotation, Vector3 Translation) FitRigid(Vector3[] source, Vector3[] target)
|
||||
{
|
||||
var sourceCenter = source.Aggregate(Vector3.Zero, (sum, value) => sum + value) / source.Length;
|
||||
var targetCenter = target.Aggregate(Vector3.Zero, (sum, value) => sum + value) / target.Length;
|
||||
double sxx = 0, sxy = 0, sxz = 0, syx = 0, syy = 0, syz = 0, szx = 0, szy = 0, szz = 0;
|
||||
for (var i = 0; i < source.Length; i++)
|
||||
{
|
||||
var a = source[i] - sourceCenter;
|
||||
var b = target[i] - targetCenter;
|
||||
sxx += a.X * b.X; sxy += a.X * b.Y; sxz += a.X * b.Z;
|
||||
syx += a.Y * b.X; syy += a.Y * b.Y; syz += a.Y * b.Z;
|
||||
szx += a.Z * b.X; szy += a.Z * b.Y; szz += a.Z * b.Z;
|
||||
}
|
||||
var n = new double[,] {
|
||||
{ sxx + syy + szz, syz - szy, szx - sxz, sxy - syx },
|
||||
{ syz - szy, sxx - syy - szz, sxy + syx, szx + sxz },
|
||||
{ szx - sxz, sxy + syx, -sxx + syy - szz, syz + szy },
|
||||
{ sxy - syx, szx + sxz, syz + szy, -sxx - syy + szz }
|
||||
};
|
||||
var q = new[] { 1.0, 0.0, 0.0, 0.0 };
|
||||
for (var iteration = 0; iteration < 64; iteration++)
|
||||
{
|
||||
var next = new double[4];
|
||||
for (var row = 0; row < 4; row++) for (var column = 0; column < 4; column++) next[row] += n[row, column] * q[column];
|
||||
var norm = Math.Sqrt(next.Sum(value => value * value));
|
||||
if (norm < 1e-12) break;
|
||||
for (var j = 0; j < 4; j++) q[j] = next[j] / norm;
|
||||
}
|
||||
var rotation = Quaternion.Normalize(new Quaternion((float)q[1], (float)q[2], (float)q[3], (float)q[0]));
|
||||
return (rotation, targetCenter - Vector3.Transform(sourceCenter, rotation));
|
||||
}
|
||||
|
||||
private static Vector3 RotationVector(Quaternion quaternion)
|
||||
{
|
||||
quaternion = Quaternion.Normalize(quaternion);
|
||||
if (quaternion.W < 0) quaternion = new Quaternion(-quaternion.X, -quaternion.Y, -quaternion.Z, -quaternion.W);
|
||||
var angle = 2.0 * Math.Acos(Math.Clamp(quaternion.W, -1.0f, 1.0f));
|
||||
var sinHalf = Math.Sqrt(Math.Max(0.0, 1.0 - quaternion.W * quaternion.W));
|
||||
return sinHalf < 1e-8 ? Vector3.Zero : new Vector3(quaternion.X, quaternion.Y, quaternion.Z) * (float)(angle / sinHalf);
|
||||
}
|
||||
|
||||
private static Quaternion QuaternionFromRotationVector(Vector3 value)
|
||||
{
|
||||
var angle = value.Length();
|
||||
return angle < 1e-9f ? Quaternion.Identity : Quaternion.CreateFromAxisAngle(value / angle, angle);
|
||||
}
|
||||
|
||||
private static Pair[] Sample(Pair[] pairs, int maximum)
|
||||
{
|
||||
if (pairs.Length <= maximum) return pairs;
|
||||
var stride = (double)pairs.Length / maximum;
|
||||
return Enumerable.Range(0, maximum).Select(index => pairs[(int)(index * stride)]).ToArray();
|
||||
}
|
||||
|
||||
private static double ConditionEstimate(double[] p, Pair[] pairs, int width, int height, double radius)
|
||||
{
|
||||
BuildNormalEquations(p, pairs, width, height, radius, out var normal, out _);
|
||||
var diagonal = Enumerable.Range(0, ParameterCount).Select(index => Math.Abs(normal[index, index])).Where(value => value > 1e-12).ToArray();
|
||||
return diagonal.Length == 0 ? double.PositiveInfinity : diagonal.Max() / diagonal.Min();
|
||||
}
|
||||
|
||||
private static bool Solve(double[,] matrix, double[] rhs, out double[] solution)
|
||||
{
|
||||
var n = rhs.Length;
|
||||
var augmented = new double[n, n + 1];
|
||||
for (var row = 0; row < n; row++) { for (var column = 0; column < n; column++) augmented[row, column] = matrix[row, column]; augmented[row, n] = rhs[row]; }
|
||||
for (var pivot = 0; pivot < n; pivot++)
|
||||
{
|
||||
var best = pivot;
|
||||
for (var row = pivot + 1; row < n; row++) if (Math.Abs(augmented[row, pivot]) > Math.Abs(augmented[best, pivot])) best = row;
|
||||
if (Math.Abs(augmented[best, pivot]) < 1e-12) { solution = []; return false; }
|
||||
if (best != pivot) for (var column = pivot; column <= n; column++) (augmented[pivot, column], augmented[best, column]) = (augmented[best, column], augmented[pivot, column]);
|
||||
var scale = augmented[pivot, pivot];
|
||||
for (var column = pivot; column <= n; column++) augmented[pivot, column] /= scale;
|
||||
for (var row = 0; row < n; row++) if (row != pivot)
|
||||
{
|
||||
var factor = augmented[row, pivot];
|
||||
for (var column = pivot; column <= n; column++) augmented[row, column] -= factor * augmented[pivot, column];
|
||||
}
|
||||
}
|
||||
solution = Enumerable.Range(0, n).Select(row => augmented[row, n]).ToArray();
|
||||
return solution.All(double.IsFinite);
|
||||
}
|
||||
|
||||
private static double HuberLoss(double value, double threshold) => Math.Abs(value) <= threshold ? 0.5 * value * value : threshold * (Math.Abs(value) - 0.5 * threshold);
|
||||
private static double HuberWeight(double value, double threshold) => Math.Abs(value) <= threshold ? 1.0 : threshold / Math.Abs(value);
|
||||
private static double Median(double[] sorted) => Percentile(sorted, 0.5);
|
||||
private static double Percentile(double[] sorted, double percentile)
|
||||
{
|
||||
if (sorted.Length == 0) return double.NaN;
|
||||
var position = Math.Clamp(percentile, 0, 1) * (sorted.Length - 1);
|
||||
var lower = (int)Math.Floor(position);
|
||||
var upper = (int)Math.Ceiling(position);
|
||||
return sorted[lower] + (sorted[upper] - sorted[lower]) * (position - lower);
|
||||
}
|
||||
|
||||
private static ulong Fnva64(string value)
|
||||
{
|
||||
var hash = 14695981039346656037UL;
|
||||
foreach (var item in System.Text.Encoding.UTF8.GetBytes(value)) { hash ^= item; hash *= 1099511628211UL; }
|
||||
return hash;
|
||||
}
|
||||
|
||||
private readonly record struct Pair(string MoveId, MoveVisibilityObservation First, MoveVisibilityObservation Second, long AbsoluteSkewNs);
|
||||
}
|
||||
|
|
@ -16,7 +16,7 @@ public enum MimirSensorCalibrationSessionPhase
|
|||
[CultDocument("mimir.sensor_calibration_session", "mimir.sensor_calibration_session.v1")]
|
||||
[MessagePackObject]
|
||||
public sealed record MimirSensorCalibrationSessionDocument(
|
||||
[property: Key(0)] [property: CultName] string SessionId,
|
||||
[property: Key(0)][property: CultName] string SessionId,
|
||||
[property: Key(1)] string TrackingSpaceId,
|
||||
[property: Key(2)] string Owner,
|
||||
[property: Key(3)] string StartedAtUtc,
|
||||
|
|
@ -58,7 +58,7 @@ public sealed record MimirSensorCalibrationSensorProgress(
|
|||
[CultDocument("mimir.sensor_calibration_receipt", "mimir.sensor_calibration_receipt.v1")]
|
||||
[MessagePackObject]
|
||||
public sealed record MimirSensorCalibrationReceiptDocument(
|
||||
[property: Key(0)] [property: CultName] string ReceiptId,
|
||||
[property: Key(0)][property: CultName] string ReceiptId,
|
||||
[property: Key(1)] string SessionId,
|
||||
[property: Key(2)] string TrackingSpaceId,
|
||||
[property: Key(3)] string CompletedAtUtc,
|
||||
|
|
@ -79,7 +79,9 @@ public sealed record MimirSensorCalibrationFitResiduals(
|
|||
[property: Key(4)] double MedianAssociationSkewMilliseconds,
|
||||
[property: Key(5)] double MaximumAssociationSkewMilliseconds,
|
||||
[property: Key(6)] double ConditionEstimate,
|
||||
[property: Key(7)] int Iterations);
|
||||
[property: Key(7)] int Iterations,
|
||||
[property: Key(8)] double TrainingInlierFraction = 0.0,
|
||||
[property: Key(9)] double HeldOutInlierFraction = 0.0);
|
||||
|
||||
public static class MimirSensorCalibrationSessions
|
||||
{
|
||||
|
|
|
|||
|
|
@ -185,3 +185,4 @@
|
|||
{"ts": "2026-07-13T01:41:29+01:00", "type": "verification", "status": "partial", "note": "After per-eye exposure tuning and latest-state RUDP handoff, Mimir's 15s dual-eye receipt saw all four stable Move IDs on both cameras: 16 camera observations and 8 pair records across 2 aggregate frames. Full overlap is proven; calibration is not. Only 2 unique frame IDs arrived, so directly instrument parent publication and RUDP send/receive counters before camera-matrix fitting."}
|
||||
{"ts": "2026-07-13T02:32:31+01:00", "type": "verification", "status": "ok", "note": "2026-07-13 Nightwing single-provider dual-Eye field proof: one stable Muninn parent received 622 unique evidence frames in 15 s through Mimir, with 2,647 observations, 1,384 same-ID cross-camera correspondences, and all four Move IDs visible on both Eyes. Producer transport health reported 5,302 produced, 5,301 handoffs, 771 sends, and zero local-ring admissions. Artifact: artifacts/move-calibration/muninn-clocked-aggregator.mpack. Odin owner commits: 39c839a, a340c33, 8aefd1c, f82ca7a."}
|
||||
{"ts": "2026-07-13T02:39:32+01:00", "type": "verification", "status": "rejected", "note": "2026-07-13 typed Move stereo assessment rejected calibration promotion. Corrected same-frame collector captured 798 frames and 130 same-frame/same-Move pairs at zero published-frame skew. Eye 0 covered 4/12 image grid cells; Eye 1 covered 2/12 and saw only two Move IDs in this window. Both calibrated intrinsics and measured orb radius were absent. No camera matrices were emitted. Artifacts: artifacts/move-calibration/muninn-same-frame-correspondence.mpack and muninn-same-frame-stereo-assessment.mpack."}
|
||||
{"ts": "2026-07-14T01:00:00+01:00", "type": "verification", "status": "rejected", "note": "2026-07-14 live two-wand sphere-stereo fit remained honestly rejected. Synthetic recovery passed at 0.34/0.63 px held-out median/P95 with focal errors below 0.43 px and 0.22 mm baseline error. Live robust consensus reached 0.51/2.08 px but only 53.8 percent held-out support and an out-of-range Eye 1 focal estimate; PSMoveAPI position age is being projected as confidence and blob radius is not calibrated sphere radius. Artifact live-two-wand-20260714-004210.nominal-radius-fit.mpack is diagnostic only."}
|
||||
|
|
|
|||
|
|
@ -87,6 +87,14 @@ invariants:
|
|||
are temporary smoke/bootstrap edges only.
|
||||
|
||||
accepted_design:
|
||||
- `MimirMoveSphereCalibrationFitter` owns the bounded two-Eye optical fit
|
||||
after spatial collection. Eye 0 defines the tracking-space gauge; a robust
|
||||
training consensus estimates both focal lengths and Eye 1 rigid pose, while
|
||||
a deterministic untouched 20 percent partition owns promotion evidence.
|
||||
Promotion requires low held-out median/P95 reprojection error and at least
|
||||
65 percent held-out consensus. Physical orb radius is explicit and never
|
||||
guessed. PSMoveAPI blob radius and position age are not calibrated sphere
|
||||
radius or optical confidence.
|
||||
- `Mimir.slnx` owns the app surface.
|
||||
- `src/Mimir.App` launches Fensalir as host/window/D3D12 library.
|
||||
- `src/Mimir.Runtime` owns `MimirSynchronizationHub`,
|
||||
|
|
|
|||
Loading…
Reference in a new issue