diff --git a/ios/HouseScan/Runtime/ScanEngine.swift b/ios/HouseScan/Runtime/ScanEngine.swift index 3cd2b5e3c..5ca06a55e 100644 --- a/ios/HouseScan/Runtime/ScanEngine.swift +++ b/ios/HouseScan/Runtime/ScanEngine.swift @@ -58,8 +58,11 @@ final class ScanEngine { // Wall geometry inputs private var meterAnchorID: UUID? /// The meter anchor's pose the wall and everything captured agree with, and the corrections - /// still to apply to them as ARKit refines it (`refreshMeterFromAnchor`). Nil on a replay. + /// still to apply to them as ARKit refines it (`applySpatialUpdate`). Nil on a replay. private var meterTracking: MeterAnchorTracking? + /// Which frames carried the meter's anchor since it was anchored, for the drift log + /// (`noteMeterAnchor`). Starts again with `meterTracking`. + private var meterAnchorPresence = MeterAnchorPresence() /// Whether a frame source may start, and what a failure and Start over do to it. private var sourceState = CaptureSourceState() /// Counts changes to the captured geometry: every anchor correction, ground refinement or @@ -238,8 +241,10 @@ final class ScanEngine { breakWalkedPath(because: "the walk paused (\(state.phase.rawValue) -> \(phase.rawValue))") } if state.phase == .resultAR { hideResultInCamera() } + let previous = state.phase state.phase = phase RuntimeLog.state.info("STATE=\(phase.rawValue, privacy: .public)") + logMeterAnchorSummary(from: previous, to: phase) switch phase { case .findMeter: state.guidance = .findMeter @@ -377,6 +382,7 @@ final class ScanEngine { live?.setResultVisible(frame.tracking == .normal) } if let planes = frame.groundPlanes { groundPlanes = planes } + noteMeterAnchor(frame) applySpatialUpdate(frame) // After the anchor correction, so the wall and the tap's phone position are in the frame // these planes are reported in. @@ -414,7 +420,7 @@ final class ScanEngine { /// `GroundPlaneChoice.maximumRaise` (`SpatialUpdate.raiseLimit`). private func applySpatialUpdate(_ frame: SourceFrame) { guard var map = coverage else { return } - let before = (y: map.wall.groundY, measured: groundEvidence.measured) + let before = (y: map.wall.groundY, measured: groundEvidence.measured, meter: map.wall.meter) // A frame made before the meter was anchored again (`refitWallToDetectedPlane`) carries // the old anchor's pose, which is no correction of the new one. let anchor = frame.meterAnchorID == meterAnchorID ? frame.meterAnchor : nil @@ -430,6 +436,7 @@ final class ScanEngine { if features != state.features { state.features = features } // The first tap of a mark still being made was in the old frame too. pendingTaps = pendingTaps.map(correction.point) + logAnchorCorrection(correction, step: simd_distance(map.wall.meter, before.meter), frame: frame) } if outcome.groundChanged { let source = outcome.groundChoice.map(Self.describe) ?? "no plane that qualifies" @@ -493,7 +500,8 @@ final class ScanEngine { } /// Door and window heights, spans and fence distances follow the wall frame; the tapped world - /// points stay put. + /// points stay put. An anchor correction moves the points and the wall together instead + /// (`applySpatialUpdate`), so it needs no reprojection. func reprojectFeatures() { guard let wall = coverage?.wall, !state.features.isEmpty else { return } var features = state.features @@ -502,6 +510,77 @@ final class ScanEngine { publishFeaturesPastEnds() } + // The drift log (#73). Its lines are `.notice`, which the unified log keeps on the device, so + // `log collect` after a walk and Console without "Include Info Messages" both show them. On a + // walk away and back, the lines that are there settle it, never a line being absent: + // corrections with the marks on their objects mean ARKit corrected its map and the marks + // followed; frames that had the anchor, no corrections, and marks off their objects mean + // drift ARKit never corrected, which no anchor can fix; frames that lost the anchor mean + // nothing could follow it. `t` is the capture clock the packet's manifest times use. + + /// One line per correction applied (`applySpatialUpdate`), with the total since the meter was + /// anchored. `MeterAnchorTracking` already limits the rate: a correction is applied only once + /// the anchor has moved 2 cm or turned 0.4 degrees since the last one. `step` is how far it + /// moved the meter. + private func logAnchorCorrection(_ correction: YawCorrection, step: Float, frame: SourceFrame) { + guard let tracking = meterTracking else { return } + let total = tracking.sinceAnchored + let turned = correction.yaw * 180 / .pi + let totalMoved = simd_length(total.moved) + let totalTurned = total.yaw * 180 / .pi + let count = tracking.corrections + let marks = state.features.count + let t = frame.timestamp + let fromMeter = coverage.map { simd_distance(frame.camera.position, $0.wall.meter) } ?? 0 + let phase = state.phase.rawValue + RuntimeLog.capture.notice( + "meter anchor corrected at t=\(t, format: .fixed(precision: 2)) s, phone \(fromMeter, format: .fixed(precision: 1)) m from the meter: moved \(step, format: .fixed(precision: 3)) m, turned \(turned, format: .fixed(precision: 2)) degrees; since anchored (\(count) corrections): moved \(totalMoved, format: .fixed(precision: 3)) m (x \(total.moved.x, format: .fixed(precision: 3)), y \(total.moved.y, format: .fixed(precision: 3)), z \(total.moved.z, format: .fixed(precision: 3))), turned \(totalTurned, format: .fixed(precision: 2)) degrees; \(marks) marks moved with it (phase \(phase, privacy: .public))" + ) + } + + /// One line when the frames stop carrying the meter's anchor, and one when they carry it + /// again: while it is missing, no correction can reach the wall or the marks. The first frame + /// after the meter is anchored writes one too. Live only: a replay has no anchor. + private func noteMeterAnchor(_ frame: SourceFrame) { + guard let id = meterAnchorID, let tracking = meterTracking else { return } + let sighting: MeterAnchorPresence.Sighting = + frame.meterAnchorID != id ? .otherAnchor : frame.meterAnchor == nil ? .missing : .present + guard let change = meterAnchorPresence.observe(sighting) else { return } + let t = frame.timestamp + let fromMeter = coverage.map { simd_distance(frame.camera.position, $0.wall.meter) } ?? 0 + let count = tracking.corrections + let phase = state.phase.rawValue + RuntimeLog.capture.notice( + "meter anchor in frames: \(change.rawValue, privacy: .public) at t=\(t, format: .fixed(precision: 2)) s, phone \(fromMeter, format: .fixed(precision: 1)) m from the meter (\(count) corrections so far, phase \(phase, privacy: .public))" + ) + } + + /// The tracking's totals on every phase change once the meter is anchored, zero corrections + /// included, and whether the latest frame carried the anchor. Sending the scan and opening the + /// AR result always write one, with no anchor too (a replay), so a walk's log ends with it. + private func logMeterAnchorSummary(from previous: ScanPhase, to phase: ScanPhase) { + let from = previous.rawValue, to = phase.rawValue + let t = captureClock.map { String(format: "%.2f", $0) } ?? "none" + guard let tracking = meterTracking else { + guard phase == .uploading || phase == .resultAR else { return } + RuntimeLog.capture.notice( + "meter anchor at \(from, privacy: .public) -> \(to, privacy: .public), t=\(t, privacy: .public): not tracked (no meter anchor)" + ) + return + } + let total = tracking.sinceAnchored + let totalMoved = simd_length(total.moved) + let totalTurned = total.yaw * 180 / .pi + let count = tracking.corrections + let frames = meterAnchorPresence + let last = frames.last?.rawValue ?? "no frame yet" + let wall = coverage == nil ? "no wall" : "wall set" + let marks = state.features.count + RuntimeLog.capture.notice( + "meter anchor at \(from, privacy: .public) -> \(to, privacy: .public), t=\(t, privacy: .public): \(count) corrections since anchored, moved \(totalMoved, format: .fixed(precision: 3)) m (x \(total.moved.x, format: .fixed(precision: 3)), y \(total.moved.y, format: .fixed(precision: 3)), z \(total.moved.z, format: .fixed(precision: 3))), turned \(totalTurned, format: .fixed(precision: 2)) degrees; latest frame: \(last, privacy: .public); frames with the anchor \(frames.present), without it \(frames.missing), naming another \(frames.otherAnchor); \(wall, privacy: .public), \(marks) marks" + ) + } + private func closeUp(_ frame: SourceFrame) { guard let wall = coverage?.wall else { return } if case .captured = state.closeUp { return } @@ -1198,6 +1277,8 @@ final class ScanEngine { coverage = nil meterAnchorID.map { live?.removeAnchor($0) } meterAnchorID = nil + meterTracking = nil + meterAnchorPresence = MeterAnchorPresence() meterPlaneSource = .detectedPlane meterTapCamera = nil state.wall = nil @@ -1692,6 +1773,8 @@ final class ScanEngine { coverage = nil meterAnchorID.map { live?.removeAnchor($0) } meterAnchorID = nil + meterTracking = nil + meterAnchorPresence = MeterAnchorPresence() meterPlaneSource = .detectedPlane meterTapCamera = nil store = KeyframeStore() @@ -1760,13 +1843,15 @@ final class ScanEngine { func setMeterAnchor(_ id: UUID?, pose: simd_float4x4?) { meterAnchorID = id meterTracking = pose.map(MeterAnchorTracking.init) + meterAnchorPresence = MeterAnchorPresence() } /// The meter anchored again at `pose` in the same world frame, by a re-fit. The corrections /// logged so far stay (`MeterAnchorTracking.anchorAgain`): the close-up photo and anything - /// else captured before the re-fit still needs them. + /// else captured before the re-fit still needs them. The drift log's totals start again. private func anchorMeterAgain(_ id: UUID?, pose: simd_float4x4) { meterAnchorID = id + meterAnchorPresence = MeterAnchorPresence() if meterTracking == nil { meterTracking = MeterAnchorTracking(pose: pose) } else { @@ -1807,6 +1892,7 @@ final class ScanEngine { groundMeasured = false lastFrame = nil meterTracking = nil + meterAnchorPresence = MeterAnchorPresence() meterAnchorID = nil state.projection = nil state.tracking = .notAvailable diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Geometry/MeterAnchorTracking.swift b/ios/HouseScanKit/Sources/HouseScanKit/Geometry/MeterAnchorTracking.swift index c8acca55e..96d2d8b93 100644 --- a/ios/HouseScanKit/Sources/HouseScanKit/Geometry/MeterAnchorTracking.swift +++ b/ios/HouseScanKit/Sources/HouseScanKit/Geometry/MeterAnchorTracking.swift @@ -75,6 +75,13 @@ public struct MeterAnchorTracking: Sendable, Equatable { /// frame (`LiveCapture.showResult`), so it follows the anchor however far it has moved since. public private(set) var pose: simd_float4x4 + /// The anchor's pose when the meter was anchored (tapped, or anchored again by a re-fit): + /// where everything captured since started out. + public private(set) var anchoredPose: simd_float4x4 + + /// How many corrections have been applied since the meter was anchored. + public private(set) var corrections = 0 + /// A correction is applied once it moves the meter 2 cm, or turns the wall 0.4 degrees, which /// moves a point 3 m from the meter (about where a battery stands) 2 cm. ARKit nudges the /// anchor by millimetres most frames, and each correction redraws the overlays; 2 cm is far @@ -87,10 +94,21 @@ public struct MeterAnchorTracking: Sendable, Equatable { public init(pose: simd_float4x4) { self.pose = pose + anchoredPose = pose + } + + /// All the corrections applied since the meter was anchored, as one: how far ARKit has moved + /// the meter (world x, y, z, in meters) and turned the wall about gravity (radians). What + /// hasn't reached `minimumMove` or `minimumTurn` yet isn't in it. A device log of it, with + /// `MeterAnchorPresence`, tells a map ARKit corrected from drift it never corrected (#73): the + /// marks and the result follow the first, and nothing on the phone can follow the second. + public var sinceAnchored: (moved: SIMD3, yaw: Float) { + (Self.origin(pose) - Self.origin(anchoredPose), YawCorrection(from: anchoredPose, to: pose).yaw) } public static func == (a: MeterAnchorTracking, b: MeterAnchorTracking) -> Bool { - a.pose == b.pose && a.log.map(\.time) == b.log.map(\.time) && a.log.map(\.correction) == b.log.map(\.correction) + a.pose == b.pose && a.anchoredPose == b.anchoredPose && a.corrections == b.corrections + && a.log.map(\.time) == b.log.map(\.time) && a.log.map(\.correction) == b.log.map(\.correction) } /// The correction from `pose` to `anchor`, seen on the frame at `time`, when it is large @@ -102,17 +120,20 @@ public struct MeterAnchorTracking: Sendable, Equatable { let moved = simd_distance(correction.point(origin), origin) guard moved > Self.minimumMove || abs(correction.yaw) > Self.minimumTurn else { return nil } pose = anchor + corrections += 1 log.append((time, correction)) return correction } /// The meter anchored again at `pose`, in the same world frame (a re-fit moved the wall to a - /// detected plane): later corrections are measured from it. The log stays, because it is still - /// what takes a pose captured before now into the current frame; starting it over would leave - /// the close-up photo, and any keyframe saved before the re-fit, uncorrected for the moves - /// ARKit made after it was taken. + /// detected plane): later corrections, and the totals since anchored, are measured from it. + /// The log stays, because it is still what takes a pose captured before now into the current + /// frame; starting it over would leave the close-up photo, and any keyframe saved before the + /// re-fit, uncorrected for the moves ARKit made after it was taken. public mutating func anchorAgain(at pose: simd_float4x4) { self.pose = pose + anchoredPose = pose + corrections = 0 } /// What takes a pose captured at `time` into the frame the wall agrees with now: every @@ -132,6 +153,47 @@ public struct MeterAnchorTracking: Sendable, Equatable { public func correctedCamera(_ raw: CameraFrame, capturedAt time: Double) -> CameraFrame { correction(since: time).moved(raw) } + + private static func origin(_ m: simd_float4x4) -> SIMD3 { + SIMD3(m.columns.3.x, m.columns.3.y, m.columns.3.z) + } +} + +/// Whether the frames since the meter was anchored carried its anchor, for the drift log (#73). +/// A correction can only reach the marks from a frame that carries the anchor, so a walk with no +/// corrections logged is read against this: frames that had the anchor and never moved it +/// (drift ARKit never corrected), or frames that lost it (nothing could follow it). +public struct MeterAnchorPresence: Sendable, Equatable { + public enum Sighting: String, Sendable, CaseIterable { + /// The frame carries the anchor's pose. + case present + /// The frame names the anchor, but ARKit no longer lists it among the frame's anchors. + case missing + /// The frame names another anchor or none: made before the meter was anchored again, or + /// after the session let the anchor go. + case otherAnchor = "other anchor" + } + + /// What the latest frame showed; nil before the first frame since the meter was anchored. + public private(set) var last: Sighting? + public private(set) var present = 0 + public private(set) var missing = 0 + public private(set) var otherAnchor = 0 + + public init() {} + + /// Counts a frame. Returns `sighting` when it differs from the frame before's (the first + /// frame included), nil when it is the same. + public mutating func observe(_ sighting: Sighting) -> Sighting? { + switch sighting { + case .present: present += 1 + case .missing: missing += 1 + case .otherAnchor: otherAnchor += 1 + } + let changed = sighting != last + last = sighting + return changed ? sighting : nil + } } /// The corrections to apply to poses captured at given times, for work off the main actor (the diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/AnchoredMarksTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/AnchoredMarksTests.swift new file mode 100644 index 000000000..51f402c1b --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/AnchoredMarksTests.swift @@ -0,0 +1,130 @@ +import Foundation +@testable import HouseScanKit +import simd +import Testing + +// #73 (build 4.1, run 2): a window's outline stayed where it was tapped while the meter pin +// followed the anchor, and ended up on the panel boxes. In that build a correction moved only the +// meter, and the marks kept their world points. The engine now moves the tapped points with the +// same correction as the wall (`ScanEngine.applySpatialUpdate`); these pin what that has to +// keep true. + +/// The marks move with the meter's anchor as one body with the wall. +@Suite struct AnchoredMarksTests { + /// The anchor moved by (0.2, 0, 0.1) m and turned 3 degrees about its own vertical, as ARKit + /// might correct it after a walk 5 m away and back. + static func corrected(_ pose: simd_float4x4) -> simd_float4x4 { + var new = MeterAnchorCorrectionTests.turned(pose, degrees: 3) + new.columns.3 += SIMD4(0.2, 0, 0.1, 0) + return new + } + + static func anchorLocal(_ world: SIMD3, _ pose: simd_float4x4) -> SIMD3 { + let local = pose.inverse * SIMD4(world, 1) + return SIMD3(local.x, local.y, local.z) + } + + /// A window's two corners 1.2 to 2.0 m along, a fence tap 2 m out: moved with the correction, + /// each keeps its place in the anchor's frame, and its s, height and distance out from the + /// moved wall (what `ScanEngine.project` reads) are what they were at the tap. Left where they + /// were tapped, as in 4.1, the window's s is off by centimetres. + @Test func marksKeepTheirPlaceRelativeToTheMeter() throws { + var wall = standardWall() + let pose = MeterAnchorCorrectionTests.wallHitPose(meter: wall.meter, outward: wall.outward) + var tracking = MeterAnchorTracking(pose: pose) + let taps = [WallPoint(s: 1.2, height: 1.0, out: 0), WallPoint(s: 2.0, height: 2.2, out: 0), WallPoint(s: -0.8, height: 0, out: 2)] + let points = taps.map { wall.world($0) } + + let newPose = Self.corrected(pose) + let update = tracking.update(to: newPose) + let correction = try #require(update) + wall.apply(correction) + let moved = points.map(correction.point) + + #expect(nearlyEqual(wall.meter, SIMD3(newPose.columns.3.x, newPose.columns.3.y, newPose.columns.3.z))) + for (point, after) in zip(points, moved) { + #expect(nearlyEqual(Self.anchorLocal(after, newPose), Self.anchorLocal(point, pose))) + } + for (tap, after) in zip(taps, moved) { + let now = wall.wallPoint(after) + #expect(nearlyEqual(now.s, tap.s)) + #expect(nearlyEqual(now.height, tap.height)) + #expect(nearlyEqual(now.out, tap.out)) + } + let stale = wall.wallPoint(points[1]) + #expect(abs(stale.s - taps[1].s) > 0.05) + } + + /// The drift log's totals: every correction applied since the meter was anchored, and none + /// still waiting under the 2 cm and 0.4 degree thresholds. + @Test func correctionsSinceAnchoredAddUp() throws { + let pose = MeterAnchorCorrectionTests.wallHitPose(meter: SIMD3(0.4, 1.5, -0.2), outward: SIMD3(0, 0, 1)) + var tracking = MeterAnchorTracking(pose: pose) + #expect(tracking.corrections == 0) + #expect(nearlyEqual(tracking.sinceAnchored.moved, .zero)) + #expect(nearlyEqual(tracking.sinceAnchored.yaw, 0, 1e-6)) + + let first = Self.corrected(pose) + let firstUpdate = tracking.update(to: first) + _ = try #require(firstUpdate) + #expect(tracking.corrections == 1) + #expect(nearlyEqual(tracking.sinceAnchored.moved, SIMD3(0.2, 0, 0.1))) + #expect(nearlyEqual(tracking.sinceAnchored.yaw, 3 * .pi / 180, 1e-5)) + + // Under both thresholds: waits, and isn't in the total yet. + let small = tracking.update(to: MeterAnchorCorrectionTests.turned(first, degrees: 0.1)) + #expect(small == nil) + #expect(tracking.corrections == 1) + #expect(nearlyEqual(tracking.sinceAnchored.yaw, 3 * .pi / 180, 1e-5)) + + var second = first + second.columns.3 += SIMD4(0, 0, -0.05, 0) + let secondUpdate = tracking.update(to: second) + _ = try #require(secondUpdate) + #expect(tracking.corrections == 2) + #expect(nearlyEqual(tracking.sinceAnchored.moved, SIMD3(0.2, 0, 0.05))) + #expect(nearlyEqual(tracking.sinceAnchored.yaw, 3 * .pi / 180, 1e-5)) + #expect(tracking.anchoredPose == pose) + } + + /// A re-fit anchors the meter again: the drift totals start from the new anchor, so the + /// re-fit's own move isn't read as a correction ARKit made, while the correction log that + /// corrects earlier poses stays. + @Test func anchoringAgainStartsTheTotalsButKeepsTheLog() throws { + let pose = MeterAnchorCorrectionTests.wallHitPose(meter: SIMD3(0, 1.5, 0), outward: SIMD3(0, 0, 1)) + var tracking = MeterAnchorTracking(pose: pose) + let first = Self.corrected(pose) + let update = tracking.update(to: first, at: 2) + _ = try #require(update) + #expect(tracking.corrections == 1) + let refitPose = MeterAnchorCorrectionTests.wallHitPose(meter: SIMD3(0.3, 1.5, 0.8), outward: SIMD3(0, 0, 1)) + tracking.anchorAgain(at: refitPose) + #expect(tracking.corrections == 0) + #expect(tracking.anchoredPose == refitPose) + #expect(nearlyEqual(tracking.sinceAnchored.moved, .zero)) + #expect(tracking.log.count == 1) + var later = refitPose + later.columns.3 += SIMD4(0, 0, 0.05, 0) + let next = tracking.update(to: later, at: 3) + _ = try #require(next) + #expect(tracking.corrections == 1) + #expect(nearlyEqual(tracking.sinceAnchored.moved, SIMD3(0, 0, 0.05))) + #expect(tracking.log.count == 2) + } + + /// The drift log's anchor lines: one for the first frame, one per change, none while the + /// frames go on the same, and a count of every frame. + @Test func anchorPresenceReportsOnlyChanges() { + var presence = MeterAnchorPresence() + #expect(presence.last == nil) + let frames: [MeterAnchorPresence.Sighting] = [.missing, .present, .present, .present, .missing, .missing, .otherAnchor, .present] + var lines: [MeterAnchorPresence.Sighting?] = [] + for sighting in frames { lines.append(presence.observe(sighting)) } + #expect(lines == [.missing, .present, nil, nil, .missing, nil, .otherAnchor, .present]) + #expect(presence.last == .present) + #expect(presence.present == 4) + #expect(presence.missing == 3) + #expect(presence.otherAnchor == 1) + #expect(MeterAnchorPresence.Sighting.otherAnchor.rawValue == "other anchor") + } +}