diff --git a/ios/HouseScan/Runtime/LiveCapture.swift b/ios/HouseScan/Runtime/LiveCapture.swift index 1a0dcf4c3..027b7e746 100644 --- a/ios/HouseScan/Runtime/LiveCapture.swift +++ b/ios/HouseScan/Runtime/LiveCapture.swift @@ -425,18 +425,19 @@ final class LiveSessionDelegate: NSObject, ARSessionDelegate, Sendable { guard count % Self.sampleEvery == 0 else { let pose = SourceFrame( id: "live-\(count)", timestamp: frame.timestamp, camera: camera, tracking: tracking, - quality: nil, jpeg: .none, still: nil, meterAnchor: meterAnchor, isPoseOnly: true + quality: nil, jpeg: .none, still: nil, meterAnchor: meterAnchor, meterAnchorID: shared.meterAnchorID, isPoseOnly: true ) encodeQueue.async { [self, pose] in deliver(pose) } return } let quality = Self.quality(frame.capturedImage) - let ground = frame.anchors.compactMap { $0 as? ARPlaneAnchor } - .filter { $0.alignment == .horizontal } - .map(Self.groundEvidence) + let planes = frame.anchors.compactMap { $0 as? ARPlaneAnchor } + let ground = planes.filter { $0.alignment == .horizontal }.map(Self.groundEvidence) + let walls = planes.filter { $0.alignment == .vertical }.map(Self.wallEvidence) var snapshot = SourceFrame( id: "live-\(count)", timestamp: frame.timestamp, camera: camera, tracking: tracking, - quality: quality, jpeg: .none, still: nil, meterAnchor: meterAnchor, groundPlanes: ground + quality: quality, jpeg: .none, still: nil, meterAnchor: meterAnchor, meterAnchorID: shared.meterAnchorID, + groundPlanes: ground, wallPlanes: walls ) let wantsSpatial = self.shared.withLock { state in defer { state.wantsSpatial = false } @@ -631,7 +632,28 @@ final class LiveSessionDelegate: NSObject, ARSessionDelegate, Sendable { let world = plane.transform * SIMD4(vertex, 1) return SIMD2(world.x, world.z) } - return GroundPlaneEvidence(y: center.y, kind: kind, boundary: boundary) + return GroundPlaneEvidence(y: center.y, kind: kind, boundary: boundary, id: plane.identifier.uuidString) + } + + /// A vertical plane as the meter tap's checks read it (`MeterTap`): its class, a point on it, + /// its normal (the anchor's y axis) and its outline, world meters. On a phone without plane + /// classification every plane is `.none`, so every one is unclassified. + private static func wallEvidence(_ plane: ARPlaneAnchor) -> WallPlaneEvidence { + let center = plane.transform * SIMD4(plane.center, 1) + let normal = plane.transform.columns.1 + let kind: WallPlaneEvidence.Kind = switch plane.classification { + case .wall: .wall + case .none: .unclassified + default: .other + } + let boundary = plane.geometry.boundaryVertices.map { vertex -> SIMD3 in + let world = plane.transform * SIMD4(vertex, 1) + return SIMD3(world.x, world.y, world.z) + } + return WallPlaneEvidence( + id: plane.identifier.uuidString, kind: kind, center: SIMD3(center.x, center.y, center.z), + normal: SIMD3(normal.x, normal.y, normal.z), boundary: boundary + ) } // MARK: Session events diff --git a/ios/HouseScan/Runtime/ScanEngine+Actions.swift b/ios/HouseScan/Runtime/ScanEngine+Actions.swift index 3149a6303..fb2812d92 100644 --- a/ios/HouseScan/Runtime/ScanEngine+Actions.swift +++ b/ios/HouseScan/Runtime/ScanEngine+Actions.swift @@ -17,6 +17,7 @@ extension ScanEngine: ScanActions { // A replay has no live surfaces to raycast; its wall comes from the recording (or is // assumed from the trajectory, see ReplayPlayer.wallDescription). let wall = replay.wall + meterTapCamera = nil guard setWall(meter: wall.meter, outward: wall.outward, groundY: wall.groundY, groundMeasured: replay.groundMeasured) else { return } markTimes[MarkKey.meter] = captureClock go(.meterCloseUp) @@ -36,29 +37,48 @@ extension ScanEngine: ScanActions { RuntimeLog.engine.info("meter tap refused: no vertical plane") return } + // An estimated plane can put the wall far from the real one (#69): one out of reach, + // turned from the phone, or behind a detected plane is refused, and the homeowner is asked + // to let ARKit find the wall first. + if let refusal = MeterTap.refusal( + hit: hit.position, normal: hit.normal, source: hit.source, camera: frame.camera, planes: detectedWallPlanes + ) { + state.guidance = .aimAtWallForMeter + RuntimeLog.engine.info("meter tap refused: \(refusal.description, privacy: .public)") + return + } meterPlaneSource = hit.source + meterTapCamera = frame.camera.position var outward = SIMD3(hit.normal.x, 0, hit.normal.z) if simd_dot(outward, frame.camera.position - hit.position) < 0 { outward = -outward } // Until a horizontal plane shows up below the wall, the ground is a guess: a phone held at - // chest height, 1.4 m above it. `refineGround` replaces the guess as planes arrive. - let measured = groundBelow(hit.position, along: simd_normalize(simd_cross(-outward, SIMD3(0, 1, 0)))) - guard setWall(meter: hit.position, outward: outward, groundY: measured ?? frame.camera.position.y - 1.4, groundMeasured: measured != nil) else { + // chest height, 1.4 m above it. `SpatialUpdate` replaces the guess as planes arrive. + let ground = groundBelow(hit.position, along: simd_normalize(simd_cross(-outward, SIMD3(0, 1, 0))), current: nil) + guard setWall(meter: hit.position, outward: outward, groundY: ground?.plane.y ?? frame.camera.position.y - 1.4, groundMeasured: ground != nil) else { state.guidance = .aimAtWallForMeter return } + let groundNote = ground.map(Self.describe) ?? "a guess" + RuntimeLog.engine.info("meter marked on \(hit.source == .estimatedPlane ? "an estimated" : "a detected", privacy: .public) plane; ground from \(groundNote, privacy: .public)") // The map carries the line's source: the export writes it and the walked clearance // takes the server's error for it. updateCoverage { $0.setWallLineSource(meterLineSource) } setMeterAnchor(live.addMeterAnchor(at: hit.transform), pose: hit.transform) markTimes[MarkKey.meter] = captureClock go(.meterCloseUp) + // A wall plane ARKit already knows may disagree with an estimated hit; waiting for the + // planes to change would leave the close-up, and maybe the walk, on the estimated wall. + refitWallToDetectedPlane() } /// The ground at the wall of the meter at `meter`, running along `along`: a detected plane - /// that reaches the wall's foot by the meter and isn't furniture, floor-classified first, - /// then the lowest (`GroundPlaneChoice`). Nil when none does; the ground stays a guess. - func groundBelow(_ meter: SIMD3, along: SIMD3) -> Float? { - GroundPlaneChoice.groundY(meter: meter, along: along, planes: detectedGroundPlanes) + /// the meter is a plausible height above, that reaches the wall's foot by the meter and isn't + /// furniture, floor-classified first, then the one under where the phone stood to mark the + /// meter, then the lowest (`GroundPlaneChoice`). With `current`, the ground already measured, + /// it is never raised more than `GroundPlaneChoice.maximumRaise`. Nil when no plane qualifies; + /// the ground stays as it is. + func groundBelow(_ meter: SIMD3, along: SIMD3, current: Float?) -> GroundPlaneChoice.Choice? { + GroundPlaneChoice.choose(meter: meter, along: along, phone: meterTapCamera, current: current, planes: detectedGroundPlanes) } func skipCloseUp() { diff --git a/ios/HouseScan/Runtime/ScanEngine.swift b/ios/HouseScan/Runtime/ScanEngine.swift index 60013075d..5ca06a55e 100644 --- a/ios/HouseScan/Runtime/ScanEngine.swift +++ b/ios/HouseScan/Runtime/ScanEngine.swift @@ -19,6 +19,10 @@ final class ScanEngine { /// Which kind of plane the meter tap hit; an estimated plane widens the meter's error in the /// export. A replay's wall comes from the recording, so it counts as detected. var meterPlaneSource: MeterPlaneSource = .detectedPlane + /// Where the phone was when the meter was marked: the ground choice prefers the plane under + /// it, and a re-fit follows the tap's line of sight from it (`refitWallToDetectedPlane`). Nil + /// on a replay. + var meterTapCamera: SIMD3? private var live: LiveCapture? // Capture logic (HouseScanKit) @@ -54,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 @@ -69,6 +76,8 @@ final class ScanEngine { private(set) var lastUploadSnapshot: UploadSnapshot? /// Detected horizontal planes, with their classes and outlines. private var groundPlanes: [GroundPlaneEvidence] = [] + /// Detected vertical planes, with their classes, normals and outlines. + private var wallPlanes: [WallPlaneEvidence] = [] private var lastFrame: SourceFrame? /// Whether `WallFrame.groundY` comes from a detected plane (or a recording's wall taps) rather /// than the chest-height guess, and the guess's error. The export widens position errors @@ -232,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 @@ -371,7 +382,14 @@ 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. + if let planes = frame.wallPlanes, planes != wallPlanes { + wallPlanes = planes + refitWallToDetectedPlane() + } // After the frame's corrections, so a snapshot taken on it agrees with its mesh. completeUploadSnapshot(with: frame) guard !frame.isPoseOnly else { return } @@ -395,14 +413,19 @@ final class ScanEngine { /// ARKit's correction to the meter's anchor, then the ground from the planes this frame /// reports, in that order (`SpatialUpdate`): the correction moves everything captured (the - /// wall and corners, the kept cameras, and here the tapped marks) as one body into the frame - /// ARKit reports now, and the ground is then read in that same frame. A plane that stops - /// supporting the ground (reclassified, removed) puts it back to a guess. + /// wall and corners, the kept cameras, where the phone stood to mark the meter, and here the + /// tapped marks) as one body into the frame ARKit reports now, and the ground is then read in + /// that same frame. A plane that stops supporting the ground (reclassified, removed) puts it + /// back to a guess, and once the walk has kept a view the ground is never raised by more than + /// `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 let outcome = SpatialUpdate.apply( - anchor: frame.meterAnchor, planes: frame.groundPlanes, time: frame.timestamp, + anchor: anchor, planes: frame.groundPlanes, time: frame.timestamp, phone: &meterTapCamera, map: &map, tracking: &meterTracking, ground: &groundEvidence) guard outcome.changed else { return } spatialRevision += 1 @@ -413,15 +436,53 @@ 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 { - RuntimeLog.engine.info("ground \(self.groundEvidence.measured ? "measured" : "a guess again", privacy: .public) at y=\(map.wall.groundY) (was \(before.y), \(before.measured ? "measured" : "estimated", privacy: .public))") + let source = outcome.groundChoice.map(Self.describe) ?? "no plane that qualifies" + RuntimeLog.engine.info("ground \(self.groundEvidence.measured ? "measured" : "a guess again", privacy: .public) at y=\(map.wall.groundY) from \(source, privacy: .public) (was \(before.y), \(before.measured ? "measured" : "estimated", privacy: .public))") } publishWall() publishCoverage() reprojectFeatures() } + /// A ground choice for the log: the plane's id, its class and why it was chosen. + nonisolated static func describe(_ choice: GroundPlaneChoice.Choice) -> String { + "plane \(choice.plane.id) (\(choice.plane.kind), \(choice.reason.rawValue))" + } + + /// Moves the wall to a detected wall plane that disagrees with a meter tap on an estimated + /// plane (`MeterTap.refit`): the meter goes where the tap's line of sight meets the plane, the + /// wall faces the plane's normal, and the meter is anchored again there. Tried on entering the + /// close-up, with the planes already known, and whenever the planes change during it. Only + /// during the close-up, before the walk has kept any view: coverage can't turn a wall it has + /// already seen (`CoverageMap.updateWall`), so a wall found wrong later stays wrong. + func refitWallToDetectedPlane() { + guard state.phase == .meterCloseUp, replay == nil, meterPlaneSource == .estimatedPlane, + let live, let tapCamera = meterTapCamera, let map = coverage, + let refit = MeterTap.refit(meter: map.wall.meter, outward: map.wall.outward, tapCamera: tapCamera, planes: wallPlanes) else { return } + let wall = map.wall + let along = simd_normalize(simd_cross(-refit.outward, SIMD3(0, 1, 0))) + // The same raise limit as every other ground update. No view is kept during the close-up, + // so it is nil here: the ground at the moved wall's foot may be higher than at the old one. + let choice = groundBelow(refit.meter, along: along, current: SpatialUpdate.raiseLimit(map: map, ground: groundEvidence)) + // No plane qualifies at the moved wall: the old ground stays for now, and the next frame's + // planes measure it again for the new wall or put it back to a guess (`SpatialUpdate`). + let measured = choice != nil || groundMeasured + guard setWall(meter: refit.meter, outward: refit.outward, groundY: choice?.plane.y ?? wall.groundY, groundMeasured: measured) else { return } + meterPlaneSource = .detectedPlane + updateCoverage { $0.setWallLineSource(meterLineSource) } + meterAnchorID.map { live.removeAnchor($0) } + // Axes as a wall hit's: y the wall's normal, z up the wall. + let x = simd_normalize(simd_cross(refit.outward, SIMD3(0, 1, 0))) + let pose = simd_float4x4(SIMD4(x, 0), SIMD4(refit.outward, 0), SIMD4(simd_cross(x, refit.outward), 0), SIMD4(refit.meter, 1)) + anchorMeterAgain(live.addMeterAnchor(at: pose), pose: pose) + let degrees = refit.turned * 180 / .pi + let ground = choice.map(Self.describe) ?? "kept" + RuntimeLog.engine.info("wall re-fitted to detected plane \(refit.planeID, privacy: .public): meter moved \(refit.moved) m, wall turned \(degrees) degrees; ground \(ground, privacy: .public)") + } + /// A raw pose ARKit reported at `time`, in the frame the wall agrees with now. The same raw /// pose without an anchor (a replay). func correctedPose(_ raw: simd_float4x4, capturedAt time: Double) -> simd_float4x4 { @@ -439,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 @@ -448,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 } @@ -1136,6 +1269,7 @@ final class ScanEngine { resetPacketLog() relocalizingSince = nil groundPlanes = [] + wallPlanes = [] groundMeasured = false lastFrame = nil // A fresh map: the old world frame is gone, so its anchors and planes are meaningless. @@ -1143,7 +1277,10 @@ final class ScanEngine { coverage = nil meterAnchorID.map { live?.removeAnchor($0) } meterAnchorID = nil + meterTracking = nil + meterAnchorPresence = MeterAnchorPresence() meterPlaneSource = .detectedPlane + meterTapCamera = nil state.wall = nil state.coverage = .empty state.target = nil @@ -1636,7 +1773,10 @@ final class ScanEngine { coverage = nil meterAnchorID.map { live?.removeAnchor($0) } meterAnchorID = nil + meterTracking = nil + meterAnchorPresence = MeterAnchorPresence() meterPlaneSource = .detectedPlane + meterTapCamera = nil store = KeyframeStore() recorder = Self.makeRecorder(store) live?.setRecorder(recorder) @@ -1703,9 +1843,24 @@ 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. 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 { + meterTracking?.anchorAgain(at: pose) + } } var detectedGroundPlanes: [GroundPlaneEvidence] { groundPlanes } + var detectedWallPlanes: [WallPlaneEvidence] { wallPlanes } /// The camera failed after the scan was sent: no frame will come to say tracking was lost, so /// the last one's "normal" would keep the AR result up and offered. The answer stays; the @@ -1732,9 +1887,12 @@ final class ScanEngine { state.feed = .none // What the retired source saw is in its own world frame; the next source starts another. groundPlanes = [] + wallPlanes = [] + meterTapCamera = nil groundMeasured = false lastFrame = nil meterTracking = nil + meterAnchorPresence = MeterAnchorPresence() meterAnchorID = nil state.projection = nil state.tracking = .notAvailable diff --git a/ios/HouseScan/Runtime/SourceFrame.swift b/ios/HouseScan/Runtime/SourceFrame.swift index 257b4128d..8d1fd75d0 100644 --- a/ios/HouseScan/Runtime/SourceFrame.swift +++ b/ios/HouseScan/Runtime/SourceFrame.swift @@ -20,10 +20,17 @@ struct SourceFrame: Sendable { var still: CGImage? /// World transform of the meter's ARAnchor in this frame, when one exists. var meterAnchor: simd_float4x4? + /// Which anchor `meterAnchor` is. A frame made before the meter was anchored again + /// (`ScanEngine.refitWallToDetectedPlane`) still carries the old one's pose, which the engine + /// must not take as a correction of the new one. + var meterAnchorID: UUID? /// Detected horizontal planes, with their classes and outlines, on a frame that reports them: /// empty when ARKit has found none (or removed the ones it had). Nil on a frame that doesn't /// report planes (a pose-only frame, a replay), which says nothing about the ground. var groundPlanes: [GroundPlaneEvidence]? = nil + /// Detected vertical planes, with their classes, normals and outlines, on a frame that reports + /// them: empty when ARKit has found none. Nil on a frame that doesn't report planes. + var wallPlanes: [WallPlaneEvidence]? = nil /// LiDAR depth copied with the photo of a frame that could be kept; nil without LiDAR, and on /// frames without a photo. Coverage reads it; a replay's recorded depth plays in here too. var depth: DepthImage? diff --git a/ios/HouseScan/UI/Copy/ScanCopy.swift b/ios/HouseScan/UI/Copy/ScanCopy.swift index 725f7c823..74cc875bd 100644 --- a/ios/HouseScan/UI/Copy/ScanCopy.swift +++ b/ios/HouseScan/UI/Copy/ScanCopy.swift @@ -22,7 +22,7 @@ enum ScanCopy { detail: "A gray box with a round glass dial or a small screen, usually on an outside wall." ) case .aimAtWallForMeter: - Instruction(title: "Step a little closer to the wall", detail: "Then aim at your meter again.") + Instruction(title: "Hold on, finding your wall", detail: "Move your phone slowly side to side, then aim at your meter again.") case .holdOnMeter: Instruction(title: "Hold your meter in the circle", detail: "Your phone takes the photo by itself.") case .walk(let side, let remaining): diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Geometry/GroundPlaneChoice.swift b/ios/HouseScanKit/Sources/HouseScanKit/Geometry/GroundPlaneChoice.swift index 5ac5c03c8..1c10a47ca 100644 --- a/ios/HouseScanKit/Sources/HouseScanKit/Geometry/GroundPlaneChoice.swift +++ b/ios/HouseScanKit/Sources/HouseScanKit/Geometry/GroundPlaneChoice.swift @@ -19,11 +19,14 @@ public struct GroundPlaneEvidence: Sendable, Equatable { public var kind: Kind /// The plane's boundary in plan, [x, z] per vertex, in order around it. public var boundary: [SIMD2] + /// ARKit's anchor identifier, for the log. Empty when there is none. + public var id: String - public init(y: Float, kind: Kind, boundary: [SIMD2]) { + public init(y: Float, kind: Kind, boundary: [SIMD2], id: String = "") { self.y = y self.kind = kind self.boundary = boundary + self.id = id } } @@ -32,9 +35,19 @@ public struct GroundPlaneEvidence: Sendable, Equatable { /// height (`CoverageMap.heightError`). A tabletop taken for ground would move every height in the /// scene to the wrong datum with no error to show it, so this errs toward keeping the guess. public enum GroundPlaneChoice { - /// The ground lies at least this far below the meter. A meter sits well above the ground; a - /// plane nearer it is a sill, a step or a ledge. Unchanged from before. - public static let minimumDrop: Float = 0.3 + /// How far above the ground the meter may be, meters: 0.9 to 2.0 m (about 3 ft to 6 ft 7 in). + /// A plane that puts the meter lower is a sill, a step, a ledge or a tabletop; one that puts it + /// higher is a lower yard or a street past the wall's own ground. A guess from where meters + /// are usually mounted, not measured: no survey of meter heights exists for this app. It + /// replaces the old 0.3 m minimum drop, which let a folding table 0.5 m under the meter pass + /// (#62). + public static let meterHeight: ClosedRange = 0.9...2.0 + /// Once the ground is measured and the walk has kept a view (`SpatialUpdate.raiseLimit`), a + /// later plane may not raise it by more than this, meters. A raise re-projects every coverage + /// row, so cells the homeowner was just told were done go back to unseen (#62); a real floor + /// ARKit refines moves by a few centimetres, not 10. A lower plane may still replace the + /// current one. A guess, not measured. + public static let maximumRaise: Float = 0.1 /// How far along the wall either side of the meter the plane's edge is looked for, meters: the /// stretch where batteries are tried first, and where the meter's own foot is. public static let alongMeter: Float = 2 @@ -46,18 +59,54 @@ public enum GroundPlaneChoice { /// measurement of ARKit's plane edges at a wall exists. public static let footReach: Float = 0.5 + /// The plane taken as ground, and why that one. + public struct Choice: Sendable, Equatable { + public enum Reason: String, Sendable, Equatable { + /// Its outline contains where the phone stood when the meter was marked: the ground + /// the homeowner was standing on. + case underThePhone + /// Its outline contains the point on the ground straight below the meter. + case atTheMetersFoot + /// Neither holds for any plane considered; the lowest of them. + case lowest + } + + public var plane: GroundPlaneEvidence + public var reason: Reason + + public init(plane: GroundPlaneEvidence, reason: Reason) { + self.plane = plane + self.reason = reason + } + } + /// The ground's y at the wall whose meter is at `meter`, running along `along`, or nil when no - /// plane qualifies. A plane qualifies when it lies at least `minimumDrop` below the meter, is - /// not classified as furniture or anything but floor or unclassified, and its outline comes - /// within `footReach` of the wall's foot line within `alongMeter` of the meter. Planes - /// classified floor are preferred; among those considered, the lowest wins: a raised surface - /// by the wall (a planter's rim, a low table) is higher than the ground it stands on. + /// plane qualifies (`choose` without a phone position or a current ground). public static func groundY(meter: SIMD3, along: SIMD3, planes: [GroundPlaneEvidence]) -> Float? { + choose(meter: meter, along: along, phone: nil, current: nil, planes: planes)?.plane.y + } + + /// The ground plane at the wall whose meter is at `meter`, running along `along`, or nil when + /// no plane qualifies. A plane qualifies when the meter is `meterHeight` above it, it is not + /// classified as furniture or anything but floor or unclassified, and its outline comes within + /// `footReach` of the wall's foot line within `alongMeter` of the meter. With `current`, the + /// ground already measured, a plane more than `maximumRaise` above it doesn't qualify either. + /// + /// Planes classified floor are preferred. Among those considered, one whose outline contains + /// `phone` (where the phone stood when the meter was marked, in plan) wins, then one that + /// contains the meter's foot, then the lowest: a raised surface by the wall (a planter's rim, a + /// low table) is higher than the ground it stands on. Containment comes first because the + /// lowest can be wrong where the ground steps down: in a field run the ground at the wall was + /// the higher of two planes classified floor, and the other lay 0.25 m below it. + public static func choose( + meter: SIMD3, along: SIMD3, phone: SIMD3?, current: Float?, planes: [GroundPlaneEvidence] + ) -> Choice? { let direction = simd_normalize(SIMD2(along.x, along.z)) let foot = SIMD2(meter.x, meter.z) let line = (foot - direction * alongMeter, foot + direction * alongMeter) let qualifying = planes.filter { plane in - guard plane.y <= meter.y - minimumDrop else { return false } + guard meterHeight.contains(meter.y - plane.y) else { return false } + if let current, plane.y > current + maximumRaise { return false } switch plane.kind { case .floor, .unclassified: break case .furniture, .other: return false @@ -65,7 +114,15 @@ public enum GroundPlaneChoice { return distance(from: plane.boundary, to: line) <= footReach } let floors = qualifying.filter { $0.kind == .floor } - return (floors.isEmpty ? qualifying : floors).map(\.y).min() + let lowestFirst = (floors.isEmpty ? qualifying : floors).sorted { $0.y < $1.y } + guard let lowest = lowestFirst.first else { return nil } + if let phone, let plane = lowestFirst.first(where: { contains($0.boundary, SIMD2(phone.x, phone.z)) }) { + return Choice(plane: plane, reason: .underThePhone) + } + if let plane = lowestFirst.first(where: { contains($0.boundary, foot) }) { + return Choice(plane: plane, reason: .atTheMetersFoot) + } + return Choice(plane: lowest, reason: .lowest) } /// Plan distance between a polygon and a segment: 0 when the segment crosses or lies inside it. diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Geometry/MeterAnchorTracking.swift b/ios/HouseScanKit/Sources/HouseScanKit/Geometry/MeterAnchorTracking.swift index c0d21ed4b..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,10 +120,22 @@ 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, 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 /// correction applied after it, in order. A frame's own anchor is already the corrected one, /// so a correction applied on the frame at `time` itself doesn't apply to it. @@ -123,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/Sources/HouseScanKit/Geometry/MeterTap.swift b/ios/HouseScanKit/Sources/HouseScanKit/Geometry/MeterTap.swift new file mode 100644 index 000000000..cfb30f54a --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Geometry/MeterTap.swift @@ -0,0 +1,179 @@ +import Foundation +import simd + +/// Checks on the meter tap's raycast hit, and the re-fit of the wall to a detected wall plane +/// before the walk (#69). +/// +/// A hit on a plane ARKit detected is taken as it is. A hit on a plane ARKit only estimated from +/// feature points around the tap can be far off: in a field run one sat 1.1 m behind the real wall +/// and turned 46 degrees from it, and the whole scan was built on a wall that isn't there. So an +/// estimated hit is refused when it is out of reach, turned away from the phone, or behind a +/// detected plane the phone looks through to reach it. Every threshold is a guess, not measured; +/// a field test counts refusals of good taps to tune them. +public enum MeterTap { + /// How far from the phone the meter may be, meters, for an estimated hit to be taken, and for + /// a detected wall plane to count as the one the phone was looking at. The app asks for the + /// meter from close by; the close-up is taken from about 0.5 m. A guess. + public static let maximumReach: Float = 2 + /// The most an estimated plane may be turned from facing the phone, radians: the horizontal + /// angle between its normal and the phone's view, reversed. Someone marking a meter faces it + /// roughly square-on. 30 degrees is a guess; the field run's bad hit was 46 degrees off. + public static let maximumFacingAngle: Float = 30 * .pi / 180 + /// Below this horizontal length of the phone's unit view direction (pitched more than 60 + /// degrees up or down) the view says too little about which way the wall faces, and the facing + /// check is skipped. + public static let minimumLevelView: Float = 0.5 + /// How far behind a detected plane an estimated hit may lie, meters, where the phone looks + /// through that plane to reach it. A meter stands proud of its wall by a few inches, so a hit in + /// front of a plane is never refused; one this far behind the surface the phone sees first is + /// not on it. A guess. + public static let behindTolerance: Float = 0.1 + /// How far beyond a detected plane's outline its surface is still taken to be there, meters. + /// ARKit's outlines stop short of a surface's real edge. A guess. + public static let coverMargin: Float = 0.25 + /// A detected wall plane moves the wall to it once the meter would move more than this, + /// meters, or the wall would turn more than `refitAngle`. Under both, the tap agrees with it + /// well enough. Guesses: 0.2 m and 10 degrees are what the field analysis proposed. + public static let refitOffset: Float = 0.2 + public static let refitAngle: Float = 10 * .pi / 180 + /// How far past the tapped point, along the tap's line of sight, a detected wall plane may lie + /// and still take the meter, meters. The tap passes a hit in front of a detected plane because + /// a meter stands proud of its wall; a wall plane much farther back is the wall behind a + /// meter on a pedestal or a garden wall, not a better fit for the tap. A meter box's depth + /// with some to spare; a guess. The field run's bad hit lay behind the real wall, so the + /// crossing was in front of the tap, which this doesn't limit. + public static let refitBehindTap: Float = 0.3 + + /// Why an estimated hit was refused, for the log. + public enum Refusal: Sendable, Equatable, CustomStringConvertible { + /// The hit is this many meters from the phone. + case tooFar(meters: Float) + /// The hit plane is turned this many degrees from facing the phone. + case turnedFromPhone(degrees: Float) + /// The hit lies this many meters behind the detected plane with id `planeID`, which the + /// phone's line of sight crosses first. + case behindDetectedPlane(planeID: String, meters: Float) + + public var description: String { + switch self { + case .tooFar(let meters): + "estimated plane \(MeterTap.format(meters)) m from the phone (over \(MeterTap.format(MeterTap.maximumReach)) m)" + case .turnedFromPhone(let degrees): + "estimated plane turned \(MeterTap.format(degrees)) degrees from the phone's view" + case .behindDetectedPlane(let planeID, let meters): + "estimated plane \(MeterTap.format(meters)) m behind detected plane \(planeID)" + } + } + } + + /// Why the tap's hit at `hit`, on a plane with normal `normal`, can't be the meter, or nil when + /// it can. A hit on a detected plane always can. `camera` is the frame the tap was made on; + /// `planes` are the vertical planes ARKit has detected. Only planes classified wall or not + /// classified can refuse a hit: a door or a window may stand in front of a wall. + public static func refusal( + hit: SIMD3, normal: SIMD3, source: MeterPlaneSource, camera: CameraFrame, planes: [WallPlaneEvidence] + ) -> Refusal? { + switch source { + case .detectedPlane: return nil + case .estimatedPlane: break + } + let reach = simd_distance(hit, camera.position) + if reach > maximumReach { return .tooFar(meters: reach) } + let view = camera.forward + let towardPhone = SIMD3(-view.x, 0, -view.z) + let flatNormal = SIMD3(normal.x, 0, normal.z) + if simd_length(towardPhone) >= minimumLevelView, simd_length(flatNormal) > 1e-3 { + let cosine = abs(simd_dot(simd_normalize(towardPhone), simd_normalize(flatNormal))) + let angle = acos(min(1, cosine)) + if angle > maximumFacingAngle { return .turnedFromPhone(degrees: angle * 180 / .pi) } + } + for plane in planes where plane.kind != .other { + guard let crossing = sightCrossing(from: camera.position, through: hit, plane: plane), + crossing.depthBehind > behindTolerance, plane.covers(crossing.point, margin: coverMargin) else { continue } + return .behindDetectedPlane(planeID: plane.id, meters: crossing.depthBehind) + } + return nil + } + + /// Where a detected wall plane puts the meter and the wall, when it disagrees with a tap on an + /// estimated plane. + public struct Refit: Sendable, Equatable { + /// Where the tap's line of sight meets the plane. + public var meter: SIMD3 + /// The plane's horizontal normal, facing the side the tap was made from. + public var outward: SIMD3 + public var planeID: String + /// How far the meter moves, meters. + public var moved: Float + /// How far the wall turns, radians. + public var turned: Float + + public init(meter: SIMD3, outward: SIMD3, planeID: String, moved: Float, turned: Float) { + self.meter = meter + self.outward = outward + self.planeID = planeID + self.moved = moved + self.turned = turned + } + } + + /// The wall a detected wall plane gives, for a meter marked at `meter` on an estimated plane + /// facing `outward`, from a phone at `tapCamera`; nil when no detected wall plane disagrees + /// with the tap. The meter was on the tap's line of sight, wherever the estimated plane put it + /// along it, so the plane that line crosses first, within `maximumReach` of the phone and + /// within the plane's outline, is the surface the homeowner aimed at. It disagrees when the + /// meter would move more than `refitOffset` to it or the wall would turn more than + /// `refitAngle`. Only planes classified wall count: an unclassified one near a meter may be + /// the meter's own board, or a fence. A plane the line crosses more than `refitBehindTap` past + /// the tapped point doesn't count either. + public static func refit(meter: SIMD3, outward: SIMD3, tapCamera: SIMD3, planes: [WallPlaneEvidence]) -> Refit? { + let flat = SIMD3(outward.x, 0, outward.z) + guard simd_length(flat) > 1e-3 else { return nil } + let reach = simd_distance(meter, tapCamera) + var nearest: (plane: WallPlaneEvidence, crossing: SightCrossing)? + for plane in planes where plane.kind == .wall { + guard let crossing = sightCrossing(from: tapCamera, through: meter, plane: plane), + simd_distance(crossing.point, tapCamera) <= maximumReach, + (crossing.fraction - 1) * reach <= refitBehindTap, + plane.covers(crossing.point, margin: coverMargin) else { continue } + if let best = nearest, best.crossing.fraction <= crossing.fraction { continue } + nearest = (plane: plane, crossing: crossing) + } + guard let nearest else { return nil } + let turned = acos(min(1, abs(simd_dot(simd_normalize(flat), nearest.crossing.normal)))) + let moved = simd_distance(nearest.crossing.point, meter) + guard moved > refitOffset || turned > refitAngle else { return nil } + return Refit(meter: nearest.crossing.point, outward: nearest.crossing.normal, planeID: nearest.plane.id, moved: moved, turned: turned) + } + + /// Where the line of sight from `eye` through `target` meets a vertical plane. + struct SightCrossing { + var point: SIMD3 + /// The plane's horizontal normal, turned toward the eye. + var normal: SIMD3 + /// How far along from the eye to the target the crossing is: under 1 before the target. + var fraction: Float + /// How far the target lies behind the plane as the eye sees it, meters; negative in front. + var depthBehind: Float + } + + /// Nil when the plane isn't vertical, the eye is on it, or the line of sight runs along it or + /// away from it. + static func sightCrossing(from eye: SIMD3, through target: SIMD3, plane: WallPlaneEvidence) -> SightCrossing? { + guard var normal = plane.horizontalNormal else { return nil } + var eyeSide = simd_dot(eye - plane.center, normal) + if eyeSide < 0 { + normal = -normal + eyeSide = -eyeSide + } + let targetSide = simd_dot(target - plane.center, normal) + let closing = eyeSide - targetSide + guard eyeSide > 1e-3, closing > 1e-4 else { return nil } + let fraction = eyeSide / closing + return SightCrossing(point: eye + (target - eye) * fraction, normal: normal, fraction: fraction, depthBehind: -targetSide) + } + + static func format(_ value: Float) -> String { + String(format: "%.2f", Double(value)) + } +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Geometry/SpatialUpdate.swift b/ios/HouseScanKit/Sources/HouseScanKit/Geometry/SpatialUpdate.swift index bd8bd992b..578e5e83c 100644 --- a/ios/HouseScanKit/Sources/HouseScanKit/Geometry/SpatialUpdate.swift +++ b/ios/HouseScanKit/Sources/HouseScanKit/Geometry/SpatialUpdate.swift @@ -30,6 +30,8 @@ public enum SpatialUpdate { public var correction: YawCorrection? /// The ground moved, or became measured or a guess again. public var groundChanged = false + /// The plane taken as ground and why, when the ground moved to one or became measured. + public var groundChoice: GroundPlaneChoice.Choice? /// Whether the captured geometry changed: an answer to a scene sent before it is stale /// (`AnswerFreshness`). @@ -39,24 +41,51 @@ public enum SpatialUpdate { /// Ground moves under 1 cm are plane jitter and republish nothing. public static let groundJitter: Float = 0.01 + /// The height a later plane may not raise the ground by more than + /// `GroundPlaneChoice.maximumRaise`: the measured ground, once coverage has kept a view. A + /// raise re-projects every row, so cells the homeowner was told were done would go back to + /// unseen (#62). Before any view is kept (the meter mark and its close-up) there are no such + /// cells, so a plane found later may still raise it: the one under where the phone stood, + /// arriving after a lower one reached the foot line first, or the ground at the foot of a wall + /// a re-fit moved. Nil while the ground is a guess, or no view is kept. + public static func raiseLimit(map: CoverageMap, ground: GroundEvidence) -> Float? { + ground.measured && !map.observedCameras.isEmpty ? map.wall.groundY : nil + } + + /// `apply` without where the phone stood when the meter was marked. + public static func apply( + anchor: simd_float4x4?, planes: [GroundPlaneEvidence]?, time: Double, + map: inout CoverageMap, tracking: inout MeterAnchorTracking?, ground: inout GroundEvidence + ) -> Outcome { + var phone: SIMD3? + return apply(anchor: anchor, planes: planes, time: time, phone: &phone, map: &map, tracking: &tracking, ground: &ground) + } + /// `anchor` is the meter anchor's pose on this frame, nil when the frame has none. `planes` /// are the horizontal planes ARKit reports on this frame; nil when the frame carries no plane - /// information (a pose-only frame), empty when ARKit reports none. A measured ground whose - /// supporting plane is gone, or no longer qualifies (reclassified as furniture, shrunk away - /// from the wall), goes back to a guess: the height stays where it was, and the guess's - /// error comes back on every height. + /// information (a pose-only frame), empty when ARKit reports none. `phone` is where the phone + /// stood when the meter was marked, which the ground choice prefers the plane under; a + /// correction moves it with everything else, since it was recorded in the old frame. A + /// measured ground whose supporting plane is gone, or no longer qualifies (reclassified as + /// furniture, shrunk away from the wall, or more than `GroundPlaneChoice.maximumRaise` above it + /// once `raiseLimit` holds), goes back to a guess: the height stays where it was, and the + /// guess's error comes back on every height. public static func apply( - anchor: simd_float4x4?, planes: [GroundPlaneEvidence]?, time: Double, + anchor: simd_float4x4?, planes: [GroundPlaneEvidence]?, time: Double, phone: inout SIMD3?, map: inout CoverageMap, tracking: inout MeterAnchorTracking?, ground: inout GroundEvidence ) -> Outcome { var outcome = Outcome() if let anchor, let correction = tracking?.update(to: anchor, at: time) { map.apply(correction) + phone = phone.map(correction.point) outcome.correction = correction } guard let planes else { return outcome } let wall = map.wall - if let y = GroundPlaneChoice.groundY(meter: wall.meter, along: wall.along, planes: planes) { + if let choice = GroundPlaneChoice.choose( + meter: wall.meter, along: wall.along, phone: phone, current: raiseLimit(map: map, ground: ground), planes: planes + ) { + let y = choice.plane.y guard !ground.measured || abs(y - wall.groundY) > groundJitter else { return outcome } var moved = wall moved.groundY = y @@ -64,6 +93,7 @@ public enum SpatialUpdate { map.heightError = 0 map.updateWall(moved) outcome.groundChanged = true + outcome.groundChoice = choice } else if ground.measured { ground.set(measured: false) map.heightError = ground.guessError diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Geometry/WallPlaneEvidence.swift b/ios/HouseScanKit/Sources/HouseScanKit/Geometry/WallPlaneEvidence.swift new file mode 100644 index 000000000..d4868b5b4 --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Geometry/WallPlaneEvidence.swift @@ -0,0 +1,55 @@ +import Foundation +import simd + +/// A vertical plane ARKit detected, as the meter tap's checks need it: what ARKit classified it +/// as, where it is, which way it faces and its outline. World meters. +public struct WallPlaneEvidence: Sendable, Equatable { + /// ARKit's plane classifications that matter here (`ARPlaneAnchor.Classification`). + public enum Kind: Sendable, Equatable { + case wall + /// `.none`: not classified, which is every plane on a phone without classification. + case unclassified + /// Anything else: a door, a window, or a class added after iOS 26. + case other + } + + /// ARKit's anchor identifier, for the log. + public var id: String + public var kind: Kind + /// A point on the plane: its centre. + public var center: SIMD3 + /// The plane's normal, either way round. A vertical plane's is horizontal. + public var normal: SIMD3 + /// The plane's outline, in order around it. + public var boundary: [SIMD3] + + public init(id: String, kind: Kind, center: SIMD3, normal: SIMD3, boundary: [SIMD3]) { + self.id = id + self.kind = kind + self.center = center + self.normal = normal + self.boundary = boundary + } + + /// The normal made horizontal and unit length, or nil when the plane isn't close to vertical + /// (its normal is more than 60 degrees from horizontal). + var horizontalNormal: SIMD3? { + let flat = SIMD3(normal.x, 0, normal.z) + let length = simd_length(normal) + guard length > 0, simd_length(flat) > 0.5 * length else { return nil } + return simd_normalize(flat) + } + + /// Whether `point`, on or near the plane, lies within its outline or at most `margin` beyond + /// it: along the plane horizontally, and in height. The outline is taken as the box around it + /// in the plane, so a notch in it counts as covered. + func covers(_ point: SIMD3, margin: Float) -> Bool { + guard let normal = horizontalNormal, !boundary.isEmpty else { return false } + let along = SIMD3(-normal.z, 0, normal.x) + let offsets = boundary.map { simd_dot($0 - center, along) } + let heights = boundary.map(\.y) + guard let first = offsets.min(), let last = offsets.max(), let bottom = heights.min(), let top = heights.max() else { return false } + let offset = simd_dot(point - center, along) + return offset >= first - margin && offset <= last + margin && point.y >= bottom - margin && point.y <= top + margin + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/AnchorRepairTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/AnchorRepairTests.swift index 99667ecc3..6eedfc8d2 100644 --- a/ios/HouseScanKit/Tests/HouseScanKitTests/AnchorRepairTests.swift +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/AnchorRepairTests.swift @@ -72,6 +72,29 @@ import Testing } } +/// A re-fit anchors the meter again in the same world frame (`MeterAnchorTracking.anchorAgain`). +@Suite struct AnchorAgainTests { + /// A close-up photo taken at t = 1, a correction at t = 2, then the re-fit at t = 3: the photo + /// still gets the correction made after it, and later corrections are measured from the new + /// anchor, not the tapped one. + @Test func anchoringAgainKeepsTheCorrectionsLogged() throws { + let pose = MeterAnchorCorrectionTests.wallHitPose(meter: SIMD3(0, 1.5, 0), outward: SIMD3(0, 0, 1)) + var tracking = MeterAnchorTracking(pose: pose) + var slid = pose + slid.columns.3 += SIMD4(0.1, 0, 0, 0) + let update = tracking.update(to: slid, at: 2) + let slide = try #require(update) + let photo = wallCamera(s: 0.4).cameraToWorld + let refitPose = MeterAnchorCorrectionTests.wallHitPose(meter: SIMD3(0.1, 1.5, 0.6), outward: SIMD3(0, 0, 1)) + tracking.anchorAgain(at: refitPose) + #expect(tracking.pose == refitPose) + #expect(tracking.correctedPose(photo, capturedAt: 1) == slide.pose(photo)) + #expect(PoseCorrections(tracking).pose(photo, capturedAt: 1) == slide.pose(photo)) + // The new anchor where it was put: no correction. + #expect(tracking.update(to: refitPose, at: 4) == nil) + } +} + @Suite struct SpatialUpdateTests { static let lawn = GroundPlaneEvidence(y: -0.10, kind: .unclassified, boundary: GroundPlaneChoiceTests.rectangle(x: -3...3, z: 0.2...4)) 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") + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/GroundPlaneTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/GroundPlaneTests.swift new file mode 100644 index 000000000..f6ec26412 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/GroundPlaneTests.swift @@ -0,0 +1,168 @@ +import Foundation +@testable import HouseScanKit +import simd +import Testing + +// #62: the ground at the wall was taken from a tabletop, and a later plane could raise a ground +// already measured. Made-up numbers, not field values. The wall is the plane z = 0, running along +// +x, facing +z; the meter is 1.5 m up at the origin. + +@Suite struct GroundPlaneTests { + static let meter = SIMD3(0, 1.5, 0) + static let along = SIMD3(1, 0, 0) + + static func rectangle(x: ClosedRange, z: ClosedRange) -> [SIMD2] { + [SIMD2(x.lowerBound, z.lowerBound), SIMD2(x.upperBound, z.lowerBound), SIMD2(x.upperBound, z.upperBound), SIMD2(x.lowerBound, z.upperBound)] + } + + static func choose(_ planes: [GroundPlaneEvidence], phone: SIMD3? = nil, current: Float? = nil) -> GroundPlaneChoice.Choice? { + GroundPlaneChoice.choose(meter: meter, along: along, phone: phone, current: current, planes: planes) + } + + /// A tabletop 0.5 m under the meter and a floor 1 m below it. The table is refused whatever + /// its class or reach: the meter would be only 0.5 m above it. + @Test func aTabletopNearTheMeterLosesToTheFloor() { + let floor = GroundPlaneEvidence(y: 0, kind: .floor, boundary: Self.rectangle(x: -6...6, z: 0...8), id: "floor") + let farTable = GroundPlaneEvidence(y: 1, kind: .furniture, boundary: Self.rectangle(x: -1...5, z: 1.4...2.4), id: "table") + #expect(Self.choose([farTable, floor])?.plane.id == "floor") + // Unclassified and against the wall, under the phone: still not ground. + let nearTable = GroundPlaneEvidence(y: 1, kind: .unclassified, boundary: Self.rectangle(x: -1...1, z: 0...1), id: "table") + #expect(Self.choose([nearTable], phone: SIMD3(0, 1.4, 0.5)) == nil) + #expect(Self.choose([nearTable, floor], phone: SIMD3(0, 1.4, 0.5))?.plane.id == "floor") + } + + /// Two floors where the ground steps down: the one under the phone that marked the meter wins, + /// though the other is lower. + @Test func theFloorUnderThePhoneWinsOverALowerOne() { + let upper = GroundPlaneEvidence(y: 0, kind: .floor, boundary: Self.rectangle(x: -2...2, z: 0.05...2), id: "upper") + let lower = GroundPlaneEvidence(y: -0.3, kind: .floor, boundary: Self.rectangle(x: 2.3...10, z: 0...10), id: "lower") + let phone = SIMD3(0.3, 1.4, 0.6) + let chosen = Self.choose([lower, upper], phone: phone) + #expect(chosen?.plane.id == "upper") + #expect(chosen?.reason == .underThePhone) + // Without the phone's position neither contains anything; the lowest is taken. + let unplaced = Self.choose([lower, upper]) + #expect(unplaced?.plane.id == "lower") + #expect(unplaced?.reason == .lowest) + } + + /// A floor whose outline contains the point below the meter beats a lower one beside it, + /// unless the phone stood over the other: the phone's position is checked first. + @Test func theFloorAtTheMetersFootWinsOverALowerOne() { + let under = GroundPlaneEvidence(y: 0, kind: .floor, boundary: Self.rectangle(x: -2...2, z: -0.1...2), id: "under") + let lower = GroundPlaneEvidence(y: -0.3, kind: .floor, boundary: Self.rectangle(x: 2.3...10, z: 0...10), id: "lower") + let footOnly = Self.choose([lower, under]) + #expect(footOnly?.plane.id == "under") + #expect(footOnly?.reason == .atTheMetersFoot) + let phoneOverLower = Self.choose([lower, under], phone: SIMD3(3, 1.1, 1)) + #expect(phoneOverLower?.plane.id == "lower") + #expect(phoneOverLower?.reason == .underThePhone) + } + + /// The meter must be 0.9 to 2.0 m above the plane. + @Test func aPlaneThatPutsTheMeterTooLowOrTooHighIsNotGround() { + func plane(_ y: Float) -> GroundPlaneEvidence { + GroundPlaneEvidence(y: y, kind: .floor, boundary: Self.rectangle(x: -3...3, z: 0...3), id: "y\(y)") + } + #expect(Self.choose([plane(1.1)]) == nil) + #expect(Self.choose([plane(-1)]) == nil) + #expect(Self.choose([plane(0.55)])?.plane.y == 0.55) + #expect(Self.choose([plane(-0.45)])?.plane.y == -0.45) + } + + /// Once the ground is measured, a plane more than 0.1 m above it is refused; one a little + /// above it, or any lower one, may replace it. + @Test func aMeasuredGroundIsNeverRaisedMuch() { + func plane(_ y: Float) -> GroundPlaneEvidence { + GroundPlaneEvidence(y: y, kind: .floor, boundary: Self.rectangle(x: -3...3, z: 0...3), id: "y\(y)") + } + #expect(Self.choose([plane(0.15)], current: 0) == nil) + #expect(Self.choose([plane(0.15)]) != nil) + #expect(Self.choose([plane(0.08)], current: 0)?.plane.y == 0.08) + #expect(Self.choose([plane(-0.3)], current: 0)?.plane.y == -0.3) + // A higher plane can't win over a lower one by containing the phone. + let higher = GroundPlaneEvidence(y: 0.3, kind: .floor, boundary: Self.rectangle(x: -1...1, z: 0...1), id: "higher") + #expect(Self.choose([higher, plane(0)], phone: SIMD3(0, 1.4, 0.5), current: 0)?.plane.id == "y0.0") + } + + /// Without plane classification every plane is unclassified, and every one is a candidate. + @Test func withoutClassificationEveryPlaneIsACandidate() { + let upper = GroundPlaneEvidence(y: 0, kind: .unclassified, boundary: Self.rectangle(x: -2...2, z: 0.05...2), id: "upper") + let lower = GroundPlaneEvidence(y: -0.3, kind: .unclassified, boundary: Self.rectangle(x: 2.3...10, z: 0...10), id: "lower") + #expect(Self.choose([lower, upper], phone: SIMD3(0.3, 1.4, 0.6))?.plane.id == "upper") + #expect(Self.choose([lower, upper])?.plane.id == "lower") + #expect(GroundPlaneChoice.groundY(meter: Self.meter, along: Self.along, planes: [upper]) == 0) + } +} + +/// The ground as frames update it (`SpatialUpdate`): the raise limit holds only once the walk has +/// kept a view, and the phone's position at the tap moves with an anchor correction. +@Suite struct GroundUpdateTests { + /// Two floors where the ground steps down, as in `theFloorUnderThePhoneWinsOverALowerOne`. + static let upper = GroundPlaneEvidence(y: 0, kind: .floor, boundary: GroundPlaneTests.rectangle(x: -2...2, z: 0.05...2), id: "upper") + static let lower = GroundPlaneEvidence(y: -0.3, kind: .floor, boundary: GroundPlaneTests.rectangle(x: 2.3...10, z: 0...10), id: "lower") + static let phone = SIMD3(0.3, 1.4, 0.6) + + static func setUp() -> (map: CoverageMap, tracking: MeterAnchorTracking?, ground: GroundEvidence) { + var map = CoverageMap(wall: standardWall()) + map.heightError = 0.3 + return (map, nil, GroundEvidence(measured: false, guessError: 0.3)) + } + + /// The lower floor is detected first and measures the ground. The floor under the phone + /// arrives during the close-up, 0.3 m higher: nothing has been kept yet, so it replaces the + /// lower one. Without this the choice depended on which plane ARKit found first. + @Test func theFloorUnderThePhoneRaisesTheGroundBeforeTheWalk() { + var (map, tracking, ground) = Self.setUp() + var phone: SIMD3? = Self.phone + let first = SpatialUpdate.apply(anchor: nil, planes: [Self.lower], time: 1, phone: &phone, map: &map, tracking: &tracking, ground: &ground) + #expect(first.groundChoice?.plane.id == "lower" && first.groundChoice?.reason == .lowest) + #expect(ground.measured && nearlyEqual(map.wall.groundY, -0.3)) + #expect(SpatialUpdate.raiseLimit(map: map, ground: ground) == nil) + let second = SpatialUpdate.apply(anchor: nil, planes: [Self.lower, Self.upper], time: 2, phone: &phone, map: &map, tracking: &tracking, ground: &ground) + #expect(second.groundChanged) + #expect(second.groundChoice?.plane.id == "upper" && second.groundChoice?.reason == .underThePhone) + #expect(nearlyEqual(map.wall.groundY, 0)) + } + + /// Once the walk has kept a view, the same higher floor is refused (#62): the ground stays + /// measured on the lower one. + @Test func onceAViewIsKeptTheGroundIsNotRaised() { + var (map, tracking, ground) = Self.setUp() + var phone: SIMD3? = Self.phone + _ = SpatialUpdate.apply(anchor: nil, planes: [Self.lower], time: 1, phone: &phone, map: &map, tracking: &tracking, ground: &ground) + map.observe(wallCamera(s: 0), trackingNormal: true) + #expect(SpatialUpdate.raiseLimit(map: map, ground: ground).map { nearlyEqual($0, -0.3) } == true) + let outcome = SpatialUpdate.apply(anchor: nil, planes: [Self.lower, Self.upper], time: 2, phone: &phone, map: &map, tracking: &tracking, ground: &ground) + #expect(!outcome.groundChanged) + #expect(ground.measured && nearlyEqual(map.wall.groundY, -0.3)) + } + + /// Where the phone stood is a point in the old frame: a correction moves it with the wall, so + /// the plane under it in the new frame is still found. Left where it was, it misses the + /// upper floor and the lower one wins. + @Test func thePhonesPositionMovesWithACorrection() throws { + let shift = SIMD4(2.6, 0, 0, 0) + func shifted(_ plane: GroundPlaneEvidence) -> GroundPlaneEvidence { + var moved = plane + moved.boundary = plane.boundary.map { $0 + SIMD2(shift.x, shift.z) } + return moved + } + let pose = MeterAnchorCorrectionTests.wallHitPose(meter: SIMD3(0, 1.5, 0), outward: SIMD3(0, 0, 1)) + var moved = pose + moved.columns.3 += shift + let planes = [shifted(Self.lower), shifted(Self.upper)] + + var (map, _, ground) = Self.setUp() + var tracking: MeterAnchorTracking? = MeterAnchorTracking(pose: pose) + var phone: SIMD3? = Self.phone + let outcome = SpatialUpdate.apply(anchor: moved, planes: planes, time: 1, phone: &phone, map: &map, tracking: &tracking, ground: &ground) + #expect(outcome.correction != nil) + let movedPhone = try #require(phone) + #expect(nearlyEqual(movedPhone, Self.phone + SIMD3(2.6, 0, 0))) + #expect(outcome.groundChoice?.plane.id == "upper" && outcome.groundChoice?.reason == .underThePhone) + + let unmoved = GroundPlaneChoice.choose(meter: map.wall.meter, along: map.wall.along, phone: Self.phone, current: nil, planes: planes) + #expect(unmoved?.plane.id == "lower" && unmoved?.reason == .lowest) + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/MeterTapTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/MeterTapTests.swift new file mode 100644 index 000000000..07f4f2d84 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/MeterTapTests.swift @@ -0,0 +1,155 @@ +import Foundation +@testable import HouseScanKit +import simd +import Testing + +// #69: a meter tap on an estimated plane put the wall behind the real one and turned from it, and +// nothing corrected it. Made-up numbers, not field values. The real wall is the plane z = 0, +// facing +z, detected by ARKit from x = -3 to 3 m and 0 to 2.4 m up; the meter is 1.5 m up at x = 0. + +@Suite struct MeterTapTests { + static let house = WallPlaneEvidence( + id: "house", kind: .wall, center: SIMD3(0, 1.2, 0), normal: SIMD3(0, 0, 1), + boundary: [SIMD3(-3, 0, 0), SIMD3(3, 0, 0), SIMD3(3, 2.4, 0), SIMD3(-3, 2.4, 0)] + ) + + /// A phone `distance` in front of the meter, facing the wall. + static func phone(_ distance: Float) -> CameraFrame { + portraitCamera(at: SIMD3(0, 1.5, distance), forward: SIMD3(0, 0, -1)) + } + + static func turned(_ degrees: Float) -> SIMD3 { + let a = degrees * .pi / 180 + return SIMD3(sin(a), 0, cos(a)) + } + + static func refusal(hit: SIMD3, normal: SIMD3, source: MeterPlaneSource = .estimatedPlane, camera: CameraFrame = MeterTapTests.phone(0.4), planes: [WallPlaneEvidence] = [MeterTapTests.house]) -> MeterTap.Refusal? { + MeterTap.refusal(hit: hit, normal: normal, source: source, camera: camera, planes: planes) + } + + /// An estimated hit 1.1 m behind the wall, its plane turned 46 degrees: refused for its turn, + /// and, square-on, for lying behind the wall the phone looks through. + @Test func anEstimatedHitTurnedAndBehindTheWallIsRefused() throws { + let hit = SIMD3(0, 1.5, -1.1) + let turned = try #require(Self.refusal(hit: hit, normal: Self.turned(46))) + guard case .turnedFromPhone(let degrees) = turned else { + Issue.record("expected a turn, got \(turned)") + return + } + #expect(nearlyEqual(degrees, 46, 1e-3)) + let behind = try #require(Self.refusal(hit: hit, normal: SIMD3(0, 0, 1))) + guard case .behindDetectedPlane(let planeID, let meters) = behind else { + Issue.record("expected behind a plane, got \(behind)") + return + } + #expect(planeID == "house") + #expect(nearlyEqual(meters, 1.1, 1e-4)) + // With no plane detected yet, the square-on hit behind the wall can't be told apart. + #expect(Self.refusal(hit: hit, normal: SIMD3(0, 0, 1), planes: []) == nil) + } + + @Test func anEstimatedHitOutOfReachIsRefused() throws { + let refusal = try #require(Self.refusal(hit: SIMD3(0, 1.5, -1.7), normal: SIMD3(0, 0, 1), planes: [])) + guard case .tooFar(let meters) = refusal else { + Issue.record("expected out of reach, got \(refusal)") + return + } + #expect(nearlyEqual(meters, 2.1, 1e-4)) + } + + /// A square-on hit on the wall 0.5 m from the phone, either way round, and one on the meter's + /// face 0.15 m in front of the wall, are taken. + @Test func aSquareOnHitAtHalfAMeterIsAccepted() { + #expect(Self.refusal(hit: SIMD3(0, 1.5, 0), normal: SIMD3(0, 0, 1), camera: Self.phone(0.5)) == nil) + #expect(Self.refusal(hit: SIMD3(0, 1.5, 0), normal: SIMD3(0, 0, -1), camera: Self.phone(0.5)) == nil) + #expect(Self.refusal(hit: SIMD3(0, 1.5, 0.15), normal: Self.turned(20), camera: Self.phone(0.6)) == nil) + } + + /// A hit on a detected plane is taken as it is, however it lies. + @Test func aHitOnADetectedPlaneIsAccepted() { + #expect(Self.refusal(hit: SIMD3(0, 1.5, -1.1), normal: Self.turned(46), source: .detectedPlane) == nil) + #expect(Self.refusal(hit: SIMD3(0, 1.5, -3), normal: SIMD3(0, 0, 1), source: .detectedPlane) == nil) + } + + /// A door or a window may stand in front of a wall; only wall and unclassified planes refuse. + /// Neither does a plane the line of sight passes beside. + @Test func onlyAWallPlaneOnTheLineOfSightRefuses() { + var door = Self.house + door.kind = .other + #expect(Self.refusal(hit: SIMD3(0, 1.5, -1.1), normal: SIMD3(0, 0, 1), planes: [door]) == nil) + var unclassified = Self.house + unclassified.kind = .unclassified + #expect(Self.refusal(hit: SIMD3(0, 1.5, -1.1), normal: SIMD3(0, 0, 1), planes: [unclassified]) != nil) + var beside = Self.house + beside.center = SIMD3(5, 1.2, 0) + beside.boundary = beside.boundary.map { $0 + SIMD3(5, 0, 0) } + #expect(Self.refusal(hit: SIMD3(0, 1.5, -1.1), normal: SIMD3(0, 0, 1), planes: [beside]) == nil) + } + + /// The meter was marked on the tap's line of sight; the detected wall plane moves it to where + /// that line meets the plane once it would move more than 0.2 m. + @Test func theRefitMovesTheMeterOnceItIsOffByMoreThanTwentyCentimetres() throws { + let tapCamera = SIMD3(0, 1.5, 0.4) + #expect(MeterTap.refit(meter: SIMD3(0, 1.5, -0.15), outward: SIMD3(0, 0, 1), tapCamera: tapCamera, planes: [Self.house]) == nil) + let refit = try #require(MeterTap.refit(meter: SIMD3(0, 1.5, -0.25), outward: SIMD3(0, 0, 1), tapCamera: tapCamera, planes: [Self.house])) + #expect(nearlyEqual(refit.meter, SIMD3(0, 1.5, 0))) + #expect(nearlyEqual(refit.outward, SIMD3(0, 0, 1))) + #expect(nearlyEqual(refit.moved, 0.25)) + #expect(refit.planeID == "house") + } + + /// A meter in the right place on a wall turned from the detected one: re-fitted once the turn + /// passes 10 degrees. + @Test func theRefitTurnsTheWallOnceItIsOffByMoreThanTenDegrees() throws { + let tapCamera = SIMD3(0, 1.5, 0.4) + let meter = SIMD3(0, 1.5, 0) + #expect(MeterTap.refit(meter: meter, outward: Self.turned(8), tapCamera: tapCamera, planes: [Self.house]) == nil) + let refit = try #require(MeterTap.refit(meter: meter, outward: Self.turned(12), tapCamera: tapCamera, planes: [Self.house])) + #expect(nearlyEqual(refit.turned, 12 * .pi / 180, 1e-4)) + #expect(nearlyEqual(refit.outward, SIMD3(0, 0, 1))) + } + + /// The tap that put the meter 1.1 m behind the wall and turned 46 degrees is moved onto the + /// wall, facing the phone, by a wall plane; not by an unclassified one, nor by one the tap's + /// line of sight misses. + @Test func aBadTapIsRefittedOnlyToAWallPlaneOnItsLineOfSight() throws { + let tapCamera = SIMD3(0.2, 1.5, 0.4) + let meter = SIMD3(-0.35, 1.5, -1.1) + let refit = try #require(MeterTap.refit(meter: meter, outward: Self.turned(46), tapCamera: tapCamera, planes: [Self.house])) + #expect(nearlyEqual(refit.meter.z, 0, 1e-4)) + #expect(refit.meter.x > -0.35 && refit.meter.x < 0.2) + #expect(nearlyEqual(refit.outward, SIMD3(0, 0, 1))) + var unclassified = Self.house + unclassified.kind = .unclassified + #expect(MeterTap.refit(meter: meter, outward: Self.turned(46), tapCamera: tapCamera, planes: [unclassified]) == nil) + var beside = Self.house + beside.center = SIMD3(5, 1.2, 0) + beside.boundary = beside.boundary.map { $0 + SIMD3(5, 0, 0) } + #expect(MeterTap.refit(meter: meter, outward: Self.turned(46), tapCamera: tapCamera, planes: [beside]) == nil) + } + + /// A wall plane ARKit already knew at the tap: the gate takes an estimated hit on the meter + /// 0.05 m in front of it, turned 20 degrees, and the same planes re-fit it straight away. The + /// engine tries the re-fit on entering the close-up, not only when the planes change. + @Test func aTapTheGatePassesIsRefittedAgainstThePlanesAlreadyKnown() throws { + let camera = Self.phone(0.5) + let hit = SIMD3(0, 1.5, 0.05) + let planes = [Self.house] + #expect(Self.refusal(hit: hit, normal: Self.turned(20), camera: camera, planes: planes) == nil) + let refit = try #require(MeterTap.refit(meter: hit, outward: Self.turned(20), tapCamera: camera.position, planes: planes)) + #expect(nearlyEqual(refit.turned, 20 * .pi / 180, 1e-4)) + #expect(nearlyEqual(refit.meter, SIMD3(0, 1.5, 0))) + #expect(nearlyEqual(refit.outward, SIMD3(0, 0, 1))) + } + + /// A meter standing proud of the house wall (on a pedestal, say) tapped 0.5 m in front of it: + /// the wall behind isn't a better fit, and the meter stays. One 0.25 m in front, a meter box's + /// depth, is still moved onto the wall. + @Test func aWallFarBehindTheTapDoesNotPullTheMeterBack() throws { + let tapCamera = SIMD3(0, 1.5, 0.9) + #expect(MeterTap.refit(meter: SIMD3(0, 1.5, 0.5), outward: SIMD3(0, 0, 1), tapCamera: tapCamera, planes: [Self.house]) == nil) + let refit = try #require(MeterTap.refit(meter: SIMD3(0, 1.5, 0.25), outward: SIMD3(0, 0, 1), tapCamera: tapCamera, planes: [Self.house])) + #expect(nearlyEqual(refit.meter, SIMD3(0, 1.5, 0))) + #expect(nearlyEqual(refit.moved, 0.25)) + } +}