diff --git a/ios/.gitignore b/ios/.gitignore index 2e1efc1ee..0a5a926fb 100644 --- a/ios/.gitignore +++ b/ios/.gitignore @@ -9,6 +9,6 @@ xcuserdata/ DerivedData/ build/ -# SwiftPM build output of the capture package. -HouseScanKit/.build/ +# SwiftPM build output, of the capture package or of a swift command run from here. +.build/ HouseScanKit/.swiftpm/ diff --git a/ios/HouseScan/Contract/ScanContract.swift b/ios/HouseScan/Contract/ScanContract.swift index 55f2b8c72..86a5a58a4 100644 --- a/ios/HouseScan/Contract/ScanContract.swift +++ b/ios/HouseScan/Contract/ScanContract.swift @@ -618,6 +618,10 @@ struct ResultPresentation: Equatable, Sendable { /// True when no server answered and the result is the offline sample used by tests and /// demos. The UI must say so on screen. var isSample: Bool + /// The wall the answer's s runs along: the one scene.json described. The result screens draw + /// along it rather than `ScanViewState.wall`, which is the walk's and can differ from it under + /// `-coverage map3d`. Nil in the UI demo, which uses the state's wall. + var wall: WallGeometry? = nil } // MARK: - State and intents @@ -639,6 +643,9 @@ final class ScanViewState { var wall: WallGeometry? var coverage: CoverageStrip = .empty + /// The 3D map's unseen space and where to look next, for `Map3DOverlay`. Nil under + /// `-coverage legacy` and until the first snapshot after the meter is placed. + var map3D: Map3DFog? var closeUp: CloseUpState = .aiming(hold: 0, problem: nil) /// Close-up attempts that ended without a usable photo. "Can't get a clear shot" appears /// from the second one on, never earlier. diff --git a/ios/HouseScan/Runtime/Autopilot.swift b/ios/HouseScan/Runtime/Autopilot.swift index 193b92e57..a8534da18 100644 --- a/ios/HouseScan/Runtime/Autopilot.swift +++ b/ios/HouseScan/Runtime/Autopilot.swift @@ -30,7 +30,7 @@ final class Autopilot { guard await waitUntil(timeout: 60, { self.engine.replay != nil || self.engine.state.failure != nil }), let replay = engine.replay else { return fail("replay did not load") } - async let prepared: Void = replay.prepareHeldBack() + async let prepared: Void = replay.prepareHeldBack(map3D: engine.map3D != nil) await pause(hold) await engine.waitForGate(.onboarding) engine.finishOnboarding() @@ -52,6 +52,7 @@ final class Autopilot { await pause(0.5) guard await waitUntil(timeout: 120, { !replay.isPlaying }) else { return fail("walk replay did not finish") } await pause(1.0) + await settleMap3D() await markFeatures(replay) if engine.options.autopilotCantGetThere { @@ -70,6 +71,8 @@ final class Autopilot { // Mulch, so the replay's scene carries a ground patch over the ground its walk saw. // Answered after the UI test has finished with the screen, so its audit reads a still tree. engine.answerGround(.type(.mulch)) + // The tilt-up frames went into the map too; the gap check reads what it made of them. + await settleMap3D() engine.confirmFeatures() await pause(0.3) if engine.state.phase == .gapRequest { @@ -285,12 +288,17 @@ final class Autopilot { } } - /// With `-autopilotGate`, leaves the scene.json of the scan the result answers in the gate - /// folder, for the UI test to check its ends and what it reports as seen. + /// With `-autopilotGate`, leaves the scene.json of the scan the result answers (the one the + /// last upload sent) in the gate folder, for the UI test to check its ends and what it + /// reports as seen. private func writeSceneForTest() { guard let gate = engine.options.autopilotGate else { return } + guard let scene = engine.uploadedScene else { + log("no uploaded scene.json to write to the gate folder") + return + } do { - try engine.sceneJSON().write(to: gate.appending(path: "scene.json")) + try scene.write(to: gate.appending(path: "scene.json")) log("wrote scene.json to the gate folder") } catch { log("could not write scene.json to the gate folder: \(error)") @@ -399,6 +407,7 @@ final class Autopilot { _ = await waitUntil(timeout: 60) { !replay.isPlaying || self.engine.state.gap?.id != request.id } // Time for the last frame's keyframe to be stored and counted. await pause(hold) + await settleMap3D() if engine.state.phase == .gapRequest, let gap = engine.state.gap, gap.id == request.id, !gap.isSatisfied { log("the replay does not settle request \(request.id) (\(Int(gap.progress * 100))% of it seen); skipping it") await engine.waitForGate(.gapRequest) @@ -428,6 +437,17 @@ final class Autopilot { // MARK: Helpers + /// Under `-coverage map3d`, waits until the 3D map has integrated every frame played and its + /// last change has reached the coverage map, for at most a minute. Without it the steps after + /// the walk act on coverage the map has not caught up with: a debug build takes about 7 s per + /// snapshot (`Map3DSession`), and the replay's walk lasts about 6 s at 3x. + private func settleMap3D() async { + guard let map3D = engine.map3D else { return } + if await !waitUntil(timeout: 60, { !map3D.isCatchingUp }) { + log("the 3D map was still catching up after a minute; going on") + } + } + /// Shows the replay frame that best shows `point`, then taps where it appears on screen. @discardableResult private func tap(_ point: SIMD3, replay: ReplayPlayer, action: (CGPoint) -> Void) async -> Bool { diff --git a/ios/HouseScan/Runtime/DepthEstimator.swift b/ios/HouseScan/Runtime/DepthEstimator.swift new file mode 100644 index 000000000..f809edf4f --- /dev/null +++ b/ios/HouseScan/Runtime/DepthEstimator.swift @@ -0,0 +1,227 @@ +import ARKit +import CoreImage +import CoreImage.CIFilterBuiltins +import CoreML +import Foundation +import HouseScanKit +import Synchronization + +/// Where the depth model is: compiled into the app bundle (Xcode compiles a bundled +/// `.mlpackage` to `.mlmodelc`), else compiled in Application Support/Models. Nil when neither +/// exists; the app then runs without estimated depth. +enum DepthModelLocator { + /// Apple's Core ML Depth Anything V2 Small, Float16 (huggingface.co/apple/coreml-depth-anything-v2-small, + /// revision cfef6f6f2a70783dedc0bfae40cecbc2052285d3, Apache-2.0). + static let name = "DepthAnythingV2SmallF16" + + static func url(bundle: Bundle = .main) -> URL? { + if let bundled = bundle.url(forResource: name, withExtension: "mlmodelc") { return bundled } + guard let support = FileManager.default.urls(for: .applicationSupportDirectory, in: .userDomainMask).first else { return nil } + let cached = support.appending(path: "Models/\(name).mlmodelc", directoryHint: .isDirectory) + return FileManager.default.fileExists(atPath: cached.path) ? cached : nil + } +} + +/// What the depth model needs from one ARKit frame, copied on the AR delegate queue: the camera +/// image stretched to the model's input, and the frame's own geometry for the scale fit. +struct DepthEstimatorInput: Sendable { + /// The `SourceFrame` id of the frame, so the engine can ask for it once the frame is kept. + let frameID: String + /// `DepthEstimator.inputWidth` x `inputHeight` BGRA, rows from the top of the unrotated + /// landscape sensor image, as the intrinsics describe it. + let bgra: [UInt8] + let camera: CameraFrame + /// ARKit's `rawFeaturePoints`, world meters. + let points: [SIMD3] + let planes: [PlaneObservation] +} + +/// Estimated depth for a phone without LiDAR: runs the depth model on each kept keyframe, fits +/// its scale to that frame's feature points and detected planes (`MonocularDepth`), and hands +/// the `.estimated` depth frame to `onFrame` (the 3D map). +/// +/// The delegate queue only converts a keyframe candidate (`input(from:id:context:)`) and offers +/// it (`offer`); the model runs on this type's own queue when the engine keeps the frame +/// (`estimate(frameID:)`), never on the delegate queue or the main actor. Candidates wait a few +/// at a time, since a frame is kept only after its JPEG is encoded and stored. +final class DepthEstimator: Sendable { + static let inputWidth = 518 + static let inputHeight = 392 + /// Candidates held for a keep decision. The encode queue delivers a candidate within about + /// one JPEG encode and the engine keeps it on arrival; four covers a second of candidates at + /// the walk's fastest encode rate (one per 0.3 s). A guess, not measured. + private static let waiting = 4 + /// Kept frames waiting for the model, beyond the one it runs on; older ones are dropped. Each + /// holds the 518 x 392 image (812 KB) and the frame's geometry, and waiting longer only makes + /// its estimate staler. Two keeps the model busy without a backlog. A guess, not measured. + private static let queued = 2 + + private let model: ModelBox + private let queue = DispatchQueue(label: "dev.housescanning.housescan.depth-model", qos: .utility) + private let candidates = Mutex<[DepthEstimatorInput]>([]) + /// Kept frames for the model with the generation they were kept in, and whether the model's + /// queue is running through them. + private let work = Mutex((pending: PendingWork<(input: DepthEstimatorInput, generation: Int)>(limit: DepthEstimator.queued), running: false)) + private let onFrame: @Sendable (DepthFrame, Int) -> Void + + /// Core ML documents a loaded model's predictions as safe to call from any thread; this + /// type calls it from its one serial queue only. + private struct ModelBox: @unchecked Sendable { + let model: MLModel + } + + private init(model: MLModel, onFrame: @escaping @Sendable (DepthFrame, Int) -> Void) { + self.model = ModelBox(model: model) + self.onFrame = onFrame + } + + /// Loads the model found by `DepthModelLocator`, or returns nil when there is none or it + /// fails to load. The first load on a device specializes the model for the Neural Engine + /// and can take seconds; later loads read Core ML's cache. + /// `onFrame` gets each estimate with the generation its frame was kept in (`estimate`). + static func load(onFrame: @escaping @Sendable (DepthFrame, Int) -> Void) async -> DepthEstimator? { + guard let url = DepthModelLocator.url() else { + RuntimeLog.engine.info("depth model: none in the bundle or Application Support; estimated depth off") + return nil + } + let configuration = MLModelConfiguration() + configuration.computeUnits = .all + do { + let started = ProcessInfo.processInfo.systemUptime + let model = try await MLModel.load(contentsOf: url, configuration: configuration) + RuntimeLog.engine.info("depth model loaded in \(ProcessInfo.processInfo.systemUptime - started) s") + return DepthEstimator(model: model, onFrame: onFrame) + } catch { + RuntimeLog.engine.error("depth model failed to load: \(error.localizedDescription, privacy: .public)") + return nil + } + } + + // MARK: AR delegate queue + + /// The model's input for a frame: its camera image scaled to 518 x 392 (Lanczos, stretched + /// from the sensor's 4:3, as Apple's evaluation of the model stretched its images) and its + /// feature points and planes. Nil when the render fails. + static func input(from frame: ARFrame, id: String, context: CIContext) -> DepthEstimatorInput? { + let image = CIImage(cvPixelBuffer: frame.capturedImage) + let size = image.extent.size + guard size.width > 0, size.height > 0 else { return nil } + let scale = CIFilter.lanczosScaleTransform() + scale.inputImage = image + scale.scale = Float(CGFloat(inputHeight) / size.height) + scale.aspectRatio = Float((CGFloat(inputWidth) / size.width) / (CGFloat(inputHeight) / size.height)) + guard let scaled = scale.outputImage, let space = CGColorSpace(name: CGColorSpace.sRGB) else { return nil } + var bgra = [UInt8](repeating: 0, count: inputWidth * inputHeight * 4) + // Rows come out top first, as the pixel buffer's are. + bgra.withUnsafeMutableBytes { bytes in + context.render( + scaled, toBitmap: bytes.baseAddress!, rowBytes: inputWidth * 4, + bounds: CGRect(x: 0, y: 0, width: inputWidth, height: inputHeight), format: .BGRA8, colorSpace: space) + } + let feature = Map3DFeed.featureFrame(frame) + return DepthEstimatorInput(frameID: id, bgra: bgra, camera: feature.camera, points: feature.points, planes: Map3DFeed.planes(frame)) + } + + /// Keeps a candidate until the engine keeps or passes over its frame; the oldest goes first. + func offer(_ input: DepthEstimatorInput) { + candidates.withLock { waiting in + waiting.append(input) + if waiting.count > Self.waiting { waiting.removeFirst(waiting.count - Self.waiting) } + } + } + + // MARK: Any actor + + /// Estimates depth for a kept frame offered earlier; does nothing when it was not offered + /// (a replay, a LiDAR phone) or has already left the candidates. `generation` (the 3D map's, + /// `Map3DSession.generation`) goes back with the estimate, so one from before a reset is + /// dropped. At most `queued` frames wait for the model, the oldest dropped first. + func estimate(frameID: String, generation: Int) { + let input = candidates.withLock { waiting -> DepthEstimatorInput? in + guard let index = waiting.firstIndex(where: { $0.frameID == frameID }) else { return nil } + return waiting.remove(at: index) + } + guard let input else { return } + let start = work.withLock { work -> Bool in + work.pending.add((input, generation)) + guard !work.running else { return false } + work.running = true + return true + } + if start { queue.async { [self] in runPending() } } + } + + /// Drops the frames waiting for the model and the candidates: after a reset they belong to a + /// world frame the map no longer has. + func reset() { + candidates.withLock { $0.removeAll() } + work.withLock { $0.pending.removeAll() } + } + + // MARK: Model queue + + /// Runs the model on waiting frames, newest last, until none wait. + private func runPending() { + while let (input, generation) = work.withLock({ work -> (DepthEstimatorInput, Int)? in + guard let next = work.pending.take() else { + work.running = false + return nil + } + return (next.input, next.generation) + }) { + estimateNow(input, generation: generation) + } + } + + private func estimateNow(_ input: DepthEstimatorInput, generation: Int) { + let started = ProcessInfo.processInfo.systemUptime + guard let prediction = predict(input.bgra) else { return } + let predicted = ProcessInfo.processInfo.systemUptime + let anchors = DepthAnchor.anchors(points: input.points, camera: input.camera) + DepthAnchor.anchors(planes: input.planes, confirmedBy: input.points, camera: input.camera) + guard let estimate = MonocularDepth.estimate(prediction, anchors: anchors, photo: input.camera) else { + RuntimeLog.engine.info("depth \(input.frameID, privacy: .public): no scale fit from \(anchors.count) anchors; frame skipped") + return + } + let fitted = ProcessInfo.processInfo.systemUptime + RuntimeLog.engine.info("depth \(input.frameID, privacy: .public): model \(Int((predicted - started) * 1000)) ms, fit \(Int((fitted - predicted) * 1000)) ms, \(estimate.fit.inlierCount)/\(anchors.count) anchors, relative sigma \(estimate.fit.relativeSigma)") + onFrame(estimate.frame, generation) + } + + private func predict(_ bgra: [UInt8]) -> RelativeInverseDepth? { + var buffer: CVPixelBuffer? + guard CVPixelBufferCreate(nil, Self.inputWidth, Self.inputHeight, kCVPixelFormatType_32BGRA, nil, &buffer) == kCVReturnSuccess, let buffer else { return nil } + CVPixelBufferLockBaseAddress(buffer, []) + if let base = CVPixelBufferGetBaseAddress(buffer) { + let rowBytes = CVPixelBufferGetBytesPerRow(buffer) + bgra.withUnsafeBytes { source in + for row in 0.. 0 { autopilotHold = hold } + if let model = value(after: "-coverage") { + guard let parsed = CoverageModel(rawValue: model) else { preconditionFailure("-coverage takes map3d or legacy, not \(model)") } + coverage = parsed + } + if let setting = value(after: "-estimatedDepth") { + guard setting == "on" || setting == "off" else { preconditionFailure("-estimatedDepth takes on or off, not \(setting)") } + estimatedDepth = setting == "on" + } } /// An http(s) URL with a host, or nil. An empty build setting leaves the plist value empty, diff --git a/ios/HouseScan/Runtime/LiveCapture.swift b/ios/HouseScan/Runtime/LiveCapture.swift index d04508720..c43e0f010 100644 --- a/ios/HouseScan/Runtime/LiveCapture.swift +++ b/ios/HouseScan/Runtime/LiveCapture.swift @@ -56,9 +56,13 @@ final class LiveCapture { private var result: AnchorEntity? private var occludesPeople = false - init(onFrame: @escaping @MainActor @Sendable (SourceFrame) -> Void, onEvent: @escaping @MainActor @Sendable (LiveEvent) -> Void) { + /// `map3D` gets every sampled frame and every mesh and plane anchor; nil under `-coverage legacy`. + init( + onFrame: @escaping @MainActor @Sendable (SourceFrame) -> Void, onEvent: @escaping @MainActor @Sendable (LiveEvent) -> Void, + map3D: Map3DSession? + ) { arView = ARView(frame: .zero, cameraMode: .ar, automaticallyConfigureSession: false) - delegate = LiveSessionDelegate(onFrame: onFrame, onEvent: onEvent) + delegate = LiveSessionDelegate(onFrame: onFrame, onEvent: onEvent, map3D: map3D) arView.session.delegateQueue = DispatchQueue(label: "dev.housescanning.housescan.ar-delegate", qos: .userInitiated) arView.session.delegate = delegate arView.renderOptions.insert(.disableMotionBlur) @@ -102,6 +106,11 @@ final class LiveCapture { ) } + /// Where keyframe candidates of a phone without LiDAR go for estimated depth; nil for none. + func setDepthEstimator(_ estimator: DepthEstimator?) { + delegate.shared.withLock { $0.depthEstimator = estimator } + } + /// Where every ARFrame's pose goes as it arrives (`CaptureRecorder.recordPose`). func setRecorder(_ recorder: CaptureRecorder?) { delegate.shared.withLock { $0.recorder = recorder } @@ -321,6 +330,7 @@ struct LiveShared: Sendable { var mode: LiveMode = .idle var meterAnchorID: UUID? var recorder: CaptureRecorder? + var depthEstimator: DepthEstimator? } /// Receives ARSession callbacks on a private serial queue. Each sampled frame is reduced to a @@ -336,6 +346,7 @@ final class LiveSessionDelegate: NSObject, ARSessionDelegate, Sendable { let shared = Mutex(LiveShared()) private let onFrame: @MainActor @Sendable (SourceFrame) -> Void private let onEvent: @MainActor @Sendable (LiveEvent) -> Void + private let map3D: Map3DSession? private let queueState = Mutex(QueueState()) private let context = CIContext(options: [.cacheIntermediates: false]) /// Serial, and every frame the main actor gets passes through it, pose-only ones too, then @@ -373,9 +384,13 @@ final class LiveSessionDelegate: NSObject, ARSessionDelegate, Sendable { /// standing still costs little; not measured on a phone. private static let encodeStill = 1.0 - init(onFrame: @escaping @MainActor @Sendable (SourceFrame) -> Void, onEvent: @escaping @MainActor @Sendable (LiveEvent) -> Void) { + init( + onFrame: @escaping @MainActor @Sendable (SourceFrame) -> Void, onEvent: @escaping @MainActor @Sendable (LiveEvent) -> Void, + map3D: Map3DSession? + ) { self.onFrame = onFrame self.onEvent = onEvent + self.map3D = map3D } func session(_ session: ARSession, didUpdate frame: ARFrame) { @@ -407,6 +422,8 @@ final class LiveSessionDelegate: NSObject, ARSessionDelegate, Sendable { encodeQueue.async { [self, pose] in deliver(pose) } return } + // The map takes the sampled frames: about 10 a second, and it drops any it can't keep up with. + map3D?.ingest(frame, trackingNormal: tracking == .normal) let quality = Self.quality(frame.capturedImage) let ground = frame.anchors.compactMap { $0 as? ARPlaneAnchor } .filter { $0.alignment == .horizontal } @@ -429,6 +446,10 @@ final class LiveSessionDelegate: NSObject, ARSessionDelegate, Sendable { meters: copied.meters, width: copied.width, height: copied.height, confidence: copied.confidence, intrinsics: DepthImage.intrinsics(scaling: camera.intrinsics, from: camera.imageSize, toWidth: copied.width, height: copied.height) ) + } else if let estimator = shared.depthEstimator, let input = DepthEstimator.input(from: frame, id: snapshot.id, context: context) { + // Without LiDAR: a copy of the image and the frame's geometry, so the model can + // run once the engine keeps the frame, without holding the ARFrame. + estimator.offer(input) } } encodeQueue.async { [self, snapshot] in @@ -586,6 +607,20 @@ final class LiveSessionDelegate: NSObject, ARSessionDelegate, Sendable { } } + // MARK: Anchors + + func session(_ session: ARSession, didAdd anchors: [ARAnchor]) { + map3D?.ingest(updated: anchors) + } + + func session(_ session: ARSession, didUpdate anchors: [ARAnchor]) { + map3D?.ingest(updated: anchors) + } + + func session(_ session: ARSession, didRemove anchors: [ARAnchor]) { + map3D?.ingest(removed: anchors) + } + /// A horizontal plane as the ground choice reads it (`GroundPlaneChoice`): its height, its /// class and its outline in plan, world meters. private static func groundEvidence(_ plane: ARPlaneAnchor) -> GroundPlaneEvidence { diff --git a/ios/HouseScan/Runtime/Map3DCoverageSource.swift b/ios/HouseScan/Runtime/Map3DCoverageSource.swift new file mode 100644 index 000000000..278564e2b --- /dev/null +++ b/ios/HouseScan/Runtime/Map3DCoverageSource.swift @@ -0,0 +1,151 @@ +import Foundation +import HouseScanKit +import simd + +/// What scene.json says about the wall and what was seen, when the 3D map is the coverage model. +struct Map3DExport: Sendable { + /// The wall scene.json describes: the measured chain, or the walk's tapped wall. It and its + /// corners carry their pieces' sources (`WallFrame.source`, `WallCorner.source`). + var wall: WallFrame + /// The described stretch in s of `wall`; contains 0, the meter. + var baselineS: ClosedRange + /// Measured along `wall` and clipped to `baselineS`, the wall band with the height each + /// stretch was seen to. + var coverage: SceneCoverage + /// Position error of each piece's line, meters, per `wall.segments` piece + /// (`SceneInput.wallPlusMinus`); empty for the tapped wall, which takes the server's default. + var plusMinus: [Float?] + /// Why the tapped wall was written instead of the measured chain; nil when the chain was. + var tappedBecause: String? +} + +enum Map3DCoverageError: Error, CustomStringConvertible { + /// The snapshot was read along another wall than the one being exported, so its s would not + /// match the scene's. + case wallMismatch + + var description: String { + switch self { + case .wallMismatch: "The 3D map's snapshot was read along a different wall than the one being exported." + } + } +} + +/// Turns a `Map3DSnapshot` into what the export and the walk read. +enum Map3DCoverageSource { + /// How the chosen spot's check comes out (`SpotDecision.decide`). `span` is the battery's + /// stretch in meters of s along `wall`, the wall that was exported (the server's `span_ft` + /// is in its s). With no session, or no map, the map's view is unknown and the homeowner's + /// answer decides. A measured occluder needs another view whatever the answer. + static func spotDecision(session: Map3DSession?, span: ClosedRange, along wall: WallFrame, homeownerSaysClear: Bool?) -> SpotDecision { + SpotDecision.decide(session?.spotView(span: span, along: wall) ?? .unknown, homeownerSaysClear: homeownerSaysClear) + } + + /// The wall, baseline, coverage and wall sources scene.json gets. + /// + /// The measured chain is used when `snapshot.measured` exists (`finalSnapshot()` fills it) + /// and it agrees with the walk (`MeasuredChainAdmission`). Otherwise the tapped wall is + /// written with source `tap`, and `tappedBecause` says which test failed. Coverage is always + /// the one measured along + /// the wall written, so s agrees. + /// + /// `walkedFacing` (`CoverageMap.facingSpans()`: where the phone was carried, less its position + /// error) and `confirmedOverhead` (`CoverageMap.overheadSpans()`: tilt-up views the homeowner + /// said are clear) are in the tapped wall's s. Each is merged into the map's band of the same + /// name, taking the larger reach wherever either has one. When the measured chain is written + /// they are first restated along it (`ObservedSpan.carried`), on the meter's piece only: + /// past a corner of either wall one straight stretch of the other can face two ways. + /// + /// Every band is clipped to the baseline. The map sees ground in front of the chain only, + /// and the server reads a ground span past a limit end as covering both sides of the wall's + /// continued line, so reporting it would claim ground nobody saw. + /// + /// `tapWall` and `baselineS` are the walk's (`CoverageMap.wall`, `ScanEngine.exportSpan`). + static func export( + _ snapshot: Map3DSnapshot, tapWall: WallFrame, baselineS: ClosedRange, + leftEndMarked: Bool, rightEndMarked: Bool, walkedFacing: [ObservedSpan], confirmedOverhead: [ObservedSpan] + ) throws(Map3DCoverageError) -> Map3DExport { + guard snapshot.wall == tapWall else { throw .wallMismatch } + switch measuredExport(snapshot, tapWall: tapWall, baselineS: baselineS) { + case .success(let measured): + var coverage = measured.coverage + coverage.facing = largerReach(coverage.facing, ObservedSpan.carried(walkedFacing, depth: nil, from: tapWall, to: measured.wall)) + // The session's map runs with the default config, whose battery depth the map's own + // overhead band is judged over. + let batteryDepth = Map3DConfig().overheadDepth + coverage.overhead = largerReach( + coverage.overhead, ObservedSpan.carried(confirmedOverhead, depth: batteryDepth, from: tapWall, to: measured.wall)) + return Map3DExport( + wall: measured.wall, baselineS: measured.baselineS, + coverage: sceneCoverage(coverage, within: measured.baselineS, leftEndMarked: leftEndMarked, rightEndMarked: rightEndMarked), + plusMinus: measured.plusMinus, tappedBecause: nil) + case .failure(let refusal): + var coverage = snapshot.coverage + coverage.facing = largerReach(coverage.facing, walkedFacing) + coverage.overhead = largerReach(coverage.overhead, confirmedOverhead) + return Map3DExport( + wall: tapWall, baselineS: baselineS, + coverage: sceneCoverage(coverage, within: baselineS, leftEndMarked: leftEndMarked, rightEndMarked: rightEndMarked), + plusMinus: [], tappedBecause: refusal.reason) + } + } + + /// Why the measured chain was not written. + struct Refusal: Error { + var reason: String + } + + private static func measuredExport( + _ snapshot: Map3DSnapshot, tapWall: WallFrame, baselineS: ClosedRange + ) -> Result<(wall: WallFrame, baselineS: ClosedRange, coverage: Map3DCoverage, plusMinus: [Float?]), Refusal> { + guard let chain = snapshot.chain else { return .failure(Refusal(reason: "no measured wall passes the meter")) } + guard let measured = snapshot.measured else { return .failure(Refusal(reason: "the snapshot has no measured wall frame")) } + switch MeasuredChainAdmission.admit(chain, as: measured.wall, frame: snapshot.frame, tapWall: tapWall, baselineS: baselineS) { + case .success(let baseline): + return .success((measured.wall, baseline, measured.coverage, chain.walls.map(plusMinus(of:)))) + case .failure(let refusal): + return .failure(Refusal(reason: refusal.reason)) + } + } + + /// The line's position error, meters: the fit's two standard errors (`MeasuredWall.plusMinus`), + /// scene.json's `plus_minus_ft` for the wall. + static func plusMinus(of wall: MeasuredWall) -> Float? { + wall.plusMinus + } + + /// The larger of two reaches at every s either has one, as spans in s order, neighbours + /// with the same reach joined. Where only one list has a span, its reach stands. + static func largerReach(_ a: [ObservedSpan], _ b: [ObservedSpan]) -> [ObservedSpan] { + let all = a + b + let edges = Set(all.flatMap { [$0.span.lowerBound, $0.span.upperBound] }).sorted() + var result: [ObservedSpan] = [] + for (low, high) in zip(edges, edges.dropFirst()) where low < high { + let middle = (low + high) / 2 + guard let reach = all.filter({ $0.span.contains(middle) }).map(\.out).max() else { continue } + if let last = result.last, last.out == reach, last.span.upperBound == low { + result[result.count - 1].span = last.span.lowerBound...high + } else { + result.append(ObservedSpan(span: low...high, out: reach)) + } + } + return result + } + + static func sceneCoverage(_ coverage: Map3DCoverage, within baseline: ClosedRange, leftEndMarked: Bool, rightEndMarked: Bool) -> SceneCoverage { + func clip(_ span: ClosedRange) -> ClosedRange? { + let low = max(span.lowerBound, baseline.lowerBound) + let high = min(span.upperBound, baseline.upperBound) + return low < high ? low...high : nil + } + func clip(_ spans: [ObservedSpan]) -> [ObservedSpan] { + spans.compactMap { item in clip(item.span).map { ObservedSpan(span: $0, out: item.out) } } + } + // Each stretch of face with the height it was seen to (`Map3DCoverage.wallHeight`), not + // only the stretches seen to headroom (`Map3DCoverage.wall`): the server credits each + // entry against the height its check needs. + return SceneCoverage( + leftEndMarked: leftEndMarked, rightEndMarked: rightEndMarked, wall: clip(coverage.wallHeight), + ground: clip(coverage.ground), facing: clip(coverage.facing), overhead: clip(coverage.overhead)) + } +} diff --git a/ios/HouseScan/Runtime/Map3DFeed.swift b/ios/HouseScan/Runtime/Map3DFeed.swift new file mode 100644 index 000000000..132f09637 --- /dev/null +++ b/ios/HouseScan/Runtime/Map3DFeed.swift @@ -0,0 +1,129 @@ +import ARKit +import CoreVideo +import Foundation +import HouseScanKit +import simd + +/// Converts ARKit objects into HouseScanKit's 3D map inputs. Call on the AR delegate queue; only +/// the returned Sendable values may leave it. +/// +/// Session configuration each function needs (setting it is the engine's job): +/// - `depthFrame`: `frameSemantics.insert(.sceneDepth)` when +/// `ARWorldTrackingConfiguration.supportsFrameSemantics(.sceneDepth)`; otherwise it returns nil. +/// - `meshChunk`: `sceneReconstruction = .meshWithClassification` when +/// `ARWorldTrackingConfiguration.supportsSceneReconstruction(.meshWithClassification)`. +/// With `.mesh`, chunks have no classes. Call it from the session's anchor add and update +/// callbacks, not per frame: every chunk is redrawn into the map each time it is passed. +/// - `plane`, `planes`: `planeDetection` with `.horizontal` and/or `.vertical`. +/// - `featureFrame`: nothing extra; points are empty when ARKit has none for the frame. +enum Map3DFeed { + /// The LiDAR depth image of a frame, or nil when the frame has no `sceneDepth`, no confidence + /// map, or an unexpected pixel format. + static func depthFrame(_ frame: ARFrame) -> DepthFrame? { + guard let sceneDepth = frame.sceneDepth, let confidenceMap = sceneDepth.confidenceMap else { return nil } + let depthMap = sceneDepth.depthMap + guard CVPixelBufferGetPixelFormatType(depthMap) == kCVPixelFormatType_DepthFloat32, + CVPixelBufferGetPixelFormatType(confidenceMap) == kCVPixelFormatType_OneComponent8 + else { return nil } + let width = CVPixelBufferGetWidth(depthMap) + let height = CVPixelBufferGetHeight(depthMap) + guard width > 0, height > 0, + CVPixelBufferGetWidth(confidenceMap) == width, CVPixelBufferGetHeight(confidenceMap) == height, + var depth = copyPixels(depthMap, as: Float.self, width: width, height: height), + let confidence = copyPixels(confidenceMap, as: UInt8.self, width: width, height: height) + else { return nil } + // The packet's encoding: 0, never NaN, where there is no measurement. + for index in depth.indices where !(depth[index].isFinite && depth[index] >= 0) { depth[index] = 0 } + return DepthFrame(photo: photoCamera(frame), width: width, height: height, depth: depth, kind: .lidar(confidence: confidence)) + } + + static func meshChunk(_ anchor: ARMeshAnchor) -> MeshChunk { + let geometry = anchor.geometry + + let source = geometry.vertices + precondition(source.format == .float3, "mesh vertices have format \(source.format.rawValue), expected float3") + let vertexBase = source.buffer.contents().advanced(by: source.offset) + let vertices = (0..] + switch element.bytesPerIndex { + case 4: + let indices = faceBase.assumingMemoryBound(to: UInt32.self) + faces = (0.. PlaneObservation { + let alignment: PlaneObservation.Alignment = anchor.alignment == .vertical ? .vertical : .horizontal + let boundary = anchor.geometry.boundaryVertices.map { SIMD2($0.x, $0.z) } + if boundary.count >= 3 { + return PlaneObservation(id: anchor.identifier, worldFromPlane: anchor.transform, alignment: alignment, boundary: boundary) + } + let extent = anchor.planeExtent + return PlaneObservation( + id: anchor.identifier, worldFromPlane: anchor.transform, alignment: alignment, + center: SIMD2(anchor.center.x, anchor.center.z), + width: extent.width, length: extent.height, rotationOnYAxis: extent.rotationOnYAxis + ) + } + + static func featureFrame(_ frame: ARFrame) -> FeatureFrame { + FeatureFrame(camera: photoCamera(frame), points: frame.rawFeaturePoints?.points ?? []) + } + + static func planes(_ frame: ARFrame) -> [PlaneObservation] { + frame.anchors.compactMap { $0 as? ARPlaneAnchor }.map(plane) + } + + /// The color camera: pose, and intrinsics for the captured image at its native size. + private static func photoCamera(_ frame: ARFrame) -> CameraFrame { + let k = frame.camera.intrinsics + let resolution = frame.camera.imageResolution + return CameraFrame( + cameraToWorld: frame.camera.transform, + intrinsics: SIMD4(k.columns.0.x, k.columns.1.y, k.columns.2.x, k.columns.2.y), + imageSize: SIMD2(Float(resolution.width), Float(resolution.height))) + } + + /// Copies a single-plane pixel buffer row by row into a tightly packed array; rows may be padded. + private static func copyPixels(_ buffer: CVPixelBuffer, as _: T.Type, width: Int, height: Int) -> [T]? { + guard CVPixelBufferLockBaseAddress(buffer, .readOnly) == kCVReturnSuccess else { return nil } + defer { CVPixelBufferUnlockBaseAddress(buffer, .readOnly) } + guard let base = CVPixelBufferGetBaseAddress(buffer) else { return nil } + let bytesPerRow = CVPixelBufferGetBytesPerRow(buffer) + guard bytesPerRow >= width * MemoryLayout.stride else { return nil } + return [T](unsafeUninitializedCapacity: width * height) { output, count in + for row in 0...stride) + destination.copyMemory(from: source, byteCount: width * MemoryLayout.stride) + } + count = width * height + } + } +} diff --git a/ios/HouseScan/Runtime/Map3DSession.swift b/ios/HouseScan/Runtime/Map3DSession.swift new file mode 100644 index 000000000..9c5cb2176 --- /dev/null +++ b/ios/HouseScan/Runtime/Map3DSession.swift @@ -0,0 +1,552 @@ +import ARKit +import Foundation +import HouseScanKit +import simd +import Synchronization + +/// What the 3D map shows at one moment, read along the walk's wall. +struct Map3DSnapshot: Sendable { + /// Increments whenever the map or the wall it is read along changes. + var revision: Int + var frame: MapFrame + /// The walk's wall that `coverage`, `fog` and `nextView` are read along. + var wall: WallFrame + var coverage: Map3DCoverage + var chain: MeasuredWallChain? + var fog: FogOfWar + var nextView: ViewSuggestion? + /// The measured chain as a wall (`MeasuredWallChain.wallFrame`), with the coverage along it. + /// Only `Map3DSession.finalSnapshot()` fills it: the export is its one reader, and reading + /// coverage along a second wall twice a second would double the snapshot's cost. + var measured: (wall: WallFrame, coverage: Map3DCoverage)? + /// Whether measured depth (LiDAR or a replay's) went into the map since `start`. Without it + /// the map holds only feature points, planes and estimated depth, which certify too little + /// to report coverage from. Only `Map3DSession.finalSnapshot()` fills it, for the export. + var integratedDepth = false +} + +/// What the fog overlay draws from a snapshot (`ScanViewState.map3D`), in the map's frame: +/// `frame` converts it to world. +struct Map3DFog: Equatable, Sendable { + var fog: FogOfWar + var nextView: ViewSuggestion? + var frame: MapFrame +} + +/// The app's one `Map3D`: takes ARKit frames and anchors on the AR delegate queue and replay +/// frames on the main actor, integrates them on its own serial queue, and hands the main actor a +/// `Map3DSnapshot` at most `snapshotInterval` apart while the map changes. +/// +/// Snapshots are read from a copy of the map on a second queue, one at a time, so integration +/// never waits for one. Timed once with a scratch test on an M4 Pro (not committed), a snapshot of +/// the LiDAR UI-test fixture's map took 27 ms with HouseScanKit built for release and 7 s built +/// for debug, 6.4 s of it fog and next view. Read on the integration queue, the debug app replaced +/// 45 of that replay's frames before integrating them, and its walk never saw a covered cell. +/// +/// The AR delegate queue only converts ARKit objects (`Map3DFeed`) and drops the result into an +/// inbox, so a slow integration never holds ARKit's frames. The inbox holds one live depth or +/// feature frame: a newer one replaces an older one not yet integrated. Replay depth frames queue +/// instead, up to the replay's length (`ingest(_: SourceFrame)`). Mesh chunks and planes are kept +/// per anchor id, newest first, so the inbox is bounded by the anchors ARKit has. +/// +/// Mesh chunks go into the map as they arrive and are not kept: a copy of every chunk would grow +/// with the walk. Only chunks that arrive while there is no map (before the meter is placed, or +/// after a reset) wait, newest per anchor, for the next `start`, which integrates those that reach +/// the map's bounds and drops them all. So a map rebuilt because the ground moved, or started +/// again after `reset(forgetAnchors: false)`, lacks the mesh ARKit sent to the map before: it has +/// only the chunks ARKit updates afterwards, and a stretch ARKit no longer refines stays without +/// mesh. Planes are small and kept whole across both. `reset(forgetAnchors: true)` drops the +/// waiting chunks and the planes too, for a session restarted with its anchors removed. +final class Map3DSession: Sendable { + /// Two snapshots a second: the rate the fog overlay and the coverage strip were planned + /// around. Each snapshot reads coverage and fog over the whole region of interest; no + /// measurement of that cost on a phone exists yet. + static let snapshotInterval: Double = 0.5 + + /// Mesh vertices kept for a map not started yet (before the meter is placed, or after a + /// reset), oldest chunks dropped first. ARKit keeps meshing while the homeowner looks for the + /// meter; without a budget every chunk it made was copied. 200,000 vertices with their faces + /// is about 10 MB, a few rooms' or a house side's worth. A guess, not measured: a map started + /// after a longer search lacks the mesh first made, until ARKit updates those chunks. + static let chunkBudget = 200_000 + /// Plane boundary points kept, oldest planes dropped first; a plane has a few dozen. A + /// guess, not measured. + static let planeBudget = 20_000 + + /// How far the walk's ground may move from the map's before the map is rebuilt, meters. The + /// voxels are laid out around the map's ground and reach `Map3DConfig.groundBelow` (0.5 m) + /// under it, so within half of that the true ground stays well inside the grid; beyond it the + /// map is rebuilt and loses the rays integrated so far. A guess, not measured: the chest-height + /// ground guess the live walk starts with is ±0.3 m (`ScanEngine.estimatedGroundError`). + static let groundTolerance: Float = 0.25 + + private let queue = DispatchQueue(label: "dev.housescanning.housescan.map3d", qos: .utility) + private let snapshotQueue = DispatchQueue(label: "dev.housescanning.housescan.map3d-snapshot", qos: .utility) + /// Written by every caller; each lock is held only to add or take inputs. + private let inbox = Mutex(Inbox()) + /// The map and what it is read along. Only the queue and `finalSnapshot()` lock it, and each + /// takes the inbox inside that lock, so batches are applied in the order they were taken. + private let core = Mutex(Core()) + private let onSnapshot: @MainActor @Sendable (Map3DSnapshot) -> Void + + init(onSnapshot: @escaping @MainActor @Sendable (Map3DSnapshot) -> Void) { + self.onSnapshot = onSnapshot + } + + // MARK: Main actor + + /// Starts a new map at the meter: `MapFrame(wall:)`. Anything integrated before is dropped. + func start(wall: WallFrame) { + inbox.withLock { inbox in + inbox.generation += 1 + inbox.active = true + inbox.frame = nil + inbox.estimated = [] + inbox.replay = [] + inbox.command = .start(wall) + schedule(&inbox) + } + } + + /// The walk's wall changed. A moved meter moves the map with it (`MapFrame.following`, + /// keeping its ground); a ground moved more than `groundTolerance` rebuilds it; corners only + /// change the wall that coverage, fog and the next view are read along. Ignored before + /// `start`. + func update(wall: WallFrame) { + inbox.withLock { inbox in + guard inbox.active else { return } + switch inbox.command { + case .start?: + // Nothing is integrated yet, so the map may as well start at the newer wall. + inbox.command = .start(wall) + case .update?, nil: + inbox.command = .update(wall) + } + schedule(&inbox) + } + } + + /// Drops the map and its wall; nothing is integrated until the next `start`. With + /// `forgetAnchors`, also the planes and waiting mesh chunks: pass it when the AR session + /// restarts with its anchors removed. The mesh already in the map is lost either way (see the + /// type's comment). + func reset(forgetAnchors: Bool) { + inbox.withLock { inbox in + inbox.generation += 1 + inbox.active = false + inbox.frame = nil + inbox.estimated = [] + inbox.replay = [] + inbox.command = nil + inbox.reset = .some((inbox.reset ?? false) || forgetAnchors) + if forgetAnchors { + inbox.planes = [:] + inbox.chunks = [:] + } + schedule(&inbox) + } + } + + /// A replay frame. Only its LiDAR depth adds to the map: a replay carries no feature points, + /// and frames without normal tracking, shown for review or carrying only a pose add nothing. + /// Unlike live frames, none is dropped: `ReplayPlanning` plans the autopilot's gap with every + /// depth frame, so the map must see the same ones. They queue, at most `replayLength` of them. + @MainActor + func ingest(_ frame: SourceFrame) { + guard frame.tracking == .normal, !frame.isReview, !frame.isPoseOnly, let depth = frame.depth else { return } + let camera = frame.camera + inbox.withLock { inbox in + guard inbox.active else { return } + inbox.replay.append((depth, camera)) + if inbox.replay.count > max(1, inbox.replayLength) { + inbox.replay.removeFirst() + inbox.dropped += 1 + } + schedule(&inbox) + } + } + + /// How many frames the replay being played has: the most its queue holds. + func expectReplay(frames count: Int) { + inbox.withLock { $0.replayLength = count } + } + + // MARK: AR delegate queue + + /// A sampled ARKit frame: its LiDAR depth when it has some, else its feature points with the + /// planes ARKit has at that frame. Only frames with normal tracking count: a limited pose can + /// put rays half a meter off. + func ingest(_ frame: ARFrame, trackingNormal: Bool) { + guard trackingNormal, inbox.withLock({ $0.active }) else { return } + let input: FrameInput = if let depth = Map3DFeed.depthFrame(frame) { + .depth(depth) + } else { + .features(Map3DFeed.featureFrame(frame), planes: Map3DFeed.planes(frame)) + } + inbox.withLock { inbox in + guard inbox.active else { return } + if inbox.frame != nil { inbox.dropped += 1 } + inbox.frame = input + schedule(&inbox) + } + } + + // MARK: Depth model queue + + /// An estimated depth frame (`DepthEstimator`) of a kept keyframe. Held apart from the + /// sampled frame, which replaces itself ten times a second: an estimate comes once per kept + /// keyframe and is not replaced by the next feature frame. At most `waitingEstimates` wait. + /// + /// `generation` is the session's `generation` when the frame was kept. An estimate finishes + /// long after its frame, and one kept before a reset (tracking restarted, the meter placed + /// again) is in a world frame the map no longer has; it is dropped here, under the same lock + /// `start` and `reset` change the generation under. + func ingest(estimated depth: DepthFrame, generation: Int) { + inbox.withLock { inbox in + guard inbox.active, inbox.generation == generation else { return } + inbox.estimated.append(depth) + if inbox.estimated.count > Self.waitingEstimates { + inbox.dropped += inbox.estimated.count - Self.waitingEstimates + inbox.estimated.removeFirst(inbox.estimated.count - Self.waitingEstimates) + } + schedule(&inbox) + } + } + + /// Estimates waiting to be integrated, newest kept. One integrates in a few milliseconds and + /// keyframes are kept at most about three a second, so more than a few waiting means the + /// queue is stuck behind something else. A guess, not measured. + private static let waitingEstimates = 4 + + /// Anchors ARKit added or updated: mesh chunks and planes. Other anchors are ignored. + func ingest(updated anchors: [ARAnchor]) { + var planes: [UUID: PlaneObservation] = [:] + var chunks: [UUID: MeshChunk] = [:] + for anchor in anchors { + if let mesh = anchor as? ARMeshAnchor { + chunks[mesh.identifier] = Map3DFeed.meshChunk(mesh) + } else if let plane = anchor as? ARPlaneAnchor { + planes[plane.identifier] = Map3DFeed.plane(plane) + } + } + guard !planes.isEmpty || !chunks.isEmpty else { return } + inbox.withLock { inbox in + for (id, plane) in planes { inbox.planes[id] = .some(plane) } + for (id, chunk) in chunks { inbox.chunks[id] = .some(chunk) } + schedule(&inbox) + } + } + + func ingest(removed anchors: [ARAnchor]) { + let planes = anchors.filter { $0 is ARPlaneAnchor }.map(\.identifier) + let chunks = anchors.filter { $0 is ARMeshAnchor }.map(\.identifier) + guard !planes.isEmpty || !chunks.isEmpty else { return } + inbox.withLock { inbox in + for id in planes { inbox.planes[id] = .some(nil) } + for id in chunks { inbox.chunks[id] = .some(nil) } + schedule(&inbox) + } + } + + /// True while inputs wait to be integrated or a change to the map has not yet reached the + /// main actor as a snapshot. The autopilot waits for it before acting on coverage; a person + /// never needs to. Blocks while an integration runs. + /// Changes with every `start` and `reset`: what `ingest(estimated:generation:)` checks. + var generation: Int { inbox.withLock { $0.generation } } + + var isCatchingUp: Bool { + if inbox.withLock({ $0.drainScheduled || $0.frame != nil || !$0.estimated.isEmpty || !$0.replay.isEmpty || $0.command != nil || $0.reset != nil }) { return true } + return core.withLock { core in + core.map != nil && (core.revision != core.publishedRevision || core.snapshotRunning || core.publishScheduled) + } + } + + // MARK: Export + + /// A snapshot of everything received so far, with `measured` filled, computed on the calling + /// thread. Nil before `start`. It waits for an integration already running and can take a + /// while itself, so call it off the main actor. + /// The chosen-spot check (`Map3D.spotView`) on the map as it stands, frames still waiting + /// integrated first, for `span` in meters of s along `wall`. Unknown while there is no map, + /// which leaves the decision to the homeowner. Blocks while it reads; call it off the main actor. + func spotView(span: ClosedRange, along wall: WallFrame) -> SpotView { + core.withLock { core in + drainInbox(into: &core) + guard let map = core.map else { return .unknown } + return map.spotView(span: span, along: wall) + } + } + + func finalSnapshot() -> Map3DSnapshot? { + core.withLock { core in + drainInbox(into: &core) + guard let map = core.map, let wall = core.wall else { return nil } + var snapshot = Self.snapshot(map, wall: wall, revision: core.revision) + snapshot.integratedDepth = core.integratedDepth + if let chain = snapshot.chain, let measuredWall = chain.wallFrame(meter: wall.meter, groundY: wall.groundY, frame: map.frame) { + snapshot.measured = (measuredWall, map.coverage(along: measuredWall)) + } + return snapshot + } + } + + // MARK: Queue + + private enum FrameInput: Sendable { + case depth(DepthFrame) + case features(FeatureFrame, planes: [PlaneObservation]) + } + + private enum WallCommand: Sendable { + case start(WallFrame) + case update(WallFrame) + } + + private struct Inbox: Sendable { + /// Bumped by `start` and `reset`, so a snapshot of an older map is never delivered. + var generation = 0 + /// A wall is set: frames are taken. + var active = false + var frame: FrameInput? + var estimated: [DepthFrame] = [] + /// Replay depth frames in play order, converted on the queue, off the main actor. + var replay: [(depth: DepthImage, camera: CameraFrame)] = [] + var replayLength = 0 + /// Frames replaced before they were integrated; logged with the next snapshot. + var dropped = 0 + /// Applied before `command`. The value says whether to forget the anchors too. + var reset: Bool? + var command: WallCommand? + /// Nil removes the anchor. + var planes: [UUID: PlaneObservation?] = [:] + var chunks: [UUID: MeshChunk?] = [:] + var drainScheduled = false + } + + private struct Core: Sendable { + var map: Map3D? + var wall: WallFrame? + var generation = 0 + /// Planes ARKit has, newest kept within `anchorBudget` boundary points. + var planes = BoundedRecent(budget: Map3DSession.planeBudget) { $0.boundary.count } + /// Chunks that arrived while there was no map, for the next `start`, newest kept within + /// `chunkBudget` vertices. + var waitingChunks = BoundedRecent(budget: Map3DSession.chunkBudget) { $0.vertices.count } + /// A measured depth frame (LiDAR or a replay's) went into the map since `start` + /// (`Map3DSnapshot.integratedDepth`). Estimated depth doesn't count: it never certifies + /// coverage, so a map with only estimated depth can't decide it. A rebuild after the + /// ground moved keeps it: the phone still gives depth. + var integratedDepth = false + var revision = 0 + var publishedRevision = -1 + /// `ProcessInfo.systemUptime` at which the last snapshot was started. + var lastPublish: Double? + var publishScheduled = false + var snapshotRunning = false + var dropped = 0 + } + + /// Call with the inbox locked. + private func schedule(_ inbox: inout Inbox) { + guard !inbox.drainScheduled else { return } + inbox.drainScheduled = true + queue.async { [self] in + core.withLock { drainInbox(into: &$0) } + publishIfDue() + } + } + + /// Call with the core locked: takes everything in the inbox and applies it. + private func drainInbox(into core: inout Core) { + let taken = inbox.withLock { inbox -> Inbox in + let taken = inbox + inbox.frame = nil + inbox.estimated = [] + inbox.replay = [] + inbox.dropped = 0 + inbox.reset = nil + inbox.command = nil + inbox.planes = [:] + inbox.chunks = [:] + inbox.drainScheduled = false + return taken + } + core.generation = taken.generation + core.dropped += taken.dropped + var changed = false + + if let forgetAnchors = taken.reset { + core.map = nil + core.wall = nil + if forgetAnchors { + core.planes.removeAll() + core.waitingChunks.removeAll() + } + core.integratedDepth = false + changed = true + } + var moved: MapFrame? + switch taken.command { + case .start(let wall)?: + var map = Self.newMap(at: wall, planes: core.planes) + var outside = 0 + for chunk in core.waitingChunks.values { + if Self.reaches(chunk, map) { map.update(chunk) } else { outside += 1 } + } + let waiting = core.waitingChunks.keys.count + let overBudget = core.waitingChunks.evicted + if waiting > 0 || overBudget > 0 { + RuntimeLog.engine.info("3D map started with \(waiting - outside) waiting mesh chunks; \(outside) wholly outside its bounds and \(overBudget) over the budget dropped") + } + core.waitingChunks.removeAll() + core.integratedDepth = false + core.map = map + core.wall = wall + changed = true + case .update(let wall)?: + if follow(wall, in: &core, moved: &moved) { changed = true } + case nil: + break + } + // Anchors after the meter's move: ARKit sent them placed in the corrected world + // (`Map3D.apply`). Without a map they wait for the next start, within a budget. + for (id, plane) in taken.planes { core.planes[id] = plane } + if var map = core.map { + core.map = nil + map.apply(frame: moved, planes: taken.planes, chunks: taken.chunks) + core.map = map + if !taken.planes.isEmpty || !taken.chunks.isEmpty { changed = true } + } else { + for (id, chunk) in taken.chunks { core.waitingChunks[id] = chunk } + } + // A frame taken after a reset but before the next start has no map to go into. + if var map = core.map, let frame = taken.frame { + // Moved out of `core` so the grid is integrated in place instead of copied. + core.map = nil + switch frame { + case .depth(let depth): + map.integrate(depth) + core.integratedDepth = true + case .features(let features, let planes): + // The frame's planes are every plane ARKit had then: any other one is gone. + let current = Set(planes.map(\.id)) + for id in core.planes.keys where !current.contains(id) { + core.planes[id] = nil + map.removePlane(id: id) + } + for plane in planes { + core.planes[plane.id] = plane + map.update(plane) + } + map.integrate(features) + } + core.map = map + changed = true + } + if var map = core.map, !taken.estimated.isEmpty { + core.map = nil + for depth in taken.estimated { map.integrate(depth) } + core.map = map + changed = true + } + if var map = core.map, !taken.replay.isEmpty { + core.map = nil + for item in taken.replay { map.integrate(DepthFrame(image: item.depth, pose: item.camera)) } + core.integratedDepth = true + core.map = map + changed = true + } + if changed { core.revision += 1 } + } + + /// Applies a changed wall to the map: the map follows the meter, or is rebuilt when the ground + /// moved beyond `groundTolerance`. Returns whether anything changed. + /// `moved` is the frame the map follows the meter to, for `Map3D.apply` to reanchor before + /// the batch's anchors; a rebuilt map is already in the new frame. + private func follow(_ wall: WallFrame, in core: inout Core, moved: inout MapFrame?) -> Bool { + guard let map = core.map, let old = core.wall, wall != old else { return false } + core.wall = wall + let ground = wall.groundY - wall.meter.y + guard abs(ground - map.frame.groundY) <= Self.groundTolerance else { + RuntimeLog.engine.info("3D map rebuilt without its rays and mesh: the ground moved from \(map.frame.groundY) to \(ground) m below the meter") + core.map = Self.newMap(at: wall, planes: core.planes) + return true + } + guard wall.meter != old.meter else { return true } + moved = map.frame.following(anchorMovedFrom: Self.translation(old.meter), to: Self.translation(wall.meter)) + return true + } + + private static func newMap(at wall: WallFrame, planes: BoundedRecent) -> Map3D { + var map = Map3D(frame: MapFrame(wall: wall)) + for plane in planes.values { map.update(plane) } + return map + } + + /// Whether the box around a chunk's vertices, in the map's frame, meets the map's bounds. + private static func reaches(_ chunk: MeshChunk, _ map: Map3D) -> Bool { + let mapFromChunk = map.frame.mapFromWorld * chunk.worldFromChunk + var low = SIMD3(repeating: .infinity) + var high = SIMD3(repeating: -.infinity) + for v in chunk.vertices { + let p = mapFromChunk * SIMD4(v, 1) + let q = SIMD3(p.x, p.y, p.z) + low = simd_min(low, q) + high = simd_max(high, q) + } + return all(high .>= map.bounds.min) && all(low .< map.bounds.max) + } + + private static func translation(_ p: SIMD3) -> simd_float4x4 { + var m = matrix_identity_float4x4 + m.columns.3 = SIMD4(p, 1) + return m + } + + private static func snapshot(_ map: Map3D, wall: WallFrame, revision: Int) -> Map3DSnapshot { + Map3DSnapshot( + revision: revision, frame: map.frame, wall: wall, coverage: map.coverage(along: wall), + chain: map.measuredWalls(), fog: map.fogOfWar(along: wall), nextView: map.nextBestView(along: wall), measured: nil) + } + + /// On the queue: when the map changed, no snapshot is being read and the last one started at + /// least `snapshotInterval` ago, starts reading one from a copy of the map on the snapshot + /// queue; too soon, schedules itself for when it will be due. A finished snapshot checks again, + /// for changes made while it was read. + private func publishIfDue() { + let now = ProcessInfo.processInfo.systemUptime + let job = core.withLock { core -> (map: Map3D, wall: WallFrame, revision: Int, generation: Int, dropped: Int)? in + guard let map = core.map, let wall = core.wall, core.revision != core.publishedRevision, !core.snapshotRunning else { return nil } + if let last = core.lastPublish, now - last < Self.snapshotInterval { + guard !core.publishScheduled else { return nil } + core.publishScheduled = true + queue.asyncAfter(deadline: .now() + (Self.snapshotInterval - (now - last))) { [self] in + self.core.withLock { $0.publishScheduled = false } + publishIfDue() + } + return nil + } + core.publishedRevision = core.revision + core.lastPublish = now + core.snapshotRunning = true + let dropped = core.dropped + core.dropped = 0 + return (map, wall, core.revision, core.generation, dropped) + } + guard let job else { return } + if job.dropped > 0 { + RuntimeLog.engine.info("3D map: \(job.dropped) frames replaced before they were integrated") + } + let onSnapshot = onSnapshot + snapshotQueue.async { [self] in + let snapshot = Self.snapshot(job.map, wall: job.wall, revision: job.revision) + Task { @MainActor [self] in + // A snapshot of a map dropped since (a reset or a new start) must not reach the UI. + if inbox.withLock({ $0.generation }) == job.generation { onSnapshot(snapshot) } + // Only now, so `isCatchingUp` stays true until the snapshot is applied. + core.withLock { $0.snapshotRunning = false } + queue.async { [self] in publishIfDue() } + } + } + } +} diff --git a/ios/HouseScan/Runtime/ReplayPlayer.swift b/ios/HouseScan/Runtime/ReplayPlayer.swift index 4b734de19..8bdc0888c 100644 --- a/ios/HouseScan/Runtime/ReplayPlayer.swift +++ b/ios/HouseScan/Runtime/ReplayPlayer.swift @@ -82,12 +82,20 @@ final class ReplayPlayer { } /// Finds the frames to hold back for the autopilot's gap loop. Heavy, so it runs off the - /// main actor; call once before the walk. - func prepareHeldBack() async { + /// main actor; call once before the walk. With `map3D` (the 3D map is the coverage model) and a + /// recording with depth, the window is planned against the map the app will count coverage + /// with; otherwise against the camera coverage map. + func prepareHeldBack(map3D: Bool) async { let planned = planned let wall = wall + let session = session + let folder = folder heldBack = await Task.detached(priority: .userInitiated) { - ReplayPlanning.heldBackWindow(frames: planned, wall: wall) + guard map3D, session.frames.contains(where: { $0.depth != nil }) else { + return ReplayPlanning.heldBackWindow(frames: planned, wall: wall) + } + let depths = session.frames.map { Self.loadDepth($0, folder: folder) } + return ReplayPlanning.heldBackWindow(frames: planned, depths: depths, wall: wall) }.value } diff --git a/ios/HouseScan/Runtime/ScanEngine+Actions.swift b/ios/HouseScan/Runtime/ScanEngine+Actions.swift index 1d7453e1a..2afa57448 100644 --- a/ios/HouseScan/Runtime/ScanEngine+Actions.swift +++ b/ios/HouseScan/Runtime/ScanEngine+Actions.swift @@ -402,7 +402,7 @@ extension ScanEngine: ScanActions { let index = Int(id.replacingOccurrences(of: "missing-", with: "")), missing.indices.contains(index) else { return } let item = missing[index] - guard let map = coverage, let plan = gapPlanner.plan(for: item, leftEnd: map.leftEnd, rightEnd: map.rightEnd, limitEnds: map.limitEnds) else { return } + guard let map = coverage, let plan = gapPlanner.plan(for: walkRequest(item), leftEnd: map.leftEnd, rightEnd: map.rightEnd, limitEnds: map.limitEnds) else { return } beginServerGap(item, plan: plan) } diff --git a/ios/HouseScan/Runtime/ScanEngine+Export.swift b/ios/HouseScan/Runtime/ScanEngine+Export.swift index 00898b6b6..7d44d8b3f 100644 --- a/ios/HouseScan/Runtime/ScanEngine+Export.swift +++ b/ios/HouseScan/Runtime/ScanEngine+Export.swift @@ -10,12 +10,17 @@ extension ScanEngine { /// The scene frame is the AR world frame moved down so the ground at the wall is y = 0: the /// server reads heights (meter.pos y, pose translations) as height above that ground, and /// scene.json has no field for the ground's height otherwise. - func sceneJSON(mesh: MeshMeasurements = MeshMeasurements()) throws -> Data { - guard let map = coverage else { throw ExportError.noWall } - let wall = map.wall + /// + /// Everything is placed against `geometry.wall` (`exportGeometry()`): marks are read again + /// from their tapped world points when that is the measured chain rather than the walk's wall. + func sceneJSON(mesh: MeshMeasurements = MeshMeasurements(), geometry: ExportGeometry) throws -> Data { + guard coverage != nil else { throw ExportError.noWall } + let wall = geometry.wall + let walk = geometry.walk let drop = SIMD3(0, wall.groundY, 0) - // The corners carry their pieces' sources (`markNextWall`); the meter's piece, its own - // (`meterLineSource`, set on the map in `markMeter`). + // The corners carry their pieces' sources (`markNextWall`, or the measured chain's); the + // meter's piece, its own (`meterLineSource`, set on the map in `markMeter`, or the + // measured piece's). let sceneWall = SceneWall( meter: wall.meter - drop, outward: wall.outward, groundY: 0, leftCorners: wall.leftCorners, rightCorners: wall.rightCorners, source: wall.source) @@ -27,7 +32,9 @@ extension ScanEngine { // nearest valid shape here and logged. A driveway or fence with nothing valid left fails // the export instead (`ExportError.markCollapsed`): dropping it would send the ground // near it as seen and clear, which can pass its clearance check with the hazard unsent. - let features: [SceneFeature] = try state.features.map { feature in + let features: [SceneFeature] = try state.features.map { marked in + var feature = marked + if wall != walk { Self.project(&feature, onto: wall) } let points = feature.points.map { $0 - drop } switch feature.kind { case .door, .window: @@ -64,25 +71,87 @@ extension ScanEngine { let groundError: Float? = groundMeasured ? nil : Self.estimatedGroundError let input = SceneInput( wall: sceneWall, - baselineS: Self.exportSpan(map), + baselineS: geometry.baselineS, meterPlusMinus: groundError, meterPlane: meterPlaneSource, objectPlusMinus: groundError, features: features, - coverage: SceneCoverage( - map, leftEndMarked: wallEndKinds[.left] == .limit, rightEndMarked: wallEndKinds[.right] == .limit - ), + coverage: geometry.coverage, keyframes: keyframes, stills: store.stills, // Written without plus_minus_ft: the server takes its mesh error for both. meshFacing: mesh.facing, meshOverheads: mesh.overheads, // Unanswered exports like "Not sure": no patch, and the server reports the surface unknown. - groundType: state.groundAnswer.flatMap(Self.sceneGroundType) + groundType: state.groundAnswer.flatMap(Self.sceneGroundType), + wallPlusMinus: geometry.plusMinus, + // A measured wall lies on its fitted line; the meter stays where it was tapped, which + // may be on a box proud of that line. + meterPosition: wall.meter == walk.meter ? nil : walk.meter - drop ) return try SceneExport.jsonData(input) } + /// The wall scene.json describes, the stretch of it, what was seen along it, and how well each + /// of its pieces' lines is known. + struct ExportGeometry: Sendable { + var wall: WallFrame + /// The walk's wall this was built against: the meter, ends and marks are read from it. + var walk: WallFrame + var baselineS: ClosedRange + var coverage: SceneCoverage + /// `SceneInput.wallPlusMinus`; empty takes the server's default for every piece. + var plusMinus: [Float?] + } + + /// Under `-coverage legacy`, the walk's tapped wall and the coverage map's sightings. Under + /// `map3d`, the 3D map's (`Map3DCoverageSource.export`): the measured wall chain when it + /// matches the walk, and coverage read along whichever wall is written. + /// + /// The 3D map is used only once some depth went into it (LiDAR, a replay's, or estimated + /// with `-estimatedDepth on` and the model present). Without, it holds only feature points + /// and planes and would report near-empty coverage, so the camera coverage map decides here + /// as it does for the walk on such a phone. + /// + /// Everything is read from one state of the walk's wall (`ExportGeometry.walk`); the caller + /// must check it is still the walk's wall after any await (`upload`). + func exportGeometry() async throws -> ExportGeometry { + guard let start = coverage else { throw ExportError.noWall } + func camera(_ map: CoverageMap, because reason: String) -> ExportGeometry { + RuntimeLog.engine.info("export: camera coverage map, \(reason, privacy: .public)") + return ExportGeometry( + wall: map.wall, walk: map.wall, baselineS: Self.exportSpan(map), + coverage: SceneCoverage(map, leftEndMarked: wallEndKinds[.left] == .limit, rightEndMarked: wallEndKinds[.right] == .limit), + plusMinus: []) + } + guard let map3D else { return camera(start, because: "-coverage legacy") } + // The snapshot is computed off the main actor, and meanwhile the meter anchor can move the + // wall. One read along another wall than the current one is taken again; the next read + // includes the move. Three tries, then the export fails rather than mix two walls. + for _ in 0..<3 { + let snapshot = await Task.detached(priority: .userInitiated) { map3D.finalSnapshot() }.value + guard let snapshot else { throw ExportError.noMap3D } + guard let map = coverage else { throw ExportError.noWall } + guard snapshot.wall == map.wall else { continue } + guard snapshot.integratedDepth else { + return camera(map, because: "the 3D map integrated no depth (no LiDAR, and estimated depth off or without its model)") + } + let export = try Map3DCoverageSource.export( + snapshot, tapWall: map.wall, baselineS: Self.exportSpan(map), + leftEndMarked: wallEndKinds[.left] == .limit, rightEndMarked: wallEndKinds[.right] == .limit, + walkedFacing: map.facingSpans(), confirmedOverhead: map.overheadSpans()) + let why = if let reason = export.tappedBecause { + "tapped wall (\(reason)); walked facing and confirmed overhead merged in" + } else { + "measured wall; walked facing and confirmed overhead carried onto its meter piece" + } + RuntimeLog.engine.info("export: 3D map revision \(snapshot.revision), \(why, privacy: .public), \(export.wall.segments.count) pieces, s \(export.baselineS.lowerBound)...\(export.baselineS.upperBound)") + return ExportGeometry( + wall: export.wall, walk: map.wall, baselineS: export.baselineS, coverage: export.coverage, plusMinus: export.plusMinus) + } + throw ExportError.wallKeptMoving + } + static func sceneGroundType(_ answer: GroundAnswer) -> SceneGroundType? { switch answer { case .notSure: nil @@ -104,8 +173,10 @@ extension ScanEngine { /// where along that line the meter is. On an estimated plane there is no anchor: ARKit fits a /// plane to the feature points around the tapped pixel for that one raycast, which is what an /// AR tap is, and the schema's `plane` means detected planes, so it is `tap`. Nothing in the - /// capture takes a wall line from the LiDAR mesh (`MeshProbe` only measures in front of and - /// above a line already set), so no piece is `mesh`. + /// walk takes a wall line from the LiDAR mesh (`MeshProbe` only measures in front of and + /// above a line already set), so no tapped piece is `mesh`. Under `-coverage map3d` the export + /// can write the 3D map's measured chain instead, whose pieces are `mesh` or `plane` + /// (`exportGeometry()`). /// /// A replay's wall is `tap`: a recorded measure-lab wall runs through two tapped ground /// contacts (its `contacts`), and a wall assumed from the trajectory was never measured at @@ -172,6 +243,10 @@ extension ScanEngine { enum ExportError: Error, CustomStringConvertible { case noWall + /// Under `-coverage map3d` there is a wall but no 3D map: `setWall` starts both. + case noMap3D + /// The wall moved during each of three reads of the 3D map. + case wallKeptMoving /// A driveway or fence whose taps no longer make a line. The homeowner has to mark it /// again; `UploadFailure.packaging(_:)` says so. case markCollapsed(FeatureKind) @@ -179,16 +254,21 @@ extension ScanEngine { var description: String { switch self { case .noWall: "There is no wall to export yet." + case .noMap3D: "The 3D map was never started for this wall." + case .wallKeptMoving: "The wall moved while the 3D map was read, three times running." case .markCollapsed(let kind): "The \(kind.rawValue) mark's taps don't make a line." } } } - /// The server's result in the terms the screens use (meters, wall coordinates). + /// The server's result in the terms the screens use (meters, wall coordinates), along the + /// wall scene.json described (`exportedWall`), which under `-coverage map3d` can be the + /// measured chain rather than the walk's. func presentation(of result: PlacementResult, isSample: Bool) -> ResultPresentation { let meters: (Double) -> Float = { Float($0 * 0.3048) } - let sceneWall = coverage.map { - SceneWall(meter: $0.wall.meter, outward: $0.wall.outward, groundY: $0.wall.groundY, leftCorners: $0.wall.leftCorners, rightCorners: $0.wall.rightCorners) + let placedWall = exportedWall ?? coverage?.wall + let sceneWall = placedWall.map { + SceneWall(meter: $0.meter, outward: $0.outward, groundY: $0.groundY, leftCorners: $0.leftCorners, rightCorners: $0.rightCorners) } var spot: BatterySpot? @@ -278,7 +358,7 @@ extension ScanEngine { // a facing item its out_ft, which a walk can reach), a past_end item its side. // Otherwise the button would do nothing. A request the homeowner already skipped // or answered with something overhead stays with the installer. - capturable: gapPlanner.plan(for: item, leftEnd: coverage?.leftEnd, rightEnd: coverage?.rightEnd, limitEnds: coverage?.limitEnds ?? []) + capturable: gapPlanner.plan(for: walkRequest(item), leftEnd: coverage?.leftEnd, rightEnd: coverage?.rightEnd, limitEnds: coverage?.limitEnds ?? []) .map { !skippedGaps.contains($0) } ?? false ) } @@ -304,10 +384,20 @@ extension ScanEngine { clearances: clearances, missing: missing, unseenSide: unseen, - isSample: isSample + isSample: isSample, + wall: resultWall() ) } + /// The answer's wall (`exportedWall`, or the walk's) as the result screens draw it, with the + /// marked ends carried onto it from the walk's wall. + func resultWall() -> WallGeometry? { + guard let walk = coverage?.wall else { return nil } + let wall = exportedWall ?? walk + return Self.geometry( + wall, leftEnd: coverage?.leftEnd.map { walk.s($0, along: wall) }, rightEnd: coverage?.rightEnd.map { walk.s($0, along: wall) }) + } + static func outcome(_ outcome: PlacementOutcome) -> CheckOutcome { switch outcome { case .pass: .pass diff --git a/ios/HouseScan/Runtime/ScanEngine.swift b/ios/HouseScan/Runtime/ScanEngine.swift index feeb3e252..f2de76b4c 100644 --- a/ios/HouseScan/Runtime/ScanEngine.swift +++ b/ios/HouseScan/Runtime/ScanEngine.swift @@ -23,6 +23,36 @@ final class ScanEngine { // Capture logic (HouseScanKit) private(set) var coverage: CoverageMap? + /// The 3D map under `-coverage map3d`; nil under `legacy`. + private(set) var map3D: Map3DSession? + /// Estimated depth into `map3D` on a live phone without LiDAR, with `-estimatedDepth on` + /// and the model present; nil otherwise (`startEstimatedDepth`). + private var depthEstimator: DepthEstimator? + + /// Whether a phone without LiDAR runs the depth model by default. Off: on ETH3D electro + /// (section 7's 19.3 ft wall, photos only, depth scaled by structure-from-motion points) + /// Map3D with estimated depth claimed 0 ft of wall, ground, facing and overhead, so it is + /// not a coverage model yet, and CoverageMap's camera sightings keep deciding the walk. + /// `-estimatedDepth on` runs it for trying on a phone. + nonisolated static let estimatedDepthByDefault = false + + /// Whether the 3D map decides the strip's and planners' covered cells for the current wall: + /// with LiDAR depth, or with estimated depth running when the wall was set. Otherwise the + /// map is too sparse to walk by and `CoverageMap`'s camera sightings decide. Fixed per wall, + /// so a model that finishes loading mid-walk doesn't take covered cells off the strip. + private var mapDecidesCoverage = false + /// The wall the last scene.json described (`exportGeometry()`), with the walk's wall at that + /// moment, so the answer's wall can follow the walk's as the meter anchor is refined. + private var exported: (wall: WallFrame, walk: WallFrame)? + /// The scene.json last packaged for the upload: what the result answers. + private(set) var uploadedScene: Data? + /// The wall the last scene.json described, where it is now: the server's s runs along it. + /// Under `-coverage map3d` it can be the measured chain rather than `coverage.wall`; it moves + /// as the walk's wall moves (`WallFrame.following`). + var exportedWall: WallFrame? { + guard let exported, let walk = coverage?.wall else { return nil } + return exported.wall.following(exported.walk, to: walk) + } private var autoCapture = AutoCapture() private var closeUpGate = CloseUpGate() private var planner = GuidancePlanner() @@ -168,6 +198,9 @@ final class ScanEngine { state.isAutopilot = options.autopilot state.isReplay = options.replayFolder != nil state.usesSampleResult = resultClient.isSample + if options.coverage == .map3d { + map3D = Map3DSession { [weak self] snapshot in self?.applyMap3D(snapshot) } + } } // MARK: Start @@ -195,6 +228,7 @@ final class ScanEngine { let loaded = try await Task.detached(priority: .userInitiated) { try ReplayPlayer.load(folder: folder) }.value let player = ReplayPlayer(folder: folder, loaded: loaded) { [weak self] frame in self?.ingest(frame) } replay = player + map3D?.expectReplay(frames: player.frames.count) sourceState.sourceStarted() state.spatialResultAvailable = true let withDepth = player.frames.filter { $0.depth != nil }.count @@ -292,7 +326,8 @@ final class ScanEngine { state.spatialResultAvailable = true let capture = LiveCapture( onFrame: { [weak self] frame in self?.ingest(frame) }, - onEvent: { [weak self] event in self?.handle(event) } + onEvent: { [weak self] event in self?.handle(event) }, + map3D: map3D ) live = capture state.feed = .live @@ -300,6 +335,18 @@ final class ScanEngine { capture.setRecorder(recorder) capture.start() updateRecording() + startEstimatedDepth() + } + + /// Loads the depth model for a live phone without LiDAR when `-estimatedDepth on` and the + /// model is present. Until it loads, and without it, `CoverageMap` decides coverage. + private func startEstimatedDepth() { + guard options.estimatedDepth, let map3D, !LiveCapture.supportsDepth else { return } + Task { + guard let estimator = await DepthEstimator.load(onFrame: { map3D.ingest(estimated: $0, generation: $1) }), let live else { return } + depthEstimator = estimator + live.setDepthEstimator(estimator) + } } private static func makeRecorder(_ store: KeyframeStore) -> CaptureRecorder { @@ -353,6 +400,11 @@ final class ScanEngine { refineGround() } refreshMeterFromAnchor(frame) + // Live frames reach the 3D map on the AR delegate queue, straight from ARKit. A replay's + // frames count only on the walk and in a gap request: for the close-up a replay plays its + // whole recording, and counting those frames would put the ones held back for the gap + // loop (`ReplayPlanning.heldBackWindow`) in the map before the walk. + if replay != nil, state.phase == .wallWalk || state.phase == .gapRequest { map3D?.ingest(frame) } guard !frame.isPoseOnly else { return } trackRelocalization(frame) guard !frame.isReview else { @@ -652,6 +704,7 @@ final class ScanEngine { let delta = coverage?.observe(frame.camera, trackingNormal: frame.tracking == .normal, time: frame.timestamp, depth: frame.depth, segment: segment) RuntimeLog.capture.info("stored \(frame.id, privacy: .public) as keyframe \(index)\(frame.depth == nil ? "" : " with depth", privacy: .public): \(delta?.newlySeen ?? 0) cells newly seen, \(delta?.newlyCovered ?? 0) newly covered, \(delta?.newlyHidden ?? 0) newly hidden") if overhead { recordOverhead(frame) } + if frame.tracking == .normal, let map3D { depthEstimator?.estimate(frameID: frame.id, generation: map3D.generation) } state.captureCount += 1 state.lastCapture = CaptureEvent(id: state.captureCount, kind: kind, thumbnail: saved.thumbnail) afterCoverageChange(camera: lastFrame?.camera, time: lastFrame?.timestamp ?? frame.timestamp) @@ -1090,6 +1143,11 @@ final class ScanEngine { // A fresh map: the old world frame is gone, so its anchors and planes are meaningless. live?.restart() coverage = nil + map3D?.reset(forgetAnchors: true) + depthEstimator?.reset() + state.map3D = nil + exported = nil + uploadedScene = nil meterAnchorID.map { live?.removeAnchor($0) } meterAnchorID = nil meterPlaneSource = .detectedPlane @@ -1141,6 +1199,13 @@ final class ScanEngine { func setWall(meter: SIMD3, outward: SIMD3, groundY: Float, groundMeasured: Bool) -> Bool { guard let frame = WallFrame(meter: meter, outward: outward, groundY: groundY) else { return false } coverage = CoverageMap(wall: frame) + exported = nil + uploadedScene = nil + map3D?.start(wall: frame) + // With measured depth the 3D map decides what is covered from the start (`applyMap3D`). + // Estimated depth only clears fog and places the next view: it certifies no coverage. + mapDecidesCoverage = map3D != nil && state.depthAvailable + if mapDecidesCoverage { coverage?.setMeasuredCovered([:]) } coverage?.heightError = groundMeasured ? 0 : Self.estimatedGroundError self.groundMeasured = groundMeasured endKinds = [:] @@ -1157,23 +1222,48 @@ final class ScanEngine { func publishWall() { guard let map = coverage else { state.wall = nil; return } let wall = map.wall - state.wall = WallGeometry( + state.wall = Self.geometry(wall, leftEnd: map.leftEnd, rightEnd: map.rightEnd) + // The result is drawn along the answer's wall, which moves with this one. + if state.result?.wall != nil { state.result?.wall = resultWall() } + if state.phase == .resultAR { showResultInCamera(rising: false) } + publishFeaturesPastEnds() + // Every change to the walk's wall passes here; the map ignores an unchanged one. + map3D?.update(wall: wall) + } + + /// A 3D map snapshot, about twice a second while the map changes. Its fog goes to the overlay. + /// With depth, its seen cells become the coverage map's covered cells, so the strip and the + /// planners ask for what the export, which reads the 3D map, will send. Without depth the map + /// has only feature points (or nothing, on a replay), too sparse to walk by (ios/README.md + /// "3D map"), so the camera sightings keep deciding and the export may report less. + private func applyMap3D(_ snapshot: Map3DSnapshot) { + // Read along an older wall: the wall change has already queued a newer snapshot. + guard var map = coverage, snapshot.wall == map.wall else { return } + state.map3D = Map3DFog(fog: snapshot.fog, nextView: snapshot.nextView, frame: snapshot.frame) + guard mapDecidesCoverage else { return } + map.setMeasuredCovered(map.cells(seenIn: snapshot.coverage)) + guard map.revision != coverage?.revision else { return } + coverage = map + afterCoverageChange(camera: lastFrame?.camera, time: lastFrame?.timestamp ?? 0) + } + + static func geometry(_ wall: WallFrame, leftEnd: Float?, rightEnd: Float?) -> WallGeometry { + WallGeometry( meter: wall.meter, along: wall.along, outward: wall.outward, groundY: wall.groundY, - leftEnd: map.leftEnd, rightEnd: map.rightEnd, + leftEnd: leftEnd, rightEnd: rightEnd, cornerSegments: wall.segments.indices.filter { $0 != wall.meterSegmentIndex }.map { index in let piece = wall.segments[index] return WallGeometry.Segment(span: piece.span, along: piece.along, outward: piece.outward, anchor: piece.anchor, anchorS: piece.anchorS) } ) - if state.phase == .resultAR { showResultInCamera(rising: false) } - publishFeaturesPastEnds() } /// "See it on your wall" on the live camera: the result goes into the AR scene on the meter's /// anchor, where people and objects in front of it hide it. A replay, or a meter without an /// anchor, leaves it to the screen's overlay. private func showResultInCamera(rising: Bool) { - guard let live, let wall = state.wall, let result = state.result, let pose = meterTracking?.pose, + // Along the answer's wall (`ResultPresentation.wall`), as the screens draw it. + guard let live, let result = state.result, let wall = result.wall ?? state.wall, let pose = meterTracking?.pose, state.spatialResultAvailable else { return } let model = ResultARModel.build(wall: wall, result: result) state.resultInCamera = live.showResult(model, builtFor: pose) @@ -1376,26 +1466,59 @@ final class ScanEngine { try? await Task.sleep(for: .milliseconds(50)) } guard scan == generation else { return } - // LiDAR phones: the mesh ARKit built, measured for what faces the wall and what is - // overhead, off the main actor since ray casts over a whole mesh take a while. - let meshSnapshot = live?.meshSnapshot() - var measured = MeshMeasurements() - if let mesh = meshSnapshot?.mesh, let map = coverage { - let wall = map.wall - let span = Self.exportSpan(map) - measured = await Task.detached(priority: .userInitiated) { Self.measure(mesh, wall: wall, over: span) }.value + // The wall, the meter and the export geometry are read as one state: the meter anchor can + // move the walk's wall while the mesh is measured off the main actor, and a scene pairing + // the new meter with the old wall fails to package (`meterOffChainOrigin`). A move during + // the measurement takes it again; three tries, then the upload fails rather than mix them. + var consistent: (geometry: ExportGeometry, mesh: LiveCapture.MeshSnapshot?, measured: MeshMeasurements)? + for _ in 0..<3 { + let geometry: ExportGeometry + do { + geometry = try await exportGeometry() + } catch { + guard scan == generation else { return } + RuntimeLog.engine.error("scene.json export failed: \(String(describing: error), privacy: .public)") + state.upload = UploadFailure.packaging(error) + return + } guard scan == generation else { return } - RuntimeLog.engine.info("mesh: \(mesh.vertices.count) vertices, \(mesh.indices.count / 3) triangles; \(measured.facing.count) facing and \(measured.overheads.count) overhead measurements") + // LiDAR phones: the mesh ARKit built, measured for what faces the wall and what is + // overhead, off the main actor since ray casts over a whole mesh take a while. Measured + // along the wall the scene describes, so its s agrees with the scene's. + let meshSnapshot = live?.meshSnapshot() + var measured = MeshMeasurements() + if let mesh = meshSnapshot?.mesh { + let wall = geometry.wall + let span = geometry.baselineS + measured = await Task.detached(priority: .userInitiated) { Self.measure(mesh, wall: wall, over: span) }.value + guard scan == generation else { return } + RuntimeLog.engine.info("mesh: \(mesh.vertices.count) vertices, \(mesh.indices.count / 3) triangles; \(measured.facing.count) facing and \(measured.overheads.count) overhead measurements") + } + guard coverage?.wall == geometry.walk else { + RuntimeLog.engine.info("export: the wall moved while the mesh was measured; reading it again") + continue + } + consistent = (geometry, meshSnapshot, measured) + break + } + guard let (geometry, meshSnapshot, measured) = consistent else { + RuntimeLog.engine.error("scene.json export failed: \(ExportError.wallKeptMoving.description, privacy: .public)") + state.upload = UploadFailure.packaging(ExportError.wallKeptMoving) + updateRecording() + return } let scene: Data do { - scene = try sceneJSON(mesh: measured) + scene = try sceneJSON(mesh: measured, geometry: geometry) } catch { RuntimeLog.engine.error("scene.json export failed: \(String(describing: error), privacy: .public)") state.upload = UploadFailure.packaging(error) updateRecording() return } + uploadedScene = scene + // The answer's s runs along this wall; `presentation(of:)` places the result on it. + exported = (geometry.wall, geometry.walk) saveBundle(scene: scene, mesh: meshSnapshot) guard !Task.isCancelled else { return } state.upload = .uploading(fraction: 0) @@ -1437,6 +1560,13 @@ final class ScanEngine { } } + /// A server request with its span in the walk's s, where it is planned and tracked: the + /// answer's s runs along the wall scene.json described (`exportedWall`). + func walkRequest(_ item: PlacementMissingEvidence) -> PlacementMissingEvidence { + guard let exported = exportedWall, let walk = coverage?.wall else { return item } + return item.along(walk, from: exported) + } + /// The first item of the answer's missing evidence a capture can settle (the result's /// "capturable"), not skipped and not yet raised in this pass; nil once the homeowner said /// they can't get to one, or after `maxAutomaticGaps` requests. @@ -1445,7 +1575,7 @@ final class ScanEngine { guard !automaticGapsStopped, automaticGaps.count < Self.maxAutomaticGaps, !state.tracking.hasLostItsPlace, let map = coverage else { return nil } for item in result.missingEvidence { - guard let plan = gapPlanner.plan(for: item, leftEnd: map.leftEnd, rightEnd: map.rightEnd, limitEnds: map.limitEnds), + guard let plan = gapPlanner.plan(for: walkRequest(item), leftEnd: map.leftEnd, rightEnd: map.rightEnd, limitEnds: map.limitEnds), !skippedGaps.contains(plan), !automaticGaps.contains(plan) else { continue } return (item, plan) } @@ -1523,7 +1653,7 @@ final class ScanEngine { /// The replay frame that sees the chosen spot best, for the AR result on a replay. private func bestFrameForResult() -> Int? { - guard let replay, let wall = coverage?.wall else { return nil } + guard let replay, let wall = exportedWall ?? coverage?.wall else { return nil } let spot = state.result?.spot let s = spot.map { ($0.span.lowerBound + $0.span.upperBound) / 2 } ?? 0 let point = wall.world(s: s, height: 0.5, out: 0.3) @@ -1537,6 +1667,12 @@ final class ScanEngine { uploadTask?.cancel() replay?.stop() coverage = nil + // The AR session and its anchors go on, so the mesh ARKit built stays with the map. + map3D?.reset(forgetAnchors: false) + depthEstimator?.reset() + state.map3D = nil + exported = nil + uploadedScene = nil meterAnchorID.map { live?.removeAnchor($0) } meterAnchorID = nil meterPlaneSource = .detectedPlane diff --git a/ios/HouseScan/UI/Camera/CameraOverlays.swift b/ios/HouseScan/UI/Camera/CameraOverlays.swift index da1916cee..58a83cdfe 100644 --- a/ios/HouseScan/UI/Camera/CameraOverlays.swift +++ b/ios/HouseScan/UI/Camera/CameraOverlays.swift @@ -18,8 +18,14 @@ struct CameraOverlays: View { // path and pins would sit in the wrong place (checklist T3). They fade back on recovery. if state.tracking == .normal, let projection = state.projection, let wall = state.wall { ZStack { - FogOverlay(coverage: state.coverage, wall: wall, projection: projection, highlight: highlight) - .ignoresSafeArea() + // Under `-coverage map3d` the 3D map's frost replaces the wall strip's. + if let map3D = state.map3D { + Map3DOverlay(fog: map3D.fog, nextView: map3D.nextView, frame: map3D.frame, projection: projection) + .ignoresSafeArea() + } else { + FogOverlay(coverage: state.coverage, wall: wall, projection: projection, highlight: highlight) + .ignoresSafeArea() + } WallMarksOverlay( projection: projection, wall: wall, diff --git a/ios/HouseScan/UI/Camera/Map3DOverlay.swift b/ios/HouseScan/UI/Camera/Map3DOverlay.swift new file mode 100644 index 000000000..f90d9c10d --- /dev/null +++ b/ios/HouseScan/UI/Camera/Map3DOverlay.swift @@ -0,0 +1,638 @@ +import HouseScanKit +import simd +import SwiftUI + +/// The 3D map over the camera: frost on the space in front of the wall the phone hasn't seen yet, +/// and one blue cue for the next view, a spot on the ground to stand on and then a ring to aim at. +/// +/// The frost is FogOverlay's (the same light material, wash, blur and unseen strength) drawn from +/// the map's 0.3 m fog cubes instead of the wall strip, so it hangs in the air where the unseen +/// space is and thins as the map fills in. The cue uses WayfindingOverlay's blue: a ground disk at +/// `stand` while the homeowner walks there, then an aim ring on `target` once they arrive, with an +/// edge chevron whenever the one that matters is off screen. Only one of the two shows at a time. +/// +/// Compose it inside CameraOverlays' tracking gate: drawn from a pose the phone doesn't trust, +/// the frost and cue would hang in the wrong place. +struct Map3DOverlay: View { + var fog: FogOfWar + var nextView: ViewSuggestion? + var frame: MapFrame + var projection: CameraProjection + + @State private var motion = Map3DCueMotion() + @State private var deadline = Date.distantPast + @Environment(\.accessibilityReduceMotion) private var reduceMotion + + var body: some View { + GeometryReader { proxy in + let size = proxy.size + TimelineView(.animation(paused: deadline < .now)) { timeline in + let haze = Map3DHaze(fog: fog, frame: frame, projection: projection, size: size) + let cue = Map3DCueLayout( + drawn: motion.drawn(at: timeline.date, reduceMotion: reduceMotion), + aimWeight: motion.aimWeight(at: timeline.date), + cellSize: fog.cellSize, frame: frame, projection: projection, size: size + ) + ZStack { + // The frost itself, as in FogOverlay: the material blurs the camera so the + // haze reads on a sunlit wall and on dark brick; the canvas decides where. + Rectangle() + .fill(.thinMaterial) + .environment(\.colorScheme, .light) + .mask { + Canvas(rendersAsynchronously: false) { context, _ in + haze.draw(in: &context, color: .black, strength: 1) + } + } + Canvas(rendersAsynchronously: false) { context, _ in + haze.draw(in: &context, color: Color(white: 0.98), strength: 0.35) + cue.drawMarks(in: &context) + } + ForEach(cue.chevrons.indices, id: \.self) { index in + Map3DChevron(angle: cue.chevrons[index].angle) + .position(cue.chevrons[index].point) + .opacity(cue.chevrons[index].opacity) + } + } + .accessibilityHidden(true) + .overlay { accessibleCue(in: size) } + } + } + .ignoresSafeArea() + .allowsHitTesting(false) + .onAppear { + motion.seed(nextView.map(Map3DCueMotion.Pose.init), arrived: arrived(wasArrived: false)) + } + .onChange(of: nextView) { _, next in + extend(motion.update(to: next.map(Map3DCueMotion.Pose.init), at: .now, reduceMotion: reduceMotion)) + refreshArrival() + } + .onChange(of: projection) { _, _ in refreshArrival() } + .task(id: deadline) { + // Ends the timeline once the last cue change has finished, so a still replay frame + // stops redrawing (FogOverlay's pattern). + let remaining = deadline.timeIntervalSinceNow + guard remaining > 0 else { return } + do { try await Task.sleep(for: .seconds(remaining + 0.05)) } catch { return } + deadline = .distantPast + } + } + + private func extend(_ end: Date?) { + if let end, end > deadline { deadline = end } + } + + // MARK: Arrival + + /// Plan distance at which the homeowner has reached the spot and the cue turns to aiming, + /// and the larger one at which walking off turns it back. The gap stops the cue flickering + /// while someone stands on the edge. A display choice, not tuned on a device. + private static let arriveRadius: Float = 0.6 + private static let leaveRadius: Float = 0.9 + + /// Left alone while there is no suggestion, so a cue fading out keeps its kind; flipping to + /// "not arrived" there would flash the ground disk under a leaving ring. + private func refreshArrival() { + guard nextView != nil else { return } + extend(motion.setArrived(arrived(wasArrived: motion.arrived), at: .now)) + } + + private func arrived(wasArrived: Bool) -> Bool { + guard let nextView else { return false } + let distance = Map3DCueLayout.planDistance(frame.world(nextView.stand), projection.cameraPosition) + return distance < (wasArrived ? Self.leaveRadius : Self.arriveRadius) + } + + // MARK: Accessibility + + /// One element for VoiceOver where the cue is drawn, saying what the geometry can support: + /// how far the spot is and which way, or which way to turn the phone. The frost stays hidden. + @ViewBuilder + private func accessibleCue(in size: CGSize) -> some View { + if let nextView { + let spoken = Map3DCueLayout.spokenCue( + nextView, arrived: motion.arrived, frame: frame, projection: projection, size: size + ) + Color.clear + .frame(width: 60, height: 60) + .position(spoken.point) + .accessibilityElement() + .accessibilityLabel(spoken.label) + .accessibilityAddTraits(.updatesFrequently) + } + } +} + +// MARK: - Frost + +/// The fog cells in view, merged into a few bands of strength so thousands of cubes cost a +/// handful of fills. +/// +/// Each cell becomes a soft disc a little wider than its cube, so neighbours blur into one bank. +/// Discs are grouped by strength into `steps` bands; band k is the union of every cell at step k +/// or above, filled with the alpha that brings the stack to exactly that step. Overlapping cells +/// along a line of sight then show the strongest cell's frost, not a sum that would white out +/// the screen where the unseen space is deep. +private struct Map3DHaze { + var bands: [Path] + + private static let steps = 4 + /// Disc radius as a share of the cube's side: a cube's half-width is 0.5, so 0.7 overlaps + /// neighbours enough for the blur to join them. + private static let discScale: Float = 0.7 + /// Caps a disc right at the phone, where the near fade has it almost invisible anyway. + private static let maxDiscRadius: CGFloat = 180 + /// The homeowner stands inside the region the rules need, so cells round the phone fade + /// out; otherwise the frost would cover the whole screen. A display choice. + private static let nearFade: ClosedRange = 0.5...1.2 + /// LiDAR reads to 5 m (`Map3DConfig.maxDepth`), so fog past that can't be cleared from here; + /// it thins out rather than stacking into a white wall. A display choice. + private static let farFade: ClosedRange = 4...6 + + init(fog: FogOfWar, frame: MapFrame, projection: CameraProjection, size: CGSize) { + var bands = [Path](repeating: Path(), count: Self.steps) + let camera = projection.cameraPosition + let forward = projection.forward + let focal = CGFloat(projection.intrinsics.x) * projection.scale(in: size) + let view = CGRect(origin: .zero, size: size) + for cell in fog.cells { + let world = frame.world(cell.center) + // Distance along the view axis, the same as -z in camera space, without inverting + // the camera transform for every cell; `viewPoint` below inverts it only for the + // few hundred cells that survive this cut. + let depth = simd_dot(world - camera, forward) + guard depth > Self.nearFade.lowerBound, depth < Self.farFade.upperBound else { continue } + let fade = Self.fade(simd_distance(world, camera)) + let step = Int((Double(cell.unknown) * fade * Double(Self.steps)).rounded()) + guard step > 0, let point = projection.viewPoint(for: world, in: size) else { continue } + let radius = min(Self.maxDiscRadius, CGFloat(Self.discScale * fog.cellSize) * focal / CGFloat(depth)) + guard view.insetBy(dx: -radius, dy: -radius).contains(point) else { continue } + let disc = CGRect(x: point.x - radius, y: point.y - radius, width: radius * 2, height: radius * 2) + for band in 0.. Double { + let rise = (distance - nearFade.lowerBound) / (nearFade.upperBound - nearFade.lowerBound) + let fall = (farFade.upperBound - distance) / (farFade.upperBound - farFade.lowerBound) + return Double(min(1, max(0, min(rise, fall)))) + } + + /// Frost at `strength` of FogOverlay's unseen haze: 1 for the material's mask, 0.35 for the + /// white wash over it, as FogOverlay draws them. + func draw(in context: inout GraphicsContext, color: Color, strength: Double) { + let peak = FogOverlay.haze(.unseen) * strength + context.drawLayer { layer in + layer.addFilter(.blur(radius: 9)) + var below = 0.0 + for (index, band) in bands.enumerated() where !band.isEmpty { + let level = peak * Double(index + 1) / Double(Self.steps) + layer.fill(band, with: .color(color.opacity(1 - (1 - level) / (1 - below)))) + below = level + } + } + } +} + +// MARK: - Cue + +/// Where the cue lands on screen this frame: marks drawn on the canvas and chevrons as views. +private struct Map3DCueLayout { + struct Chevron { + var point: CGPoint + var angle: Angle + var opacity: Double + } + + private enum Mark { + case stand(Path, opacity: Double) + case aim(CGPoint, radius: CGFloat, opacity: Double) + } + + private var marks: [Mark] = [] + private(set) var chevrons: [Chevron] = [] + + /// Radius of the ground disk: about the space two feet take, so it reads as "stand here". + private static let standRadius: Float = 0.3 + + init( + drawn: [Map3DCueMotion.Drawn], aimWeight: Double, cellSize: Float, + frame: MapFrame, projection: CameraProjection, size: CGSize + ) { + let bounds = Self.markBounds(size) + for cue in drawn { + let standOpacity = cue.opacity * (1 - aimWeight) + let aimOpacity = cue.opacity * aimWeight + if standOpacity > 0.01 { + let stand = frame.world(cue.pose.stand) + if let center = projection.viewPoint(for: stand, in: size), bounds.contains(center), + let disk = Self.groundDisk(cue.pose.stand, radius: Self.standRadius * cue.scale, frame: frame, projection: projection, size: size) { + marks.append(.stand(disk, opacity: standOpacity)) + } else if let chevron = Self.chevron(toward: stand, projection: projection, size: size, opacity: standOpacity) { + chevrons.append(chevron) + } + } + if aimOpacity > 0.01 { + let target = frame.world(cue.pose.target) + if let center = projection.viewPoint(for: target, in: size), bounds.contains(center) { + marks.append(.aim(center, radius: Self.ringRadius(target, projection: projection, size: size) * CGFloat(cue.scale), opacity: aimOpacity)) + } else if let chevron = Self.chevron(toward: target, projection: projection, size: size, opacity: aimOpacity) { + chevrons.append(chevron) + } + } + } + } + + /// The ground disk and aim ring in WayfindingOverlay's style (white halo, Signal stroke, + /// the ring's dot), each over a faint dark halo: a white halo alone vanishes on sunlit + /// concrete, where Signal on its own is about 3:1. + func drawMarks(in context: inout GraphicsContext) { + for mark in marks { + switch mark { + case .stand(let disk, let opacity): + context.fill(disk, with: .color(Palette.signal.opacity(0.22 * opacity))) + // Dark halo outside the rim only; inside, it muddied the blue fill. + var outside = context + outside.clip(to: disk, options: .inverse) + outside.stroke(disk, with: .color(.black.opacity(0.28 * opacity)), style: StrokeStyle(lineWidth: 10, lineJoin: .round)) + context.stroke(disk, with: .color(.white.opacity(0.9 * opacity)), style: StrokeStyle(lineWidth: 7, lineJoin: .round)) + context.stroke(disk, with: .color(Palette.signal.opacity(opacity)), style: StrokeStyle(lineWidth: 4, lineJoin: .round)) + case .aim(let center, let radius, let opacity): + // TargetMarker's rings are 7 and 4 pt borders inside `radius`. + let ring = { (inset: CGFloat) in + Path(ellipseIn: CGRect(x: center.x - radius, y: center.y - radius, width: radius * 2, height: radius * 2).insetBy(dx: inset, dy: inset)) + } + // The dark halo only shows outside the blue edge; centred on the white band it + // drew a second, gray ring inside it. + context.stroke(ring(-1), with: .color(.black.opacity(0.28 * opacity)), lineWidth: 3) + context.stroke(ring(3.5), with: .color(.white.opacity(0.9 * opacity)), lineWidth: 7) + context.stroke(ring(2), with: .color(Palette.signal.opacity(opacity)), lineWidth: 4) + context.fill(Path(ellipseIn: CGRect(x: center.x - 4, y: center.y - 4, width: 8, height: 8)), with: .color(Palette.signal.opacity(opacity))) + } + } + } + + // MARK: Geometry + + /// A circle on the ground round `stand` (map frame), projected edge by edge so it lies flat + /// and foreshortens with the ground. Nil when any of it is behind the camera. + private static func groundDisk(_ stand: SIMD3, radius: Float, frame: MapFrame, projection: CameraProjection, size: CGSize) -> Path? { + var points: [CGPoint] = [] + for step in 0..<36 { + let angle = Float(step) * 2 * .pi / 36 + let edge = stand + SIMD3(cos(angle) * radius, 0, sin(angle) * radius) + guard let point = projection.viewPoint(for: frame.world(edge), in: size) else { return nil } + points.append(point) + } + var path = Path() + path.addLines(points) + path.closeSubpath() + return path + } + + /// WayfindingOverlay's ring size: 28 cm across the target, kept between 30 and 64 pt. + private static func ringRadius(_ target: SIMD3, projection: CameraProjection, size: CGSize) -> CGFloat { + let depth = simd_dot(target - projection.cameraPosition, projection.forward) + guard depth > 0.05 else { return 64 } + let pointsPerMeter = CGFloat(projection.intrinsics.x) * projection.scale(in: size) / CGFloat(depth) + return min(64, max(30, pointsPerMeter * 0.28)) + } + + /// Where a mark may sit: WayfindingOverlay's ring bounds, clear of the instruction card + /// above and the controls and tape below. Past them the chevron takes over. + private static func markBounds(_ size: CGSize) -> CGRect { + CGRect(x: 36, y: 150, width: size.width - 72, height: size.height - 330) + } + + /// WayfindingOverlay's edge chevron: on the edge of the lane between the card and the + /// controls, pointing toward the world point. + private static func chevron(toward world: SIMD3, projection: CameraProjection, size: CGSize, opacity: Double) -> Chevron? { + guard let direction = projection.screenDirection(toward: world) else { return nil } + let lane = CGRect(x: 40, y: 260, width: size.width - 80, height: max(size.height - 260 - 300, 80)) + let tx = direction.dx == 0 ? CGFloat.infinity : lane.width / 2 / abs(direction.dx) + let ty = direction.dy == 0 ? CGFloat.infinity : lane.height / 2 / abs(direction.dy) + let t = min(tx, ty) + return Chevron( + point: CGPoint(x: lane.midX + direction.dx * t, y: lane.midY + direction.dy * t), + angle: .radians(atan2(direction.dy, direction.dx)), + opacity: opacity + ) + } + + static func planDistance(_ a: SIMD3, _ b: SIMD3) -> Float { + simd_length(SIMD2(a.x - b.x, a.z - b.z)) + } + + // MARK: Spoken + + /// The cue in words and where its element sits. Distance is plan distance to the spot in + /// whole feet; direction is the spot's bearing from where the phone faces, or which way to + /// turn the phone for an aim point off screen. Nothing about what is there: the map knows + /// the space is unseen, not what fills it. + static func spokenCue(_ view: ViewSuggestion, arrived: Bool, frame: MapFrame, projection: CameraProjection, size: CGSize) -> (label: String, point: CGPoint) { + let bounds = markBounds(size) + if arrived { + let target = frame.world(view.target) + if let point = projection.viewPoint(for: target, in: size), bounds.contains(point) { + return ("Aim the phone at the ring, a spot it hasn't seen yet", point) + } + let chevron = chevron(toward: target, projection: projection, size: size, opacity: 1) + let turn = chevron.flatMap { turnWords($0.angle) }.map { "Turn the phone \($0)" } ?? "Look around" + return ("\(turn) to find a spot it hasn't seen yet", chevron?.point ?? CGPoint(x: bounds.midX, y: bounds.midY)) + } + let stand = frame.world(view.stand) + let feet = max(1, Int((planDistance(stand, projection.cameraPosition) / (Distance.metersPerInch * 12)).rounded())) + let distance = "about \(Distance.spoken(Float(feet) * Distance.metersPerInch * 12))" + let place = bearingWords(to: stand, projection: projection).map { "\(distance) \($0)" } ?? "\(distance) away" + let point = projection.viewPoint(for: stand, in: size).flatMap { bounds.contains($0) ? $0 : nil } + ?? chevron(toward: stand, projection: projection, size: size, opacity: 1)?.point + ?? CGPoint(x: bounds.midX, y: bounds.midY) + return ("Next place to stand, \(place)", point) + } + + /// "ahead", "ahead on your left", "to your right", "behind you": the plan bearing of a point + /// from the way the phone faces. Nil when the phone points nearly straight up or down and + /// has no heading to speak of. + private static func bearingWords(to world: SIMD3, projection: CameraProjection) -> String? { + let facing = SIMD2(projection.forward.x, projection.forward.z) + let offset = SIMD2(world.x - projection.cameraPosition.x, world.z - projection.cameraPosition.z) + guard simd_length(facing) > 0.2, simd_length(offset) > 0.05 else { return nil } + let forward = simd_normalize(facing) + // In plan (x, z) with -z ahead, the right of `forward` is (-forward.z, forward.x). + let right = SIMD2(-forward.y, forward.x) + let degrees = atan2(simd_dot(offset, right), simd_dot(offset, forward)) * 180 / .pi + let side = degrees > 0 ? "right" : "left" + switch abs(degrees) { + case ..<25: return "ahead" + case ..<70: return "ahead on your \(side)" + case ..<115: return "to your \(side)" + default: return "behind you" + } + } + + /// "up and to the left" from a chevron's screen angle (0 is right, positive turns down). + private static func turnWords(_ angle: Angle) -> String? { + let dx = cos(angle.radians), dy = sin(angle.radians) + let horizontal = dx > 0.38 ? "to the right" : dx < -0.38 ? "to the left" : nil + let vertical = dy < -0.38 ? "up" : dy > 0.38 ? "down" : nil + switch (vertical, horizontal) { + case let (vertical?, horizontal?): return "\(vertical) and \(horizontal)" + case let (vertical?, nil): return vertical + case let (nil, horizontal?): return horizontal + case (nil, nil): return nil + } + } +} + +/// WayfindingOverlay's off-screen chevron: white on a Signal disc with a white rim and a shadow. +private struct Map3DChevron: View { + var angle: Angle + + var body: some View { + Image(systemName: "chevron.right") + .font(.system(size: 22, weight: .black)) + .foregroundStyle(.white) + .rotationEffect(angle) + .frame(width: 52, height: 52) + .background(Palette.signal, in: .circle) + .overlay(Circle().strokeBorder(.white.opacity(0.85), lineWidth: 2)) + .shadow(color: .black.opacity(0.3), radius: 6, y: 2) + } +} + +// MARK: - Motion + +/// The cue's motion, kept in the map frame so an animating cue still sits on the world. +/// +/// Three changes animate, each under 250 ms with an ease-out: a cue appearing (fade, and a small +/// grow from 90% without Reduce Motion), a cue leaving (fade), and the suggestion moving (a glide +/// from where it was drawn; with Reduce Motion a crossfade when it moved far, a jump when it +/// barely moved). Arriving at the spot crossfades the ground disk into the aim ring. Nothing +/// pulses or drifts on its own. +/// +/// Observable so that seeding it on appear, or arriving, redraws a still replay frame; with a +/// plain class the cue stayed blank until the pose next changed. +@MainActor @Observable +private final class Map3DCueMotion { + struct Pose: Equatable { + var stand: SIMD3 + var target: SIMD3 + + init(stand: SIMD3, target: SIMD3) { + self.stand = stand + self.target = target + } + + init(_ view: ViewSuggestion) { + self.init(stand: view.stand, target: view.target) + } + + func mixed(to other: Pose, _ t: Float) -> Pose { + Pose(stand: simd_mix(stand, other.stand, SIMD3(repeating: t)), target: simd_mix(target, other.target, SIMD3(repeating: t))) + } + } + + struct Drawn { + var pose: Pose + var opacity: Double + /// Size of the disk and ring against their resting size. + var scale: Float + } + + private enum Change { + case appear + case leave(Pose) + case glide(from: Pose) + case crossfade(from: Pose) + + var duration: TimeInterval { + switch self { + case .appear: 0.2 + case .leave: 0.15 + case .glide: 0.25 + case .crossfade: 0.2 + } + } + } + + /// Under Reduce Motion a move shorter than this jumps rather than crossfading: the target + /// drifts a little each time the map fills in, and a crossfade per drift would flicker. + private static let crossfadeDistance: Float = 0.4 + private static let arrivalDuration: TimeInterval = 0.2 + + private var pose: Pose? + private var change: Change? + private var changeStart = Date.distantPast + private(set) var arrived = false + private var arrivalStart = Date.distantPast + private var arrivalFrom = 0.0 + + func seed(_ pose: Pose?, arrived: Bool) { + self.pose = pose + self.arrived = arrived + change = nil + arrivalFrom = arrived ? 1 : 0 + } + + /// Starts the change to `next`. Returns when it ends, or nil when nothing animates. + func update(to next: Pose?, at now: Date, reduceMotion: Bool) -> Date? { + guard next != pose else { return nil } + let shown = shownPose(at: now) + switch (shown, next) { + case (nil, _?): + change = .appear + case (let old?, nil): + change = .leave(old) + case (let old?, let new?): + let moved = max(simd_distance(old.stand, new.stand), simd_distance(old.target, new.target)) + change = !reduceMotion ? .glide(from: old) : moved >= Self.crossfadeDistance ? .crossfade(from: old) : nil + case (nil, nil): + change = nil + } + pose = next + changeStart = now + return change.map { now.addingTimeInterval($0.duration) } + } + + /// Switches between the ground disk and the aim ring. Returns when the crossfade ends, or + /// nil when nothing changed. + func setArrived(_ value: Bool, at now: Date) -> Date? { + guard value != arrived else { return nil } + arrivalFrom = aimWeight(at: now) + arrived = value + arrivalStart = now + return now.addingTimeInterval(Self.arrivalDuration) + } + + /// 0 draws the ground disk, 1 the aim ring; in between both, crossfading. + func aimWeight(at now: Date) -> Double { + let eased = Self.easeOut(now.timeIntervalSince(arrivalStart) / Self.arrivalDuration) + return arrivalFrom + ((arrived ? 1 : 0) - arrivalFrom) * eased + } + + func drawn(at now: Date, reduceMotion: Bool) -> [Drawn] { + guard let change else { return pose.map { [Drawn(pose: $0, opacity: 1, scale: 1)] } ?? [] } + let eased = Self.easeOut(now.timeIntervalSince(changeStart) / change.duration) + switch change { + case .appear: + return pose.map { [Drawn(pose: $0, opacity: eased, scale: reduceMotion ? 1 : 0.9 + 0.1 * Float(eased))] } ?? [] + case .leave(let old): + return eased < 1 ? [Drawn(pose: old, opacity: 1 - eased, scale: 1)] : [] + case .glide(let from): + return pose.map { [Drawn(pose: from.mixed(to: $0, Float(eased)), opacity: 1, scale: 1)] } ?? [] + case .crossfade(let from): + return [Drawn(pose: from, opacity: 1 - eased, scale: 1)] + (pose.map { [Drawn(pose: $0, opacity: eased, scale: 1)] } ?? []) + } + } + + /// Where the cue is drawn now, so a change that interrupts a glide starts from there. + private func shownPose(at now: Date) -> Pose? { + guard case .glide(let from) = change, let pose else { return pose } + return from.mixed(to: pose, Float(Self.easeOut(now.timeIntervalSince(changeStart) / 0.25))) + } + + /// FogOverlay's cubic ease-out, clamped to 0...1. + private static func easeOut(_ t: Double) -> Double { + let clamped = min(1, max(0, t)) + return 1 - pow(1 - clamped, 3) + } +} + +// MARK: - Preview + +#if DEBUG +/// A synthetic yard for the preview: a flat wall, flat ground and a round bush, walked along the +/// left half only, run through the real map so the fog and next view are what the app would get. +enum Map3DOverlayDemo { + struct Scene { + var fog: FogOfWar + var nextView: ViewSuggestion? + var frame: MapFrame + /// Standing back from the wall, facing right of the meter. + var approach: CameraProjection + /// At the suggested spot, aimed where it says. + var arrived: CameraProjection? + } + + static let intrinsics = SIMD4(1450, 1450, 960, 720) + static let imageSize = SIMD2(1920, 1440) + static let groundY: Float = -1.1 + static let bush = (center: SIMD3(1.3, -0.65, 0.6), radius: Float(0.5)) + + static func make() -> Scene { + // The meter frame is turned and shifted from ARKit's world so the preview exercises + // `frame.world` and not an identity. + let meter = SIMD3(0.4, 1.1, -0.3) + let outward = simd_normalize(SIMD3(0.35, 0, 1)) + let frame = MapFrame(meter: meter, outward: outward, worldGroundY: 0)! + let wall = WallFrame(meter: meter, outward: outward, groundY: 0)! + var map = Map3D(frame: frame) + for x in Swift.stride(from: Float(-4), through: 0.2, by: 0.4) { + for aim in [SIMD3(x, 0, 0), SIMD3(x, groundY, 1.2), SIMD3(x, 1.2, 0)] { + map.integrate(depthFrame(eye: SIMD3(x, groundY + 1.4, 3), aim: aim, frame: frame)) + } + } + let next = map.nextBestView(along: wall) + let arrived = next.map { camera(eye: $0.eye, aim: $0.target, frame: frame) } + return Scene( + fog: map.fogOfWar(along: wall), nextView: next, frame: frame, + approach: camera(eye: SIMD3(-1.2, groundY + 1.4, 3.2), aim: SIMD3(1.2, groundY + 0.5, 0.4), frame: frame), + arrived: arrived + ) + } + + /// A portrait-held phone at `eye` looking at `aim`, both map frame. Camera +x (sensor right) + /// is screen down and +y (sensor up) is screen right. + static func camera(eye: SIMD3, aim: SIMD3, frame: MapFrame) -> CameraProjection { + let forward = simd_normalize(aim - eye) + let right = simd_normalize(simd_cross(forward, SIMD3(0, 1, 0))) + let down = simd_cross(forward, right) + let inMap = simd_float4x4(SIMD4(down, 0), SIMD4(right, 0), SIMD4(-forward, 0), SIMD4(eye, 1)) + return CameraProjection(cameraToWorld: frame.poseInWorld * inMap, intrinsics: intrinsics, imageSize: imageSize) + } + + /// LiDAR-like depth of the wall (map z = 0), ground and bush, ray cast in the map frame. + private static func depthFrame(eye: SIMD3, aim: SIMD3, frame: MapFrame) -> DepthFrame { + let view = camera(eye: eye, aim: aim, frame: frame) + let photo = CameraFrame(cameraToWorld: view.cameraToWorld, intrinsics: intrinsics, imageSize: imageSize) + let inMap = frame.poseInWorld.inverse * view.cameraToWorld + let width = 64, height = 48 + let k = intrinsics * SIMD4(Float(width) / imageSize.x, Float(height) / imageSize.y, Float(width) / imageSize.x, Float(height) / imageSize.y) + var depth = [Float](repeating: 0, count: width * height) + for v in 0.. 0 { let hit = (-b - disc.squareRoot()) / a; if hit > 0 { t = min(t, hit) } } + // With the ray's camera z at -1, the hit's ray parameter is its depth. + if t < 5 { depth[v * width + u] = t } + } + } + return DepthFrame(photo: photo, width: width, height: height, depth: depth, kind: .lidar(confidence: nil)) + } +} + +#Preview("Walking to the next spot") { + let scene = Map3DOverlayDemo.make() + ZStack { + Color(red: 0.62, green: 0.60, blue: 0.56) + Map3DOverlay(fog: scene.fog, nextView: scene.nextView, frame: scene.frame, projection: scene.approach) + } +} + +#Preview("At the spot, aiming") { + let scene = Map3DOverlayDemo.make() + ZStack { + Color(red: 0.62, green: 0.60, blue: 0.56) + Map3DOverlay(fog: scene.fog, nextView: scene.nextView, frame: scene.frame, projection: scene.arrived ?? scene.approach) + } +} +#endif diff --git a/ios/HouseScan/UI/Screens/ResultARScreen.swift b/ios/HouseScan/UI/Screens/ResultARScreen.swift index 871da39cf..f6d566ed9 100644 --- a/ios/HouseScan/UI/Screens/ResultARScreen.swift +++ b/ios/HouseScan/UI/Screens/ResultARScreen.swift @@ -14,7 +14,7 @@ struct ResultARScreen: View { var body: some View { ZStack { - if !state.resultInCamera, state.tracking == .normal, let projection = state.projection, let wall = state.wall, let result = state.result { + if !state.resultInCamera, state.tracking == .normal, let projection = state.projection, let result = state.result, let wall = result.wall ?? state.wall { BatteryOverlay(projection: projection, wall: wall, result: result, rise: appeared ? 1 : 0) .ignoresSafeArea() .accessibilityHidden(true) diff --git a/ios/HouseScan/UI/Screens/ResultScreen.swift b/ios/HouseScan/UI/Screens/ResultScreen.swift index f47b7ccad..946391a48 100644 --- a/ios/HouseScan/UI/Screens/ResultScreen.swift +++ b/ios/HouseScan/UI/Screens/ResultScreen.swift @@ -95,7 +95,7 @@ struct ResultScreen: View { @ViewBuilder private func diorama(_ result: ResultPresentation) -> some View { ZStack(alignment: .topLeading) { - if let wall = state.wall { + if let wall = result.wall ?? state.wall { ResultScene3D(wall: wall, result: result, features: state.features, wallHeight: max(state.coverage.wallBandHeight, 2.4)) } else { Palette.canvas diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Coverage/CoverageMap.swift b/ios/HouseScanKit/Sources/HouseScanKit/Coverage/CoverageMap.swift index 018e05855..fe586d807 100644 --- a/ios/HouseScanKit/Sources/HouseScanKit/Coverage/CoverageMap.swift +++ b/ios/HouseScanKit/Sources/HouseScanKit/Coverage/CoverageMap.swift @@ -164,6 +164,10 @@ public struct CoverageMap: Sendable { public private(set) var limitEnds: Set = [] /// Increments on every change. public private(set) var revision = 0 + /// Under the 3D map, the cells it saw per band (`setMeasuredCovered`): a cell between the ends + /// is covered exactly when it is in here, whatever the camera sightings say. Seen, hidden and + /// skipped still come from the sightings. Nil, the default, leaves covering to the sightings. + public private(set) var measuredCovered: [SurfaceBand: Set]? /// How far the ground under the wall may lie from `wall.groundY` either way, meters, while it /// is a guess: 0 once the ground is measured; the engine sets its chest-height guess's error /// until then. Guessed too low, a row sampled at height h stands only h less the error above @@ -314,7 +318,42 @@ public struct CoverageMap: Sendable { } public func level(_ band: SurfaceBand, _ index: Int) -> CoverageLevel { - cells[band]?[index]?.level ?? .unseen + guard let measuredCovered else { return cells[band]?[index]?.level ?? .unseen } + if measuredCovered[band]?.contains(index) == true, allows(index) { return .covered } + guard var cell = cells[band]?[index] else { return .unseen } + cell.covered = false + return cell.level + } + + /// Makes the 3D map's seen cells the covered ones (`measuredCovered`), so the strip and the + /// planners ask for what the export, which reads the 3D map, will send. + public mutating func setMeasuredCovered(_ covered: [SurfaceBand: Set]) { + guard covered != measuredCovered else { return } + measuredCovered = covered + revision += 1 + } + + /// The cells the 3D map saw, for `setMeasuredCovered`: a wall cell whose whole s range lies in + /// a stretch seen up to the height the walk asks for (`Map3DCoverage.wallHeight` at least + /// `wallWalkHeight`, as a camera-covered cell needs), and a ground cell whose whole range + /// lies in a ground stretch seen out to at least `groundBandDepth`. `coverage` must be read + /// along `wall`. + public func cells(seenIn coverage: Map3DCoverage) -> [SurfaceBand: Set] { + // Spans are built from the same cell edges, so this only absorbs Float rounding. + let tolerance = config.cellWidth * 1e-3 + func cells(within spans: [ClosedRange]) -> Set { + var result: Set = [] + for span in spans { + for index in indices(overlapping: span) { + let cell = cellRange(index) + if cell.lowerBound >= span.lowerBound - tolerance, cell.upperBound <= span.upperBound + tolerance { result.insert(index) } + } + } + return result + } + let highWall = coverage.wallHeight.filter { $0.out + 1e-4 >= config.wallWalkHeight }.map(\.span) + let deepGround = coverage.ground.filter { $0.out + 1e-4 >= config.groundBandDepth }.map(\.span) + return [.wall: cells(within: highWall), .ground: cells(within: deepGround)] } /// Indices of every cell overlapping `range` by more than a thousandth of a cell. @@ -1081,8 +1120,8 @@ public struct CoverageMap: Sendable { /// Marks the not-yet-covered cells of `range` as skipped ("I can't get there"). public mutating func markSkipped(_ band: SurfaceBand, _ range: ClosedRange) { for index in indices(overlapping: range) where allows(index) { + guard level(band, index) != .covered else { continue } var cell = cells[band]?[index] ?? newCell(band) - guard !cell.covered else { continue } cell.skipped = true cells[band, default: [:]][index] = cell } @@ -1194,6 +1233,7 @@ public struct CoverageMap: Sendable { /// s extent of cells seen, covered or found hidden, or nil when none are. public var seenExtent: ClosedRange? { let seen = cells.values.flatMap { $0.filter { $0.value.isSeen || !$0.value.hiddenRows.isEmpty }.keys } + + (measuredCovered?.values.flatMap { $0 } ?? []) guard let low = seen.min(), let high = seen.max() else { return nil } return cellRange(low).lowerBound...cellRange(high).upperBound } @@ -1212,7 +1252,8 @@ public struct CoverageMap: Sendable { /// Covered stretches of a band, merged, in meters of s. Only covered cells count: a cell seen /// from one position has no parallax behind it. public func coveredIntervals(_ band: SurfaceBand) -> [ClosedRange] { - let indices = (cells[band] ?? [:]).filter { $0.value.covered && allows($0.key) }.keys.sorted() + let covered = measuredCovered.map { Array($0[band] ?? []) } ?? (cells[band] ?? [:]).filter { $0.value.covered }.map(\.key) + let indices = covered.filter(allows).sorted() var runs: [ClosedRange] = [] for index in indices { let range = cellRange(index) @@ -1236,7 +1277,8 @@ public struct CoverageMap: Sendable { /// Total covered cells over both bands. public var coveredCount: Int { - cells.values.reduce(0) { $0 + $1.values.filter(\.covered).count } + if let measuredCovered { return measuredCovered.values.reduce(0) { $0 + $1.count } } + return cells.values.reduce(0) { $0 + $1.values.filter(\.covered).count } } } diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Geometry/WallFrame.swift b/ios/HouseScanKit/Sources/HouseScanKit/Geometry/WallFrame.swift index edb22b5cb..aef9b7b2a 100644 --- a/ios/HouseScanKit/Sources/HouseScanKit/Geometry/WallFrame.swift +++ b/ios/HouseScanKit/Sources/HouseScanKit/Geometry/WallFrame.swift @@ -230,6 +230,25 @@ public struct WallFrame: Sendable, Equatable { return WallPoint(s: min(max(local.s, piece.span.lowerBound), piece.span.upperBound), height: d.y, out: local.out) } + /// This wall moved as the walk's wall moved from `old` to `new`: by the meter's move, the + /// meter anchor's refinement, and the ground's change. A wall equal to `old` (scene.json + /// described the walk's own) is `new`. Assumes the walk's direction is unchanged, as it is + /// after the meter is placed. + public func following(_ old: WallFrame, to new: WallFrame) -> WallFrame { + guard self != old else { return new } + var moved = self + moved.meter += new.meter - old.meter + moved.groundY += new.groundY - old.groundY + return moved + } + + /// The s on `other` of the place at `s` on this wall: its point on the ground, projected onto + /// the nearest piece of `other` (`wallPoint`). Two walls built from the same scan (the walk's + /// and the one scene.json described) agree on it within the distance between their lines. + public func s(_ s: Float, along other: WallFrame) -> Float { + other == self ? s : other.wallPoint(world(s: s, height: 0)).s + } + /// How far a point is in front of the line of the piece holding `s`, meters. func out(of world: SIMD3, pieceAtS s: Float) -> Float { segment(atS: s).coordinates(ofOffset: world - origin).out diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Guidance/GapPlanner.swift b/ios/HouseScanKit/Sources/HouseScanKit/Guidance/GapPlanner.swift index d875b944d..1bcfdeae4 100644 --- a/ios/HouseScanKit/Sources/HouseScanKit/Guidance/GapPlanner.swift +++ b/ios/HouseScanKit/Sources/HouseScanKit/Guidance/GapPlanner.swift @@ -323,3 +323,21 @@ extension GapPlanner { side == .left ? min(plan.span.lowerBound, old) : max(plan.span.upperBound, old) } } + +extension PlacementMissingEvidence { + /// This request with its `span_ft` carried from the wall scene.json described (`exported`) to + /// the walk's (`walk`), where the gap planner and the coverage map measure s. Under + /// `-coverage map3d` the scene can describe the measured wall chain, whose s differs from the + /// walk's, most past a corner only one of them has. + public func along(_ walk: WallFrame, from exported: WallFrame) -> PlacementMissingEvidence { + guard walk != exported, let span = spanFt else { return self } + let feetPerMeter = SceneUnits.feetPerMeter + func carried(_ feet: Double) -> Double { + SceneExport.round4(Double(exported.s(Float(feet / feetPerMeter), along: walk)) * feetPerMeter) + } + var mapped = self + let ends = [carried(span.x), carried(span.y)] + mapped.spanFt = SIMD2(ends.min() ?? 0, ends.max() ?? 0) + return mapped + } +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Map3D/CarriedEvidence.swift b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/CarriedEvidence.swift new file mode 100644 index 000000000..755d6d5db --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/CarriedEvidence.swift @@ -0,0 +1,56 @@ +import Foundation +import simd + +extension ObservedSpan { + /// Clear-space claims measured along one wall (`from`, the walk's tapped wall), restated + /// along another (`to`, the measured chain scene.json describes) so that each restated + /// claim covers only ground the original covered. + /// + /// A claim is the rectangle in front of the wall over its span: out to its own `out` when + /// `depth` is nil (walked facing), or out to `depth` with `out` a height (confirmed overhead + /// over the battery's depth). Only the meter's piece of each wall is used: past a corner of + /// either, one straight stretch of the other can face two ways, so what lies past one is + /// dropped. On the meter's pieces the span's ends are carried through their places on the + /// ground; the reach loses the most the new line stands in front of the old over the span; + /// both ends come in by the reach times the sine of the angle between the lines; and the + /// restated rectangle's corners are checked against the original. Walked facing is the gap + /// from the wall to whatever faces it, so only its far edge must stay within the walked + /// reach; its near edge is the new wall's face, which may stand behind the old line. An + /// overhead claim vouches for the space over the battery's depth, so its whole rectangle + /// must lie inside the original. A claim that fails the check, or has nothing left, is + /// dropped. + public static func carried(_ spans: [ObservedSpan], depth: Float?, from tapped: WallFrame, to measured: WallFrame) -> [ObservedSpan] { + guard tapped != measured else { return spans } + let tappedPiece = tapped.segments[tapped.meterSegmentIndex] + let measuredPiece = measured.segments[measured.meterSegmentIndex] + let sine = abs(simd_cross(tappedPiece.outward, measuredPiece.outward).y) + // Where a point in front of the measured wall lies in front of the tapped one. + func onTapped(_ s: Float, _ out: Float) -> (s: Float, out: Float) { + let c = tappedPiece.coordinates(ofOffset: measured.world(s: s, height: 0, out: out) - tapped.origin) + return (c.s, c.out) + } + let tolerance: Float = 1e-4 + return spans.compactMap { item in + let a = max(item.span.lowerBound, tappedPiece.span.lowerBound) + let b = min(item.span.upperBound, tappedPiece.span.upperBound) + guard a < b else { return nil } + let ends = [tapped.s(a, along: measured), tapped.s(b, along: measured)] + var low = max(ends.min() ?? 0, measuredPiece.span.lowerBound) + var high = min(ends.max() ?? 0, measuredPiece.span.upperBound) + guard low < high else { return nil } + let original = depth ?? item.out + let standsOut = max(0, onTapped(low, 0).out, onTapped(high, 0).out) + let reach = depth ?? (item.out - standsOut) + guard reach > 0 else { return nil } + low += reach * sine + high -= reach * sine + guard low < high else { return nil } + for (s, out) in [(low, Float(0)), (low, reach), (high, 0), (high, reach)] { + let p = onTapped(s, out) + guard p.s >= a - tolerance, p.s <= b + tolerance, p.out <= original + tolerance else { return nil } + if depth != nil, p.out < -tolerance { return nil } + } + return ObservedSpan(span: low...high, out: depth == nil ? reach : item.out) + } + } +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Map3D/ChosenSpot.swift b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/ChosenSpot.swift new file mode 100644 index 000000000..c2e112c23 --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/ChosenSpot.swift @@ -0,0 +1,94 @@ +import Foundation +import simd + +/// What the 3D map says about the battery's volume at a chosen stretch of wall. +public enum SpotView: Sendable, Equatable { + /// Measured depth found something standing in the volume. The stretch counts as unseen and a + /// better view is needed; the homeowner's answer can't change that. + case occluder + /// Measured rays crossed the whole volume, the ground under it and the facade behind it + /// were seen: nothing stands there. + case clear + /// Neither: too little measured evidence. The homeowner's answer decides, as on a phone + /// without LiDAR. + case unknown +} + +/// How a chosen spot's check comes out, from the map's view and the homeowner's answer. +public enum SpotDecision: Sendable, Equatable { + case clear + case obstacle + /// Ask for another view of the stretch. + case needsAnotherView + /// Ask the homeowner whether the spot is clear. + case needsConfirmation + + /// The homeowner's answer, when there is one, is `true` for clear and `false` for an + /// obstacle. An occluder the map measured always needs another view, whatever the answer: + /// the answer can't clear what depth found standing there. The map's clear needs no answer; + /// an obstacle the homeowner reports still stands. + public static func decide(_ view: SpotView, homeownerSaysClear: Bool?) -> SpotDecision { + switch view { + case .occluder: .needsAnotherView + case .clear: homeownerSaysClear == false ? .obstacle : .clear + case .unknown: + switch homeownerSaysClear { + case true?: .clear + case false?: .obstacle + case nil: .needsConfirmation + } + } + } +} + +extension Map3D { + /// What the map shows in the battery's volume over `span` (meters of s along `wall`): from + /// the facade's face out to `overheadDepth` (the battery's depth), from `groundClearance` + /// above each column's ground up to headroom. Only measured evidence decides, LiDAR or + /// feature rays; estimated depth, detected planes and the mesh never do. + /// + /// - `occluder`: a measured surface hit in at least `minOccluderHits` frames stands in the + /// volume. Two frames, so one stray depth pixel can't block a spot; a voxel hit once is + /// neither clear nor an occluder. + /// - `clear`: the facade is seen to headroom over the span (`isWallSeen`), the ground is + /// seen in every column (`groundHeight`), and every voxel of the volume is free by a + /// measured ray. + /// - `unknown`: anything else. + /// + /// The span is read along `wall` through the map's current frame, so the answer follows a + /// refined meter anchor or a changed wall the same way coverage does. + public func spotView(span: ClosedRange, along wall: WallFrame) -> SpotView { + var clear = true + let cells = cellIndices.filter { cellRange($0).upperBound > span.lowerBound && cellRange($0).lowerBound < span.upperBound } + guard !cells.isEmpty else { return .unknown } + for index in cells { + let range = cellRange(index) + let facade = facadeOffset(cell: index, along: wall) + if facade == nil || !isWallSeen(cell: index, along: wall) { clear = false } + let near = nearStart(facade ?? 0) + let ss = samples(in: max(range.lowerBound, span.lowerBound)...min(range.upperBound, span.upperBound)) + for s in ss { + for out in stride(from: near, through: config.overheadDepth, by: config.voxelSize / 2) { + let ground = groundHeight(wall, s: s, out: out) + if ground == nil { clear = false } + for height in stride(from: (ground ?? 0) + config.groundClearance, through: config.headroom, by: config.voxelSize / 2) { + guard let g = coordinate(wall, s: s, height: height, out: out) else { + clear = false + continue + } + if let voxel = grid.voxel(g), voxel.state(config) == .surface, Int(voxel.hits) >= Self.minOccluderHits, + !VoxelSources(rawValue: voxel.sources).isDisjoint(with: VoxelSources.measuredHit) { + return .occluder + } + if !grid.isMeasuredFree(g, config: config) { clear = false } + } + } + } + } + return clear ? .clear : .unknown + } + + /// Frames a measured surface must be hit in to count as an occluder at a chosen spot. A + /// guess, not tuned: one frame is a stray pixel as often as an object. + static let minOccluderHits = 2 +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Map3D/FogOfWar.swift b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/FogOfWar.swift new file mode 100644 index 000000000..ba9359655 --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/FogOfWar.swift @@ -0,0 +1,252 @@ +import Foundation +import simd + +/// The part of the region of interest nobody has seen yet, as boxes the UI can draw over the +/// camera feed. Everything is in the map frame (`Map3D.frame` converts to ARKit world), so the +/// boxes can hang off the meter's anchor. +public struct FogOfWar: Sendable, Equatable { + public struct Cell: Sendable, Equatable { + /// Center of a cube `FogOfWar.cellSize` on a side, map frame. + public var center: SIMD3 + /// Share of the cell's region-of-interest voxels still unknown, 0 to 1. + public var unknown: Float + } + + public var cellSize: Float + /// Cells with any unknown voxel of the region of interest. + public var cells: [Cell] + /// Share of the region of interest seen, 0 to 1: free or surface voxels over all of it. + public var seen: Float +} + +/// Where to stand and look next: the largest unseen part of the region of interest that a view +/// can still reveal, and a spot from which it is in sight. +public struct ViewSuggestion: Sendable, Equatable { + /// Center of the unseen region's boundary with seen free space, map frame: what to aim at. + public var target: SIMD3 + /// Unknown voxels in the region. + public var voxels: Int + /// Box around the region, map frame. + public var regionMin: SIMD3 + public var regionMax: SIMD3 + /// Where to stand, on the ground at the meter's height (`MapFrame.groundY`), map frame. + public var stand: SIMD3 + /// Camera position and unit aim direction from there, map frame. + public var eye: SIMD3 + public var aim: SIMD3 + /// Share of the region's boundary samples in unobstructed sight from `eye`, 0 to 1. Zero + /// when no candidate spot sees any of it; `stand` is then straight out from the target. + public var inSight: Float +} + +extension Map3D { + /// Fog over the region of interest along `wall` (normally the walk's chain, or + /// `MeasuredWallChain.wallFrame`): within `alongExtent` of the meter along the chain, from + /// `faceBehind` behind it out to `outDepth`, not behind any other piece of the chain, from + /// the ground to headroom, and to `top` over the battery's depth. + public func fogOfWar(along wall: WallFrame) -> FogOfWar { + let region = RegionOfInterest(self, wall: wall) + let factor = max(1, Int32((config.fogCellSize / config.voxelSize).rounded())) + var counts: [SIMD3: (unknown: Int, total: Int)] = [:] + var unknown = 0 + region.forEachVoxel { g, voxel in + let isUnknown = voxel?.state(config) ?? .unknown == .unknown + let key = g / factor + var count = counts[key] ?? (0, 0) + count.total += 1 + if isUnknown { + count.unknown += 1 + unknown += 1 + } + counts[key] = count + } + let size = config.voxelSize * Float(factor) + let cells = counts.compactMap { key, count -> FogOfWar.Cell? in + guard count.unknown > 0 else { return nil } + let center = grid.origin + (SIMD3(key) + 0.5) * size + return FogOfWar.Cell(center: center, unknown: Float(count.unknown) / Float(count.total)) + } + .sorted { ($0.center.x, $0.center.y, $0.center.z) < ($1.center.x, $1.center.y, $1.center.z) } + let seen = region.count > 0 ? 1 - Float(unknown) / Float(region.count) : 0 + return FogOfWar(cellSize: size, cells: cells, seen: seen) + } + + /// The largest unseen part of the region of interest along `wall` that borders seen free + /// space, and where to stand to see it. Unseen voxels shut in by surfaces (the inside of a + /// bush) are left out: no view can reveal them. Nil when nothing unseen borders free space. + /// + /// Candidate spots stand `viewDistances` from the target at every 30 degrees around it, in + /// front of the chain and not inside anything; the camera is at `eyeHeight` aiming at the + /// target. The spot seeing the most of the region's boundary past known surfaces wins, + /// nearer spots first on a tie. + public func nextBestView(along wall: WallFrame) -> ViewSuggestion? { + precondition(!config.viewDistances.isEmpty, "Map3DConfig.viewDistances is empty") + let region = RegionOfInterest(self, wall: wall) + // 1: unknown voxel of the region, 2: visited. + var marks = [UInt8](repeating: 0, count: Int(grid.dims.x) * Int(grid.dims.y) * Int(grid.dims.z)) + func flat(_ g: SIMD3) -> Int { Int(g.x) + Int(grid.dims.x) * (Int(g.y) + Int(grid.dims.y) * Int(g.z)) } + var seeds: [SIMD3] = [] + region.forEachVoxel { g, voxel in + guard voxel?.state(config) ?? .unknown == .unknown else { return } + marks[flat(g)] = 1 + seeds.append(g) + } + let faces: [SIMD3] = [SIMD3(1, 0, 0), SIMD3(-1, 0, 0), SIMD3(0, 1, 0), SIMD3(0, -1, 0), SIMD3(0, 0, 1), SIMD3(0, 0, -1)] + func inGrid(_ g: SIMD3) -> Bool { all(g .>= 0) && all(g .< grid.dims) } + func bordersFree(_ g: SIMD3) -> Bool { + faces.contains { f in + let n = g &+ f + return inGrid(n) && grid.voxel(n)?.state(config) == .free + } + } + var best: (voxels: [SIMD3], frontier: [SIMD3])? + for seed in seeds where marks[flat(seed)] == 1 { + marks[flat(seed)] = 2 + var queue = [seed] + var head = 0 + var frontier: [SIMD3] = [] + while head < queue.count { + let g = queue[head] + head += 1 + if bordersFree(g) { frontier.append(g) } + for dz: Int32 in -1...1 { + for dy: Int32 in -1...1 { + for dx: Int32 in -1...1 { + let n = g &+ SIMD3(dx, dy, dz) + guard inGrid(n), marks[flat(n)] == 1 else { continue } + marks[flat(n)] = 2 + queue.append(n) + } + } + } + } + if !frontier.isEmpty, queue.count > best?.voxels.count ?? 0 { best = (queue, frontier) } + } + guard let region = best else { return nil } + let frontier = region.frontier.map(grid.center) + let target = frontier.reduce(.zero, +) / Float(frontier.count) + let centers = region.voxels.map(grid.center) + let lower = centers.reduce(SIMD3(repeating: .infinity), simd_min) - config.voxelSize / 2 + let upper = centers.reduce(SIMD3(repeating: -.infinity), simd_max) + config.voxelSize / 2 + // Up to 64 boundary voxels, spread through the list, stand in for the region. + let step = max(1, frontier.count / 64) + let probes = Swift.stride(from: 0, to: frontier.count, by: step).map { frontier[$0] } + + var chosen: (stand: SIMD3, eye: SIMD3, seen: Int)? + for distance in config.viewDistances { + for step in 0..<12 { + let angle = Float(step) * .pi / 6 + let stand = SIMD3(target.x + cos(angle) * distance, frame.groundY, target.z + sin(angle) * distance) + guard canStand(at: stand, wall: wall) else { continue } + let eye = stand + SIMD3(0, config.eyeHeight, 0) + let seen = probes.count { inSight($0, from: eye, aim: simd_normalize(target - eye)) } + if seen > chosen?.seen ?? 0 { chosen = (stand, eye, seen) } + } + } + let fallback: SIMD3 = { + let point = wall.wallPoint(frame.world(target)) + let world = wall.world(s: point.s, height: 0, out: max(point.out, 0) + config.viewDistances[0]) + return frame.map(SIMD3(world.x, wall.groundY, world.z)) + }() + let stand = chosen?.stand ?? SIMD3(fallback.x, frame.groundY, fallback.z) + let eye = chosen?.eye ?? stand + SIMD3(0, config.eyeHeight, 0) + return ViewSuggestion( + target: target, voxels: region.voxels.count, regionMin: lower, regionMax: upper, stand: stand, eye: eye, + aim: simd_normalize(target - eye), inSight: Float(chosen?.seen ?? 0) / Float(probes.count)) + } + + /// Whether someone can stand at a map point: inside the bounds, in front of the chain by more + /// than a battery's depth and not + /// where the map holds a surface between `groundClearance` and just above eye height. + private func canStand(at point: SIMD3, wall: WallFrame) -> Bool { + guard bounds.contains(point + SIMD3(0, config.eyeHeight, 0)), RegionOfInterest.isInFront(self, wall: wall, map: point, minOut: config.overheadDepth) else { return false } + for height in Swift.stride(from: config.groundClearance, through: config.eyeHeight + 0.3, by: config.voxelSize / 2) where state(at: point + SIMD3(0, height, 0)) == .surface { + return false + } + return true + } + + /// Whether a voxel center is within a phone's view from `eye` aimed along `aim`, and no + /// surface lies between them. A camera sees about 30 degrees either side of its aim across + /// the narrower side of the image (ARKit's wide camera held in portrait: 53 degrees across), + /// less a margin, so 25 is used. + private func inSight(_ point: SIMD3, from eye: SIMD3, aim: SIMD3) -> Bool { + let offset = point - eye + let distance = simd_length(offset) + guard distance > 0, distance <= config.maxViewDistance else { return false } + let direction = offset / distance + guard simd_dot(direction, aim) >= cos(25 * Float.pi / 180) else { return false } + return firstSurface(from: eye, direction: direction, length: distance - config.voxelSize) == nil + } +} + +/// The voxels of the map the rules need seen, along one wall chain: from the ground to +/// headroom, and on up to `top` over the battery's depth (`overheadDepth`), where headroom is +/// judged. Higher up farther out, only open sky would be left to see, and sky returns no depth. +struct RegionOfInterest { + let map: Map3D + let wall: WallFrame + /// Per plan column of the grid (x + dims.x * z), the highest layer in the region, or -1 + /// when the column is not in it. + let columns: [Int32] + let low: Int32 + let count: Int + + init(_ map: Map3D, wall: WallFrame) { + self.map = map + self.wall = wall + let grid = map.grid + let config = map.config + // From the layer holding the ground to the layer holding headroom, or the top: the + // voxels coverage reads (`Map3DCoverage`). + let layer = { (height: Float) in min(grid.dims.y - 1, max(0, Int32(((map.frame.groundY + height - grid.origin.y) / grid.voxelSize).rounded(.down)))) } + low = layer(0) + let headroomLayer = layer(config.headroom) + let topLayer = layer(config.top) + var columns = [Int32](repeating: -1, count: Int(grid.dims.x) * Int(grid.dims.z)) + var count = 0 + for z in 0..= low else { continue } + columns[Int(x) + Int(grid.dims.x) * Int(z)] = high + count += Int(high - low + 1) + } + } + self.columns = columns + self.count = count + } + + /// In front of the chain by at least `minOut` and at most `outDepth`, and not behind any + /// other piece of it within that piece's span. + static func isInFront(_ map: Map3D, wall: WallFrame, map point: SIMD3, minOut: Float) -> Bool { + let world = map.frame.world(point) + let local = wall.wallPoint(world) + guard local.out >= minOut, local.out <= map.config.outDepth else { return false } + let offset = world - wall.origin + for piece in wall.segments { + let c = piece.coordinates(ofOffset: offset) + if piece.span.contains(c.s), c.out < -map.config.faceBehind, -c.out < map.config.outDepth { return false } + } + return true + } + + func forEachVoxel(_ body: (SIMD3, Voxel?) -> Void) { + let grid = map.grid + for z in 0..= low else { continue } + for y in low...high { + let g = SIMD3(x, y, z) + body(g, grid.voxel(g)) + } + } + } + } +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3D.swift b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3D.swift new file mode 100644 index 000000000..36192117f --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3D.swift @@ -0,0 +1,356 @@ +import Foundation +import simd + +/// A live 3D map of the space around the electric meter, built from what the phone's camera +/// and sensors saw, in the meter-anchored frame (`MapFrame`). +/// +/// It is an occupancy map, not a signed-distance (TSDF) map: coverage asks whether a ray +/// reached a place, and occupancy answers that directly. A ray marks the voxels it passes +/// through as free and the voxel it stops in as surface; a voxel no ray reached stays unknown. +/// So a surface behind a bush is never seen unless some ray got past the bush. A TSDF keeps +/// surfaces more precisely but does not separate never-seen space from free space, and can't +/// take the sparse rays a phone without LiDAR gives. +/// +/// With LiDAR, rays come from depth frames (`integrate(_: DepthFrame)`) and ARKit's mesh adds +/// its classification (`update(_: MeshChunk)`). Without it, rays go to tracked feature points +/// and to detected planes (`integrate(_: FeatureFrame)`, `update(_: PlaneObservation)`), and a +/// monocular depth model's frames, scaled by those points (`MonocularDepth`), give estimated depth. Only frames with normal +/// tracking should be integrated: a limited-tracking pose can put rays half a meter off. +public struct Map3D: Sendable { + public let config: Map3DConfig + public private(set) var frame: MapFrame + public let bounds: MapBounds + /// Increments whenever the map changes. + public private(set) var revision = 0 + + private(set) var grid: VoxelGrid + /// Detected planes by id, in world coordinates as ARKit last reported them. + public private(set) var planes: [UUID: PlaneObservation] = [:] + /// Per mesh chunk and per detected plane, the voxels it marks, so an update or removal can + /// release them (`VoxelGrid.release`). + private var meshVoxels: [UUID: Set] = [:] + private var planeVoxels: [UUID: Set] = [:] + + /// `bounds` defaults to everything the rules can ask about (`MapBounds.around`). + public init(frame: MapFrame, config: Map3DConfig = Map3DConfig(), bounds: MapBounds? = nil) { + self.config = config + self.frame = frame + self.bounds = bounds ?? .around(config, groundY: frame.groundY) + grid = VoxelGrid(bounds: self.bounds, voxelSize: config.voxelSize, center: SIMD3(0, frame.groundY, 0)) + } + + /// Moves the map with the meter's anchor. What is stored keeps its place relative to the + /// meter; frames integrated from now on are placed with the new frame. The voxels are laid + /// out around the ground's height, so the new frame must keep it, as `following` does. + public mutating func reanchor(_ frame: MapFrame) { + precondition(frame.groundY == self.frame.groundY, "reanchoring moves the ground from \(self.frame.groundY) to \(frame.groundY) m") + self.frame = frame + revision += 1 + } + + // MARK: Reading + + /// State of the voxel holding a map point; unknown outside the bounds. + public func state(at point: SIMD3) -> VoxelState { + grid.voxel(at: point)?.state(config) ?? .unknown + } + + public func evidence(at point: SIMD3) -> VoxelEvidence { + grid.voxel(at: point)?.evidence(config) + ?? VoxelEvidence(state: .unknown, hits: 0, passes: 0, nearestDistance: nil, bestViewAngle: nil, normal: nil, meshClass: nil, sources: []) + } + + /// Bytes the voxels take now, and with every brick of the bounds stored. + public var allocatedBytes: Int { grid.allocatedBytes } + public var worstCaseBytes: Int { grid.worstCaseBytes } + + // MARK: Depth + + /// Integrates a LiDAR or estimated depth frame: each sampled pixel is a ray from the camera, + /// free up to just short of its depth, stopping at a surface there. LiDAR pixels below + /// `minConfidence` are skipped. An estimated pixel carves free space to two standard + /// deviations short of its depth and marks a surface only when that is within + /// `maxSurfaceSigma`. Estimated evidence tells seen from unknown but never certifies + /// coverage (`VoxelGrid.integrate`). + public mutating func integrate(_ depth: DepthFrame) { + let mapFromCamera = frame.mapFromWorld * depth.camera.cameraToWorld + let camera = SIMD3(mapFromCamera.columns.3.x, mapFromCamera.columns.3.y, mapFromCamera.columns.3.z) + let rotation = CameraFrame.rotation(mapFromCamera) + let k = max(1, config.pixelStride) + let columns = (depth.width + k - 1) / k + let rows = (depth.height + k - 1) / k + let intrinsics = depth.camera.intrinsics + + // Camera-space points of the sampled pixels; NaN where there is no usable depth. + var points = [SIMD3](repeating: SIMD3(repeating: .nan), count: columns * rows) + var sigmas = [Float](repeating: 0, count: columns * rows) + for row in 0..= config.minDepth, d <= config.maxDepth else { continue } + switch depth.kind { + case .lidar(let confidence): + guard (confidence?[index] ?? 2) >= config.minConfidence else { continue } + case .estimated(let sigma): + guard sigma[index].isFinite, sigma[index] >= 0 else { continue } + sigmas[row * columns + column] = sigma[index] + } + let x = (Float(u) + 0.5 - intrinsics.z) / intrinsics.x * d + let y = -(Float(v) + 0.5 - intrinsics.w) / intrinsics.y * d + points[row * columns + column] = SIMD3(x, y, -d) + } + } + + // Samples next to each other lie d k / fx apart across the ray. A normal is taken from + // the samples about a voxel away on each side, so it describes the surface at the + // map's scale: between neighbours 1.5 cm apart (1.4 m away) a 1 cm depth error alone + // tilts it 30 degrees. A surface up to 80 degrees from facing the camera changes depth + // by at most 5.7 times the spacing; a bigger jump is an edge, across which no normal is + // taken. + let spacing = Float(k) / min(intrinsics.x, intrinsics.y) + var rays: [RaySample] = [] + rays.reserveCapacity(columns * rows) + let estimated: Bool = if case .estimated = depth.kind { true } else { false } + let baseline = estimated ? config.estimatedNormalBaseline : config.voxelSize + let maxReach = estimated ? 64 : 8 + for row in 0.. SIMD3? { + let c = column + dc * reach + let r = row + dr * reach + guard c >= 0, c < columns, r >= 0, r < rows else { return nil } + let q = points[r * columns + c] + guard !q.z.isNaN, abs(q.z - p.z) <= 0.02 + 5.7 * spacing * Float(reach) * -p.z else { return nil } + return q + } + let length = simd_length(p) + var normal = SIMD3.zero + var cosine: Float? + if let dx = neighbour(1, 0).map({ $0 - p }) ?? neighbour(-1, 0).map({ p - $0 }), + let dy = neighbour(0, 1).map({ $0 - p }) ?? neighbour(0, -1).map({ p - $0 }) { + let n = simd_cross(dx, dy) + if simd_length_squared(n) > 0 { + // Facing the camera, which is at the camera-space origin. + let facing = simd_normalize(simd_dot(n, p) < 0 ? n : -n) + cosine = abs(simd_dot(facing, p / length)) + normal = rotation * facing + } + } + let sigma = estimated ? sigmas[row * columns + column] : 0 + if estimated, let c = cosine { + // Two deviations of depth error across the samples the normal spans can tilt + // it by `tilt`; the view counts as that much further from square. + let across = spacing * Float(reach) * -p.z + let tilt = atan(2 * Float(2).squareRoot() * sigma / across) + cosine = cos(min(.pi / 2, acos(min(c, 1)) + tilt)) + } + var freeLength = length - freeMargin(cosine: cosine) + var hit = true + if estimated { + freeLength = min(freeLength, (-p.z - 2 * sigma) / -p.z * length) + hit = 2 * sigma <= config.maxSurfaceSigma + } + let end4 = mapFromCamera * SIMD4(p, 1) + rays.append(RaySample(end: SIMD3(end4.x, end4.y, end4.z), normal: normal, freeLength: freeLength, hit: hit, cosine: cosine)) + } + } + grid.integrate(camera: camera, rays: rays, sources: estimated ? .estimated : .lidar, config: config) + revision += 1 + } + + /// How far short of a surface free space stops, meters. A ray within one voxel of a surface + /// it meets at a glancing angle runs voxel / cos along it, through voxels the surface also + /// crosses; stopping that far short keeps glancing rays from erasing the surface. Not + /// capped: a cap let rays at 80 degrees carve wall voxels a meter short of their hit. + /// Without a normal the angle is unknown, so the ray stops `unknownNormalMargin` short. + private func freeMargin(cosine: Float?) -> Float { + guard let cosine else { return config.unknownNormalMargin } + return config.voxelSize / max(cosine, 1e-3) + } + + // MARK: Without LiDAR + + /// Integrates a frame from a phone without LiDAR. + /// + /// Each tracked feature point is a ray the camera really saw along: free up to just short of + /// the point, surface at it. A point lying on a detected plane (within `planeSnap`) takes + /// that plane's normal, which makes it a measured surface coverage can count. + /// + /// Detected planes are then drawn through a grid of `planeRenderColumns` x + /// `planeRenderRows` pixels, where no measured surface is in front of them, and the voxels + /// they are seen at are marked as plane for wall geometry. They add no occupancy, carve no + /// free space and never count as seen, because a plane's extent says nothing about what + /// stands in front of it: a box with no feature points on it would otherwise read as open + /// space and hide the wall behind it while leaving that wall "seen". An updated or removed + /// plane takes its marks with it. + public mutating func integrate(_ features: FeatureFrame) { + let camera = frame.map(features.camera.position) + let mapPlanes = planesInMap() + var featureRays: [RaySample] = [] + for point in features.points { + let end = frame.map(point) + let length = simd_distance(end, camera) + guard length.isFinite, length >= config.minDepth, length <= config.maxDepth else { continue } + let direction = (end - camera) / length + let normal = mapPlanes.first { plane in + abs(simd_dot(end - plane.point, plane.normal)) <= config.planeSnap && plane.contains(end) + }.map { simd_dot($0.normal, direction) > 0 ? -$0.normal : $0.normal } ?? .zero + let cosine = simd_length_squared(normal) > 0 ? abs(simd_dot(normal, direction)) : nil + featureRays.append(RaySample(end: end, normal: normal, freeLength: length - freeMargin(cosine: cosine), hit: true)) + } + grid.integrate(camera: camera, rays: featureRays, sources: .feature, config: config) + for hit in planeHits(from: features.camera, camera: camera, planes: mapPlanes) { + guard let index = grid.markPlane(at: hit.point, normal: hit.normal), planeVoxels[hit.id, default: []].insert(index).inserted else { continue } + grid.retain([index], as: .plane) + } + revision += 1 + } + + /// Adds or replaces a detected plane. It shows up in the map on the next feature frame that + /// views it; a plane whose pose or extent changed loses its old marks at once. The same + /// plane passed again unchanged keeps them. A plane wholly outside the bounds is dropped. + public mutating func update(_ plane: PlaneObservation) { + if let old = planes[plane.id], old.worldFromPlane == plane.worldFromPlane, old.boundary == plane.boundary { return } + removePlane(id: plane.id) + let mapFromPlane = frame.mapFromWorld * plane.worldFromPlane + let corners = plane.boundary.map { p in + let q = mapFromPlane * SIMD4(p.x, 0, p.y, 1) + return SIMD3(q.x, q.y, q.z) + } + guard let first = corners.first else { return } + let low = corners.reduce(first, simd_min) + let high = corners.reduce(first, simd_max) + guard all(high .>= bounds.min), all(low .< bounds.max) else { return } + planes[plane.id] = plane + } + + public mutating func removePlane(id: UUID) { + planes[id] = nil + if let old = planeVoxels.removeValue(forKey: id) { grid.release(old, as: .plane) } + revision += 1 + } + + private struct MapPlane { + var id: UUID + var point: SIMD3 + var normal: SIMD3 + var planeFromMap: simd_float4x4 + var boundary: [SIMD2] + + func contains(_ p: SIMD3) -> Bool { + let local = planeFromMap * SIMD4(p, 1) + return Map3D.polygon(boundary, contains: SIMD2(local.x, local.z)) + } + } + + private func planesInMap() -> [MapPlane] { + planes.values.compactMap { plane in + guard plane.boundary.count >= 3 else { return nil } + let mapFromPlane = frame.mapFromWorld * plane.worldFromPlane + let normal = SIMD3(mapFromPlane.columns.1.x, mapFromPlane.columns.1.y, mapFromPlane.columns.1.z) + guard simd_length_squared(normal) > 0 else { return nil } + return MapPlane( + id: plane.id, point: SIMD3(mapFromPlane.columns.3.x, mapFromPlane.columns.3.y, mapFromPlane.columns.3.z), + normal: simd_normalize(normal), planeFromMap: mapFromPlane.inverse, boundary: plane.boundary) + } + } + + /// Where each rendered pixel's ray meets the nearest plane, with the plane and its normal + /// facing the camera, unless a measured surface is in front of it. + private func planeHits(from cameraFrame: CameraFrame, camera: SIMD3, planes mapPlanes: [MapPlane]) -> [(id: UUID, point: SIMD3, normal: SIMD3)] { + guard !mapPlanes.isEmpty else { return [] } + var hits: [(id: UUID, point: SIMD3, normal: SIMD3)] = [] + let columns = max(1, config.planeRenderColumns) + let rows = max(1, config.planeRenderRows) + for row in 0..)? + for plane in mapPlanes { + let denominator = simd_dot(plane.normal, direction) + guard abs(denominator) > 1e-4 else { continue } + let t = simd_dot(plane.normal, plane.point - camera) / denominator + guard t >= config.minDepth, t <= config.maxDepth, t < nearest?.t ?? .infinity, plane.contains(camera + direction * t) else { continue } + nearest = (t, plane, denominator > 0 ? -plane.normal : plane.normal) + } + guard let nearest else { continue } + let limit = nearest.t - freeMargin(cosine: abs(simd_dot(nearest.normal, direction))) + guard firstSurface(from: camera, direction: direction, length: limit) == nil else { continue } + hits.append((nearest.plane.id, camera + direction * nearest.t, nearest.normal)) + } + } + return hits + } + + /// Distance along a ray at which it enters the first surface voxel, within `length`. + func firstSurface(from origin: SIMD3, direction: SIMD3, length: Float) -> Float? { + guard length > 0 else { return nil } + var found: Float? + grid.march(from: origin, direction: direction, length: length) { _, voxel, entered in + if let voxel, voxel.state(config) == .surface { + found = entered + return false + } + return true + } + return found + } + + /// Even-odd rule. + static func polygon(_ polygon: [SIMD2], contains p: SIMD2) -> Bool { + var inside = false + var j = polygon.count - 1 + for i in polygon.indices { + let a = polygon[i] + let b = polygon[j] + if (a.y > p.y) != (b.y > p.y), p.x < (b.x - a.x) * (p.y - a.y) / (b.y - a.y) + a.x { + inside.toggle() + } + j = i + } + return inside + } + + // MARK: Mesh + + /// Adds or replaces a chunk of ARKit's mesh. It labels the voxels its faces cross with their + /// classification and gives voxels no ray has given a normal the face's. It adds no + /// occupancy: a mesh face carries no camera, so it can't say what was seen from where. + public mutating func update(_ chunk: MeshChunk) { + if let old = meshVoxels.removeValue(forKey: chunk.id) { grid.release(old, as: .mesh) } + let mapFromChunk = frame.mapFromWorld * chunk.worldFromChunk + let vertices = chunk.vertices.map { v in + let p = mapFromChunk * SIMD4(v, 1) + return SIMD3(p.x, p.y, p.z) + } + var marked: Set = [] + for (index, face) in chunk.faces.enumerated() { + guard face.x < vertices.count, face.y < vertices.count, face.z < vertices.count else { continue } + let triangle = (vertices[Int(face.x)], vertices[Int(face.y)], vertices[Int(face.z)]) + // Faces wholly outside the bounds are skipped before sampling them. + let low = simd_min(triangle.0, simd_min(triangle.1, triangle.2)) + let high = simd_max(triangle.0, simd_max(triangle.1, triangle.2)) + guard all(high .>= bounds.min), all(low .< bounds.max) else { continue } + marked.formUnion(grid.markMesh(triangle, meshClass: chunk.classes.isEmpty ? nil : chunk.classes[index])) + } + if !marked.isEmpty { + grid.retain(marked, as: .mesh) + meshVoxels[chunk.id] = marked + } + revision += 1 + } + + public mutating func removeMeshChunk(id: UUID) { + if let old = meshVoxels.removeValue(forKey: id) { grid.release(old, as: .mesh) } + revision += 1 + } +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3DBatch.swift b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3DBatch.swift new file mode 100644 index 000000000..91523f4ab --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3DBatch.swift @@ -0,0 +1,18 @@ +import Foundation + +extension Map3D { + /// One batch of ARKit updates that arrived together with a move of the meter's anchor + /// (`frame`, from `MapFrame.following`; nil when the meter didn't move). The move goes first: + /// ARKit places every anchor it sends in the corrected world, and a chunk drawn with the old + /// frame would move a second time with the map. `Map3DSession` applies its batches through + /// this. + public mutating func apply(frame: MapFrame?, planes: [UUID: PlaneObservation?], chunks: [UUID: MeshChunk?]) { + if let frame { reanchor(frame) } + for (id, plane) in planes { + if let plane { update(plane) } else { removePlane(id: id) } + } + for (id, chunk) in chunks { + if let chunk { update(chunk) } else { removeMeshChunk(id: id) } + } + } +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3DConfig.swift b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3DConfig.swift new file mode 100644 index 000000000..1f48edccd --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3DConfig.swift @@ -0,0 +1,156 @@ +import Foundation + +/// Settings of the 3D map. Each value is a starting hypothesis unless its comment cites a +/// source; none has been tuned on real captures yet. +public struct Map3DConfig: Sendable, Equatable { + // MARK: Storage + + /// Voxel edge, meters. 10 cm is finer than a 6 in coverage cell and than the server's smallest + /// position error (0.3 ft, 9 cm, rules.yaml errors.meter_ft), and keeps the worst case of the + /// default bounds near 30 MB (`MapBounds.around`, 16 bytes a voxel). + public var voxelSize: Float = 0.1 + + // MARK: Region of interest + + /// How far along the wall chain from the meter the map must see, meters: 20 ft of cable + /// reach (Base help page, rules.yaml route.max_ft) plus a battery's width (2.58 ft, the + /// server's W) and the 3 ft gas, AC and opening clearances beyond its far edge. The pool's + /// 10 ft is a placeholder in rules.yaml and is not included. + public var alongExtent: Float = (20 + 2.58 + 3) * 0.3048 + /// How far out from the wall, meters: 15 ft, the ground reach `CoverageConfig.groundDepthReach` + /// asks for (a pool 10 ft from a battery 1.83 ft deep, with error to spare). + public var outDepth: Float = 4.572 + /// Headroom, meters: 6.5 ft, NEC 110.26(A)(3) and rules.yaml headroom.min_ft. The wall band + /// runs from the ground to here, as `CoverageConfig.wallBandHeight` does. + public var headroom: Float = 1.9812 + /// Top of the map above the meter's ground, meters. Above headroom by enough that an + /// overhead reach can show more than headroom clear. + public var top: Float = 2.5 + /// How far below the meter's ground the map reaches, meters, for ground that falls away. + public var groundBelow: Float = 0.5 + /// Coverage cell width along the wall, meters: `CoverageConfig.cellWidth` (6 in), so spans + /// from this map and from the 2D coverage map meet on the same edges. + public var cellWidth: Float = 0.1524 + /// Spacing of ground rows out from the wall, meters: `CoverageConfig.groundDepthSpacing`. + public var groundRowSpacing: Float = 0.1524 + + // MARK: Integration + + /// Every `pixelStride`-th pixel of a depth image along each axis is cast as a ray. 2 keeps a + /// ray every 4.4 cm at 5 m on ARKit's 256 x 192 depth (about 0.25 degrees a pixel), under half + /// a voxel, at a quarter of the work. + public var pixelStride = 2 + /// LiDAR depths below this ARKit confidence (0 low, 1 medium, 2 high) are ignored. ARKit + /// marks edges, dark and shiny surfaces low. + public var minConfidence: UInt8 = 1 + /// Depths outside this range are ignored, meters. ARKit's LiDAR is rated to 5 m. + public var minDepth: Float = 0.2 + public var maxDepth: Float = 5 + /// An estimated depth marks a surface only when two standard deviations are within this, + /// meters (one and a half voxels); free space is carved to two deviations short of it either way. + public var maxSurfaceSigma: Float = 0.15 + /// An estimated depth's normal is taken between samples about this far apart, meters, and + /// a view counts as further from square by the tilt two deviations of depth error across + /// that span could give it. At a voxel's 0.1 m, 7 cm of independent noise tilts a normal by + /// up to 45 degrees and lets views 68 degrees from the ground count as within 65 + /// (Map3DEstimatedTests); across 0.3 m the same noise costs 33 degrees, and straight-on views + /// still count. Wider spans blur more edges into a normal. A guess, not measured. + public var estimatedNormalBaseline: Float = 0.3 + /// Free space stops this far short of a surface whose normal is unknown, meters (a feature + /// point off every plane, a depth pixel at an edge): rays within 11 degrees of grazing a + /// surface could still erase it. + public var unknownNormalMargin: Float = 0.5 + /// A feature point this close to a detected plane, meters, takes the plane's normal. ARKit's + /// feature points scatter a few centimeters about the surface they lie on. + public var planeSnap: Float = 0.05 + /// Pixel grid rendered from detected planes on each feature frame, along each axis. + public var planeRenderColumns = 64 + public var planeRenderRows = 48 + + /// Log-odds of occupancy, in hundredths. A hit adds 0.85 (p = 0.7) and a pass through + /// subtracts 0.4 (p = 0.4), OctoMap's defaults; the clamp lets a voxel change its mind within + /// a few frames. At most one hit or one pass counts per voxel per frame, a hit winning. + public var hitLogOdds: Int16 = 85 + public var missLogOdds: Int16 = -40 + public var minLogOdds: Int16 = -200 + public var maxLogOdds: Int16 = 350 + /// A voxel is surface at or above this and seen free at or below `freeLogOdds`; in between, + /// or never touched, it is unknown. + public var surfaceLogOdds: Int16 = 50 + public var freeLogOdds: Int16 = -40 + + // MARK: Coverage + + /// A surface counts as seen only from this close, meters: LiDAR's rated range. + public var maxViewDistance: Float = 5 + /// A surface counts as seen only from a view within this angle of its normal, as + /// `CoverageConfig.maxAngleFromNormal`. + public var maxViewAngle: Float = 65 * .pi / 180 + /// How far behind and in front of the wall chain's line a surface still counts as the wall + /// face, meters: a voxel either way, plus half a voxel behind for a line placed a little in + /// front of the real face. Anything standing farther out hides the face, including the + /// meter itself and boxes on the wall, so the few cells behind the meter stay unseen. A + /// wider window would let a shrub against the wall stand in for the wall behind it, and the + /// wall band settles the checks for boxes, vents and openings there. + public var faceBehind: Float = 0.15 + public var faceFront: Float = 0.1 + /// Relief: attached structure standing up to this far proud of the wall line, meters, such + /// as a pilaster, column or chimney breast, counts as the facade for the wall band, because + /// nothing can be mounted on the wall behind it. ETH3D electro's pilasters stand 0.36 m + /// proud (experiments/evals/results/map3d.md on t3/evals). It counts only up to where it + /// runs without the face showing above it, where no free space was seen between it and the + /// wall, and where the mesh, when it classifies, does not call it something else; a box or + /// shrub against the wall still hides the wall. + public var reliefDepth: Float = 0.5 + /// Relief is at most this wide along the wall, meters; a wider face in front of the wall is + /// a wall of its own (a bump-out). + public var maxReliefWidth: Float = 1.0 + /// Recess: a face seen up to this far behind the wall line, meters (a door or window set + /// back), is the facade there. + public var recessDepth: Float = 0.5 + /// Ground is looked for from this far above to this far below the meter's ground, meters, + /// so a yard that slopes a little still has ground. Anything flat and lower than this, such + /// as a pad or a low step, reads as ground. + public var groundSearch: Float = 0.2 + /// Facing and overhead space are judged from this far above the ground seen under them, + /// meters, so grass and uneven ground do not read as something in front of the wall. + /// Anything lower is not told apart from the ground. + public var groundClearance: Float = 0.15 + /// Depth of the space over the battery that overhead judges, meters: the battery's 1.83 ft + /// (the server's D). + public var overheadDepth: Float = 0.5588 + + // MARK: Walls + + /// A plan cell is wall when its vertical surface voxels between `wallBottom` and `top` + /// add up to at least this height, meters. Taller than a typical shrub, shorter than a wall + /// with a window in it. + public var minWallHeight: Float = 0.8 + /// Wall evidence is taken from this height up, meters, clear of the ground and low clutter. + public var wallBottom: Float = 0.3 + /// A plan cell belongs to a line when it is this close to it, meters, and its normal is + /// within `wallNormalTolerance` of the line's. + public var wallInlierDistance: Float = 0.15 + public var wallNormalTolerance: Float = 25 * .pi / 180 + /// Wall pieces shorter than this are dropped, meters. + public var minWallLength: Float = 0.4 + /// A gap along one line longer than this splits it into two pieces, meters. + public var maxWallGap: Float = 0.35 + /// Two pieces meet at a corner when both ends are this close to where their lines cross, + /// meters; lines closer than `minCornerAngle` to parallel join straight across a gap of up + /// to `maxWallBridge` instead, the gap being wall not seen rather than no wall. + public var cornerJoinDistance: Float = 0.75 + public var minCornerAngle: Float = 20 * .pi / 180 + public var maxWallBridge: Float = 1.0 + + // MARK: Fog of war and views + + /// Edge of a fog cell, meters: three voxels, coarse enough for the UI to draw every one. + public var fogCellSize: Float = 0.3 + /// Where a suggested view stands: this far from what it should see, meters, and with the + /// camera this high. + public var viewDistances: [Float] = [1.5, 2.5, 3.5] + public var eyeHeight: Float = 1.4 + + public init() {} +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3DCoverage.swift b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3DCoverage.swift new file mode 100644 index 000000000..e8bf408d8 --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3DCoverage.swift @@ -0,0 +1,476 @@ +import Foundation +import simd + +/// What the 3D map saw along a wall chain, in the form scene.json's `coverage.observed` takes +/// (server/README.md "What settles each check", on t3/server). s and out are meters along and +/// out from the chain the spans were measured against. +public struct Map3DCoverage: Sendable, Equatable { + /// Stretches where the wall face was seen from the ground to headroom. + public var wall: [ClosedRange] + /// Stretches of wall face, each with how high above the ground it was seen without a break: + /// scene.json's wall `out_ft`, which each server check credits against its own height. + public var wallHeight: [ObservedSpan] + /// Stretches of ground, each with how far out from the wall it was seen without a break. + public var ground: [ObservedSpan] + /// Stretches with the space in front of the wall seen clear, each with how far out. + public var facing: [ObservedSpan] + /// Stretches with the space over the battery's depth seen clear, each with how high. + public var overhead: [ObservedSpan] +} + +extension SceneCoverage { + /// The 3D map's coverage, with the ends' kinds from the homeowner's answers. + public init(_ coverage: Map3DCoverage, leftEndMarked: Bool, rightEndMarked: Bool) { + self.init( + leftEndMarked: leftEndMarked, rightEndMarked: rightEndMarked, wall: coverage.wallHeight, + ground: coverage.ground, facing: coverage.facing, overhead: coverage.overhead) + } +} + +/// Coverage is read from the voxels, so a place counts as seen only where a ray reached it: +/// behind a bush stays unseen however many frames pointed at it. Each cell is judged at sample +/// points half a voxel apart, and every sample must pass. +extension Map3D { + /// Cells along the chain within `alongExtent` of the meter either way. + public var cellIndices: ClosedRange { + Int((-config.alongExtent / config.cellWidth).rounded(.down))...Int((config.alongExtent / config.cellWidth).rounded(.up)) - 1 + } + + /// A cell's s range. Edges come from the index alone, like `CoverageMap.cellRange`, so + /// neighbouring cells share bit-identical edges. + public func cellRange(_ index: Int) -> ClosedRange { + (Float(index) * config.cellWidth)...(Float(index + 1) * config.cellWidth) + } + + public func coverage(along wall: WallFrame) -> Map3DCoverage { + var seenWall: [ClosedRange] = [] + var wallHeight: [ObservedSpan] = [] + var ground: [ObservedSpan] = [] + var facing: [ObservedSpan] = [] + var overhead: [ObservedSpan] = [] + let facades = facadeOffsets(along: wall) + for index in cellIndices { + let range = cellRange(index) + let height = seenHeight(cell: index, along: wall, facade: facades[index]) + if let height { wallHeight.append(ObservedSpan(span: range, out: height)) } + let wallSeen = (height ?? 0) >= config.headroom + if wallSeen { + if let last = seenWall.last, last.upperBound == range.lowerBound { + seenWall[seenWall.count - 1] = last.lowerBound...range.upperBound + } else { + seenWall.append(range) + } + } + let facade = facades[index] ?? 0 + if let out = groundReach(cell: index, along: wall, facade: facade) { ground.append(ObservedSpan(span: range, out: out)) } + guard wallSeen else { continue } + if let out = clearReach(cell: index, along: wall, facade: facade) { facing.append(ObservedSpan(span: range, out: out)) } + if let height = clearHeight(cell: index, along: wall, facade: facades[index]) { overhead.append(ObservedSpan(span: range, out: height)) } + } + let touching = config.cellWidth * 0.01 + return Map3DCoverage( + wall: seenWall, wallHeight: ObservedSpan.merge(wallHeight, touching: touching), ground: ObservedSpan.merge(ground, touching: touching), + facing: ObservedSpan.merge(facing, touching: touching), overhead: ObservedSpan.merge(overhead, touching: touching)) + } + + // MARK: Per cell + + /// Whether the facade over a cell was seen from `voxelSize` up to headroom (`wallHeight` + /// reaches it). At every sample + /// some well-seen surface (`VoxelGrid.isWellSeenSurface`) must lie on the facade face, from + /// `recessDepth` behind the facade to `faceTolerance` in front of it, or on attached relief + /// (`attachedRelief`) up to `reliefDepth` in front. The facade is where the wall's surface + /// was measured near the cell (`facadeOffset`), so a chain line placed a few centimeters + /// off still finds it. Anything else standing in front of the wall, a box or a shrub, hides + /// it. The layer at the ground belongs to the ground band. + public func isWallSeen(cell index: Int, along wall: WallFrame) -> Bool { + isWallSeen(cell: index, along: wall, facade: facadeOffset(cell: index, along: wall)) + } + + private func isWallSeen(cell index: Int, along wall: WallFrame, facade: Float?) -> Bool { + (seenHeight(cell: index, along: wall, facade: facade) ?? 0) >= config.headroom + } + + /// How high above the ground the facade over a cell was seen without a break, meters: the + /// least over the cell's samples of `seenHeight(_:s:facade:)`. Nil where not even the + /// lowest row was. + public func wallHeight(cell index: Int, along wall: WallFrame) -> Float? { + seenHeight(cell: index, along: wall, facade: facadeOffset(cell: index, along: wall)) + } + + private func seenHeight(cell index: Int, along wall: WallFrame, facade: Float?) -> Float? { + guard let facade else { return nil } + let heights = bandHeights() + var least: Float = .infinity + for s in samples(in: cellRange(index)) { + guard let top = seenHeight(wall, s: s, heights: heights, facade: facade) else { return nil } + least = min(least, top) + } + return least.isFinite ? least : nil + } + + /// The highest row at s up to which every row, from the lowest, shows the facade: its face, + /// or attached relief (`attachedRelief`). Relief counts only where it runs on to that height; + /// where the face shows above relief (a box on the wall, a shrub), the height stops below it. + /// + /// A surface standing in front of the face at the foot, with the face behind it running on + /// up to the highest row seen, is something in front of the wall (a box on the ground), even + /// where it became the local facade: `facadeOffset` takes the most common offset, and a box + /// wider than its neighbourhood and taller than the wall seen above it wins. Such rows count + /// as relief, so the height stops below them. A window recess (the face in front above it + /// again) and a door recess (the face in front above it) are not this. + private func seenHeight(_ wall: WallFrame, s: Float, heights: [Float], facade: Float) -> Float? { + var rows = heights.map { faceSample(wall, s: s, height: $0, facade: facade) } + if let top = rows.last(where: \.face)?.faceOut { + for index in rows.indices { + guard let out = rows[index].faceOut, out > top + faceTolerance else { continue } + rows[index].face = false + rows[index].relief = out + } + } + var top = (rows.firstIndex { !$0.face && $0.relief == nil } ?? rows.count) - 1 + if let firstRelief = rows.firstIndex(where: { !$0.face }), firstRelief <= top { + let relief = Array(rows[firstRelief...top]) + if relief.contains(where: \.face) || !attachedRelief(wall, s: s, rows: relief, facade: facade) { top = firstRelief - 1 } + } + return top >= 0 ? rows[top].height : nil + } + + /// Heights the wall band is judged at: every half voxel from one voxel up to the top of the + /// map, and headroom itself, which the facing and overhead bands need seen. + private func bandHeights() -> [Float] { + (Array(stride(from: config.voxelSize, to: config.top, by: config.voxelSize / 2)) + [config.headroom]).sorted() + } + + /// Front of the facade face, meters past the facade: a voxel's center lies at most 7.1 cm + /// from a vertical plane through it (half a voxel's diagonal), so 7.5 cm keeps every voxel + /// the face runs through and turns away the next layer out. Something standing closer to + /// the wall than about that is part of the facade. + var faceTolerance: Float { config.voxelSize * 0.75 } + + /// Where the facade runs near a cell, meters out from the chain's line: the mode (2 cm bins) + /// of the offsets of well-seen, outward-facing surface voxels within `faceBehind` of the + /// line, over the cells within about a meter either side. Nil where none was seen. + func facadeOffset(cell index: Int, along wall: WallFrame) -> Float? { + facadeHistogram(cells: (index - 6)...(index + 6), along: wall).mode + } + + private func facadeOffsets(along wall: WallFrame) -> [Int: Float] { + var result: [Int: Float] = [:] + let perCell = Dictionary(uniqueKeysWithValues: cellIndices.map { ($0, facadeHistogram(cells: $0...$0, along: wall)) }) + for index in cellIndices { + var sum = Histogram() + for neighbour in (index - 6)...(index + 6) { if let h = perCell[neighbour] { sum.add(h) } } + result[index] = sum.mode + } + return result + } + + struct Histogram { + /// Count per 2 cm bin of offset. + var bins: [Int: Int] = [:] + static let bin: Float = 0.02 + + mutating func add(_ offset: Float) { bins[Int((offset / Self.bin).rounded()), default: 0] += 1 } + mutating func add(_ other: Histogram) { for (k, v) in other.bins { bins[k, default: 0] += v } } + + /// The most common offset, ties toward the line. + var mode: Float? { + bins.max { a, b in a.value != b.value ? a.value < b.value : abs(a.key) > abs(b.key) }.map { Float($0.key) * Self.bin } + } + } + + private func facadeHistogram(cells: ClosedRange, along wall: WallFrame) -> Histogram { + var histogram = Histogram() + for index in cells { + for s in samples(in: cellRange(index)) { + let outward = frame.mapDirection(wall.segment(atS: s).outward) + var seen = Set() + for height in stride(from: config.voxelSize * 2, to: config.headroom, by: config.voxelSize * 2) { + for out in stride(from: -config.faceBehind, through: config.faceBehind, by: config.voxelSize / 2) { + guard let g = coordinate(wall, s: s, height: height, out: out), let i = grid.index(g), seen.insert(i).inserted else { continue } + let voxel = grid.pool[i] + guard grid.isWellSeenSurface(i, config: config), simd_dot(voxel.normal ?? .zero, outward) >= cos(Float.pi / 6) else { continue } + histogram.add(wall.out(of: frame.world(grid.center(of: g)), pieceAtS: s)) + } + } + } + } + return histogram + } + + + struct FaceSample { + var height: Float + /// A well-seen surface lies on the wall face. + var face: Bool + /// The frontmost such surface, meters past the facade. + var faceOut: Float? + /// The nearest well-seen surface standing proud of the face, facing out, and how far out. + var relief: Float? + } + + /// What the voxels along the wall's normal at (s, height) show, judged by voxel centers + /// against the facade's offset. + func faceSample(_ wall: WallFrame, s: Float, height: Float, facade: Float) -> FaceSample { + let outward = frame.mapDirection(wall.segment(atS: s).outward) + var sample = FaceSample(height: height, face: false, faceOut: nil, relief: nil) + for out in stride(from: facade - config.recessDepth, through: facade + config.reliefDepth, by: config.voxelSize / 2) { + guard let g = coordinate(wall, s: s, height: height, out: out), let voxel = grid.voxel(g), grid.isWellSeenSurface(g, config: config) else { continue } + let centerOut = wall.out(of: frame.world(grid.center(of: g)), pieceAtS: s) - facade + if centerOut >= -config.recessDepth - 1e-4, centerOut <= faceTolerance + 1e-4 { + sample.face = true + sample.faceOut = max(sample.faceOut ?? -.infinity, centerOut) + } else if centerOut <= config.reliefDepth + 1e-4, sample.relief == nil, simd_dot(voxel.normal ?? .zero, outward) >= cos(Float.pi / 4), + !Self.clutterClasses.contains(voxel.meshLabel) { + sample.relief = centerOut + } + } + return sample + } + + /// Mesh classes that are never part of the facade (`Voxel.meshLabel` values). + private static let clutterClasses: Set = Set([MeshClass.none, .floor, .ceiling, .table, .seat].map { $0.rawValue + 1 }) + + /// Whether the relief rows at s are attached structure. No measured free voxel may lie + /// between the relief and the wall at any of them (a gap makes it something standing in + /// front of the wall), and there must be positive evidence of attachment: at one of the + /// relief's lateral edges, found within `maxReliefWidth` either way, its side face shows as + /// well-seen surface from its front back to the facade face over every row. An unseen gap + /// is not evidence: the review of 2f17d67 had a freestanding slab 0.3 m out, whose gap no + /// view reached, pass as relief and the wall behind it counted as seen. + private func attachedRelief(_ wall: WallFrame, s: Float, rows: [FaceSample], facade: Float) -> Bool { + let relief = rows.filter { !$0.face } + for row in relief { + guard let front = row.relief else { return false } + for out in stride(from: facade + faceTolerance, to: facade + front, by: config.voxelSize / 2) { + if coordinate(wall, s: s, height: row.height, out: out).map({ grid.isMeasuredFree($0, config: config) }) == true { return false } + } + } + guard !relief.isEmpty else { return true } + let probe = relief[relief.count / 2] + for direction: Float in [-1, 1] { + guard let edge = reliefEdge(wall, from: s, direction: direction, at: probe.height, facade: facade) else { continue } + if relief.allSatisfy({ sideFaceShows(wall, edge: edge, row: $0, facade: facade) }) { return true } + } + return false + } + + /// Where the relief at s runs out going `direction` along the wall at `height`: halfway + /// between its last sample and the first without it. Nil when it runs on past + /// `maxReliefWidth`, which makes it a wall of its own rather than relief. + private func reliefEdge(_ wall: WallFrame, from s: Float, direction: Float, at height: Float, facade: Float) -> Float? { + let step = config.voxelSize / 2 + for d in stride(from: step, through: config.maxReliefWidth, by: step) { + let sample = faceSample(wall, s: s + direction * d, height: height, facade: facade) + if sample.face || sample.relief == nil { return s + direction * (d - step / 2) } + } + return nil + } + + /// Whether the relief's side face at `edge` shows at a row: at every half voxel from the + /// facade face out to a voxel short of the relief's front, a well-seen surface within half a + /// voxel of the edge, facing along the wall within 45 degrees. The voxel at the front corner + /// holds the front face too, and its normal turns halfway between them (on the synthetic + /// pilasters, 37 to 60 degrees from the side). Relief whose front stands less than a voxel + /// past `faceTolerance` has no sample left to check and passes: a gap that thin can't be + /// told from contact at 10 cm voxels. + private func sideFaceShows(_ wall: WallFrame, edge: Float, row: FaceSample, facade: Float) -> Bool { + guard let front = row.relief else { return false } + let step = config.voxelSize / 2 + let along = frame.mapDirection(wall.segment(atS: edge).along) + for out in stride(from: facade + faceTolerance, through: facade + front - config.voxelSize, by: step) { + let shows = [edge - step, edge, edge + step].contains { s in + guard let g = coordinate(wall, s: s, height: row.height, out: out), grid.isWellSeenSurface(g, config: config), + let normal = grid.voxel(g)?.normal else { return false } + return abs(simd_dot(normal, along)) >= cos(Float.pi / 4) + } + if !shows { return false } + } + return true + } + + /// How far out from the wall the ground in front of a cell was seen without a break, + /// meters, in whole rows of `groundRowSpacing`; nil when not even the first voxel in front + /// of the wall was. Ground is checked every half voxel out (`groundHeight`), so no voxel + /// between two rows goes unchecked. + /// + /// The reach is reported from the wall's line, but ground can only be judged from the first + /// voxel past the facade's face (`nearStart`): nearer, the voxels hold the face. That strip + /// counts as seen only where the facade shows at its foot at every sample (`footShowsFace`): + /// the rays that reached the face's lowest row crossed the strip just above the ground, as + /// the rays to the face do for `facingReach`. + public func groundReach(cell index: Int, along wall: WallFrame) -> Float? { + groundReach(cell: index, along: wall, facade: facadeOffset(cell: index, along: wall) ?? 0) + } + + /// Where the space in front of the facade starts, meters out from the chain's line: the + /// middle of the first voxel past the facade's face. Ground is judged from here. + func nearStart(_ facade: Float) -> Float { facade + faceTolerance + config.voxelSize / 2 } + + private func groundReach(cell index: Int, along wall: WallFrame, facade: Float) -> Float? { + let ss = samples(in: cellRange(index)) + guard ss.allSatisfy({ footShowsFace(wall, s: $0, facade: facade) }) else { return nil } + guard let seen = contiguousReach(from: nearStart(facade), to: config.outDepth, { out in + ss.allSatisfy { groundHeight(wall, s: $0, out: out) != nil } + }) else { return nil } + return (seen / config.groundRowSpacing + 1e-4).rounded(.down) * config.groundRowSpacing + } + + /// How far the facade face at its lowest row may turn up from facing out, radians. A low + /// object standing at the foot, too near the face to part from it at 10 cm voxels, gives the + /// face's voxel its top as well: in the synthetic foot scene (`Map3DFootTests`) a clear foot + /// faces straight out, and a box 0.1 m tall and 0.05 m deep turns it 27 degrees up. 20 + /// degrees is between them: a guess from that scene, not measured on a phone. + static let footFaceTilt: Float = 20 * .pi / 180 + + /// Whether the facade shows at its lowest row at s: a well-seen surface on the face + /// (`faceSample`), nothing proud of it there, and facing out within `footFaceTilt`. + private func footShowsFace(_ wall: WallFrame, s: Float, facade: Float) -> Bool { + let height = config.voxelSize + let sample = faceSample(wall, s: s, height: height, facade: facade) + guard sample.face, sample.relief == nil else { return false } + let outward = frame.mapDirection(wall.segment(atS: s).outward) + for out in stride(from: facade - config.recessDepth, through: facade + faceTolerance, by: config.voxelSize / 2) { + guard let g = coordinate(wall, s: s, height: height, out: out), let voxel = grid.voxel(g), grid.isWellSeenSurface(g, config: config), + let normal = voxel.normal else { continue } + let centerOut = wall.out(of: frame.world(grid.center(of: g)), pieceAtS: s) - facade + guard centerOut >= -config.recessDepth - 1e-4, centerOut <= faceTolerance + 1e-4 else { continue } + if simd_dot(simd_normalize(normal), outward) >= cos(Self.footFaceTilt) { return true } + } + return false + } + + /// Height of the ground seen at a point in front of the wall, meters above the chain's + /// ground; nil where it was not seen. Looking down from `groundSearch` above to as far + /// below, the first voxel that is not free must be a well-seen surface facing within 45 + /// degrees of up, and every voxel above it must be free, except within `groundClearance` + /// of it: a ray that ended on the ground leaves the last stretch before it unmarked (see + /// `VoxelGrid.carveFree`), and anything that low is not told apart from the ground anyway. + /// Ground under a bush, or under anything else taller than that, is not seen. Free means + /// measured free (`VoxelGrid.isMeasuredFree`). + func groundHeight(_ wall: WallFrame, s: Float, out: Float) -> Float? { + var unknownFrom: Float? + for height in stride(from: config.groundSearch, through: -config.groundSearch, by: -config.voxelSize / 2) { + guard let g = coordinate(wall, s: s, height: height, out: out), let voxel = grid.voxel(g) else { return nil } + switch voxel.state(config) { + // Free only by estimated depth is not known free here. + case .free where grid.isMeasuredFree(g, config: config): + guard unknownFrom == nil else { return nil } + case .free, .unknown: + if unknownFrom == nil { unknownFrom = height } + case .surface: + guard grid.isWellSeenSurface(g, config: config), (voxel.normal?.y ?? 0) >= cos(Float.pi / 4) else { return nil } + if let unknownFrom, unknownFrom - height > config.groundClearance { return nil } + return frame.world(grid.center(of: g)).y - wall.groundY + } + } + return nil + } + + /// How far out from the wall the space in front of a cell was seen clear, meters, or nil. + /// Only for a cell whose facade was seen: the rays that reached it crossed the space just + /// in front of it, up to the last stretch before the face (`VoxelGrid.carveFree`), which is + /// the face's own depth. From one voxel past `faceFront` out to the distance returned, the + /// ground was seen and every voxel from `groundClearance` above it to headroom is free, and + /// a voxel is free only where a ray crossed it and ended beyond it. + public func facingReach(cell index: Int, along wall: WallFrame) -> Float? { + let facade = facadeOffset(cell: index, along: wall) + guard let facade, isWallSeen(cell: index, along: wall, facade: facade) else { return nil } + return clearReach(cell: index, along: wall, facade: facade) + } + + /// Space is judged from half a voxel past `nearStart`: the voxel next to the facade's face + /// only ever holds the last stretch of rays that ended on the face, never known free. A + /// reach short of `overheadDepth` (the battery's depth) is not reported: the server's + /// facing check needs space past the battery's front, so it could never settle anything, + /// and space that close to a surface is what depth measures least surely (ETH3D electro's + /// laser truth can't confirm or refute it within 12 to 20 cm of a face). + private func clearReach(cell index: Int, along wall: WallFrame, facade: Float) -> Float? { + let ss = samples(in: cellRange(index)) + let reach = contiguousReach(from: nearStart(facade) + config.voxelSize / 2, to: config.outDepth) { out in + ss.allSatisfy { isClear(wall, s: $0, out: out, upTo: config.headroom) } + } + return reach.flatMap { $0 >= config.overheadDepth ? $0 : nil } + } + + /// How high above the ground the space over a battery at this cell was seen clear, meters, + /// or nil. Only for a cell whose facade was seen, as for `facingReach`. From one voxel past + /// `faceFront` out to `overheadDepth`, the ground was seen and every voxel from + /// `groundClearance` above it up to the height returned is free; above headroom the face + /// must also be seen at each height, so the near stretch the rays crossed goes up with it. + public func overheadReach(cell index: Int, along wall: WallFrame) -> Float? { + let facade = facadeOffset(cell: index, along: wall) + return isWallSeen(cell: index, along: wall, facade: facade) ? clearHeight(cell: index, along: wall, facade: facade) : nil + } + + /// The least, over the columns from one voxel past `faceFront` out to `overheadDepth`, of + /// the height seen clear above each column's own ground: every voxel from + /// `groundClearance` above it up to that height is free, and above headroom the facade face + /// is seen at that height too. + private func clearHeight(cell index: Int, along wall: WallFrame, facade: Float?) -> Float? { + guard let facade else { return nil } + let outs = Array(stride(from: nearStart(facade) + config.voxelSize / 2, through: config.overheadDepth, by: config.voxelSize / 2)) + var reach: Float = .infinity + for s in samples(in: cellRange(index)) { + var faceAbove: [Float: Bool] = [:] + for out in outs { + guard let ground = groundHeight(wall, s: s, out: out) else { return nil } + var column: Float? + for height in stride(from: ground + config.groundClearance, to: config.top - config.voxelSize / 2, by: config.voxelSize / 2) { + guard isFree(wall, s: s, height: height, out: out) else { break } + if height > config.headroom { + let face = faceAbove[height] ?? faceSample(wall, s: s, height: height, facade: facade).face + faceAbove[height] = face + guard face else { break } + } + column = height - ground + } + guard let column else { return nil } + reach = min(reach, column) + } + } + return reach.isFinite ? reach : nil + } + + /// The ground at a point was seen and the space above it from `groundClearance` up to + /// `height` is free. + private func isClear(_ wall: WallFrame, s: Float, out: Float, upTo height: Float) -> Bool { + guard let ground = groundHeight(wall, s: s, out: out) else { return false } + return stride(from: ground + config.groundClearance, through: height, by: config.voxelSize / 2).allSatisfy { + isFree(wall, s: s, height: $0, out: out) + } + } + + private func isFree(_ wall: WallFrame, s: Float, height: Float, out: Float) -> Bool { + coordinate(wall, s: s, height: height, out: out).map { grid.isMeasuredFree($0, config: config) } ?? false + } + + /// The largest distance, from `start` in steps of half a voxel up to `end`, up to which + /// `passes` holds at every step; nil when it fails at `start`. + private func contiguousReach(from start: Float, to end: Float, _ passes: (Float) -> Bool) -> Float? { + var reach: Float? + for distance in stride(from: start, through: end, by: config.voxelSize / 2) { + guard passes(distance) else { break } + reach = distance + } + return reach + } + + // MARK: Sampling + + /// s values across a cell at most half a voxel apart, the first and last a hair inside its + /// edges, so every voxel column the cell overlaps is sampled. + func samples(in range: ClosedRange) -> [Float] { + let inset: Float = 1e-4 + let low = range.lowerBound + inset + let high = range.upperBound - inset + let n = max(1, Int(((high - low) / (config.voxelSize / 2)).rounded(.up))) + return (0...n).map { low + Float($0) * (high - low) / Float(n) } + } + + /// The voxel at wall coordinates; nil outside the bounds. + func coordinate(_ wall: WallFrame, s: Float, height: Float, out: Float) -> SIMD3? { + grid.coordinate(of: frame.map(wall.world(s: s, height: height, out: out))) + } +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3DInputs.swift b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3DInputs.swift new file mode 100644 index 000000000..08c6002a2 --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/Map3DInputs.swift @@ -0,0 +1,165 @@ +import Foundation +import simd + +// What the 3D map is built from, as plain values: the app's ARKit adapter +// (HouseScan/Runtime/Map3DFeed.swift) fills them on the AR delegate queue. World coordinates +// are ARKit's gravity-aligned world in meters. + +/// A per-pixel depth image with the camera that took it, encoded as a capture packet's `depth` +/// (packet/README.md, "Depth alignment", on t3/packet). +/// +/// `camera` describes the depth image itself (`camera.imageSize` is (`width`, `height`)); build +/// it from the photo's camera with `init(photo:...)`. A value is meters along the camera's -z +/// axis, not along the pixel's ray, as ARKit's `sceneDepth` reports it, and 0 means no +/// measurement. Pixel (u, v) covers the square from (u, v) to (u + 1, v + 1), row `v` from the +/// top of the unrotated landscape image; its value is at index v * width + u. +/// +/// A packet's photo pose maps the camera into the meter frame: integrate it with a `Map3D` +/// whose `MapFrame` is the packet's `meter_anchor`, and `camera.cameraToWorld` = +/// `poseInWorld * pose`. +public struct DepthFrame: Sendable { + public enum Kind: Sendable { + /// LiDAR depth (ARKit `sceneDepth`), with ARKit's confidence per pixel when there is one: + /// 0 low, 1 medium, 2 high (`ARConfidenceLevel` raw values). Without it every pixel is + /// taken as high. + case lidar(confidence: [UInt8]?) + /// Depth estimated from the color image, for example by a monocular depth model, with one + /// standard deviation per pixel in meters. + case estimated(sigma: [Float]) + } + + public var camera: CameraFrame + public var width: Int + public var height: Int + /// Meters, finite and not negative; 0 where there is no measurement. + public var depth: [Float] + public var kind: Kind + + public init(camera: CameraFrame, width: Int, height: Int, depth: [Float], kind: Kind) { + precondition(width > 0 && height > 0 && depth.count == width * height, "depth has \(depth.count) values for \(width) x \(height)") + precondition(depth.allSatisfy { $0.isFinite && $0 >= 0 }, "depth holds a negative or non-finite value; 0 marks no measurement") + switch kind { + case .lidar(let confidence): + precondition(confidence.map { $0.count == depth.count } ?? true, "confidence has \(confidence?.count ?? 0) values for \(depth.count) depths") + case .estimated(let sigma): + precondition(sigma.count == depth.count, "sigma has \(sigma.count) values for \(depth.count) depths") + } + self.camera = camera + self.width = width + self.height = height + self.depth = depth + self.kind = kind + } + + /// A stored LiDAR depth image (a replay's, `DepthImage`) taken with `pose`'s camera. The image + /// keeps its own intrinsics and size; both types use z-depth and the same pixel convention, + /// so only millimeters become meters (0 stays no measurement). + public init(image: DepthImage, pose: CameraFrame) { + let camera = CameraFrame( + cameraToWorld: pose.cameraToWorld, intrinsics: image.intrinsics, + imageSize: SIMD2(Float(image.width), Float(image.height))) + self.init( + camera: camera, width: image.width, height: image.height, + depth: image.millimeters.map { Float($0) / 1000 }, kind: .lidar(confidence: image.confidence)) + } + + /// A depth image covering the same view as a photo taken by `photo`, at `width` x `height`. + /// Depth pixel (u, v) covers photo pixels from u W / w to (u + 1) W / w across and likewise + /// down, so the intrinsics scale by w / W and h / H. + public init(photo: CameraFrame, width: Int, height: Int, depth: [Float], kind: Kind) { + let scale = SIMD2(Float(width), Float(height)) / photo.imageSize + let k = photo.intrinsics + let camera = CameraFrame( + cameraToWorld: photo.cameraToWorld, intrinsics: SIMD4(k.x * scale.x, k.y * scale.y, k.z * scale.x, k.w * scale.y), + imageSize: SIMD2(Float(width), Float(height))) + self.init(camera: camera, width: width, height: height, depth: depth, kind: kind) + } +} + +/// ARKit's per-face mesh classification. Raw values are `ARMeshClassification`'s. +public enum MeshClass: UInt8, Sendable, CaseIterable { + case none = 0 + case wall = 1 + case floor = 2 + case ceiling = 3 + case table = 4 + case seat = 5 + case window = 6 + case door = 7 +} + +/// One chunk of ARKit's reconstructed mesh (an `ARMeshAnchor`), replaced whole when ARKit +/// updates it. Classes are the packet's `lidar/mesh.ply` `classification` values. A packet's +/// merged mesh, already in the meter frame, is one chunk with `worldFromChunk` = +/// `MapFrame.poseInWorld`. +public struct MeshChunk: Sendable { + public var id: UUID + /// Chunk-local to world (the anchor's transform). + public var worldFromChunk: simd_float4x4 + /// Chunk-local positions. + public var vertices: [SIMD3] + /// Three vertex indices per face. + public var faces: [SIMD3] + /// One per face, or empty when the session doesn't classify. + public var classes: [MeshClass] + + public init(id: UUID, worldFromChunk: simd_float4x4, vertices: [SIMD3], faces: [SIMD3], classes: [MeshClass] = []) { + precondition(classes.isEmpty || classes.count == faces.count, "\(classes.count) classes for \(faces.count) faces") + self.id = id + self.worldFromChunk = worldFromChunk + self.vertices = vertices + self.faces = faces + self.classes = classes + } +} + +/// A plane ARKit detected (an `ARPlaneAnchor`), replaced whole when ARKit updates it. +public struct PlaneObservation: Sendable { + public enum Alignment: Sendable { + case horizontal + case vertical + } + + public var id: UUID + /// Plane-local to world. The plane is local y = 0 with its normal along local +y. + public var worldFromPlane: simd_float4x4 + public var alignment: Alignment + /// The plane's extent as a polygon of local (x, z) points: ARKit's boundary vertices, or the + /// four corners of its extent rectangle. + public var boundary: [SIMD2] + + public init(id: UUID, worldFromPlane: simd_float4x4, alignment: Alignment, boundary: [SIMD2]) { + self.id = id + self.worldFromPlane = worldFromPlane + self.alignment = alignment + self.boundary = boundary + } + + /// A rectangular extent `width` along local x and `length` along local z, centred on local + /// `center` and turned by `rotationOnYAxis` radians (ARKit's `planeExtent`). + public init( + id: UUID, worldFromPlane: simd_float4x4, alignment: Alignment, + center: SIMD2, width: Float, length: Float, rotationOnYAxis: Float = 0 + ) { + let c = cos(rotationOnYAxis) + let s = sin(rotationOnYAxis) + let corners = [SIMD2(-width, -length), SIMD2(width, -length), SIMD2(width, length), SIMD2(-width, length)].map { half in + let p = half / 2 + // Rotation about +y by the angle, written in (x, z). + return center + SIMD2(c * p.x + s * p.y, -s * p.x + c * p.y) + } + self.init(id: id, worldFromPlane: worldFromPlane, alignment: alignment, boundary: corners) + } +} + +/// What a phone without LiDAR knows about one frame: its camera and the feature points ARKit +/// tracked in it (`ARFrame.rawFeaturePoints`, world meters). Planes arrive separately. +public struct FeatureFrame: Sendable { + public var camera: CameraFrame + public var points: [SIMD3] + + public init(camera: CameraFrame, points: [SIMD3]) { + self.camera = camera + self.points = points + } +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Map3D/MapFrame.swift b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/MapFrame.swift new file mode 100644 index 000000000..0f57342b6 --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/MapFrame.swift @@ -0,0 +1,112 @@ +import Foundation +import simd + +/// The frame the 3D map is stored in: the capture packet's meter frame (packet/README.md, "Units, +/// frames and clocks", on t3/packet). +/// +/// Origin at the meter anchor, the point tapped on the meter. +y is up, +z is the wall's +/// outward normal, horizontal, toward the homeowner, and +x = y × z runs along the wall to the +/// right as seen facing it (scene.json's +s). ARKit runs with `.gravity` alignment, so the frame +/// differs from ARKit's world only by a turn about +y and a shift. `groundY` is the height of the +/// ground at the meter in this frame (negative), the packet's `ground_y_m`. Anything stored in +/// the frame moves with the meter anchor when ARKit refines it +/// (`following(anchorMovedFrom:to:)`). +public struct MapFrame: Sendable, Equatable { + /// Map point to ARKit world point: the packet's `session.meter_anchor.pose_in_world`. + public private(set) var poseInWorld: simd_float4x4 + public private(set) var mapFromWorld: simd_float4x4 + /// Height of the ground at the meter, map frame, meters. + public let groundY: Float + + /// `meter` and `worldGroundY` in ARKit world. Nil when `outward` has no horizontal part (a + /// floor or ceiling hit). + public init?(meter: SIMD3, outward: SIMD3, worldGroundY: Float) { + let flat = SIMD3(outward.x, 0, outward.z) + guard simd_length(flat) > 1e-3 else { return nil } + let z = simd_normalize(flat) + let x = simd_normalize(simd_cross(SIMD3(0, 1, 0), z)) + self.init( + poseInWorld: simd_float4x4(SIMD4(x, 0), SIMD4(0, 1, 0, 0), SIMD4(z, 0), SIMD4(meter, 1)), + groundY: worldGroundY - meter.y) + } + + /// The meter's wall: origin at `wall.meter`, +x along and +z outward of the meter's piece. + public init(wall: WallFrame) { + // A WallFrame's outward is unit horizontal by construction, so this cannot fail. + self.init(meter: wall.meter, outward: wall.outward, worldGroundY: wall.groundY)! + } + + /// A packet's `session.meter_anchor`: `poseInWorld` must be a rotation about +y and a shift. + public init(poseInWorld: simd_float4x4, groundY: Float) { + let up = SIMD3(poseInWorld.columns.1.x, poseInWorld.columns.1.y, poseInWorld.columns.1.z) + precondition(abs(up.y - 1) < 1e-3, "meter frame \(poseInWorld) is not gravity-aligned: its +y is \(up)") + self.poseInWorld = poseInWorld + mapFromWorld = poseInWorld.inverse + self.groundY = groundY + } + + public func map(_ world: SIMD3) -> SIMD3 { + let p = mapFromWorld * SIMD4(world, 1) + return SIMD3(p.x, p.y, p.z) + } + + public func world(_ map: SIMD3) -> SIMD3 { + let p = poseInWorld * SIMD4(map, 1) + return SIMD3(p.x, p.y, p.z) + } + + /// A world direction in map axes. + public func mapDirection(_ world: SIMD3) -> SIMD3 { + let p = mapFromWorld * SIMD4(world, 0) + return SIMD3(p.x, p.y, p.z) + } + + public func worldDirection(_ map: SIMD3) -> SIMD3 { + let p = poseInWorld * SIMD4(map, 0) + return SIMD3(p.x, p.y, p.z) + } + + /// This frame moved as the meter's ARAnchor moved from `old` to `new` (both anchor-to-world), + /// so a map point keeps its place relative to the anchor. Any tilt in the anchor's change is + /// dropped: the map stays gravity-aligned, and ARKit's `.gravity` alignment keeps anchor + /// updates level anyway. + public func following(anchorMovedFrom old: simd_float4x4, to new: simd_float4x4) -> MapFrame { + let moved = new * old.inverse * poseInWorld + let x = SIMD3(moved.columns.0.x, 0, moved.columns.0.z) + guard simd_length(x) > 1e-3 else { return self } + let along = simd_normalize(x) + let z = simd_normalize(simd_cross(along, SIMD3(0, 1, 0))) + let origin = moved.columns.3 + return MapFrame( + poseInWorld: simd_float4x4(SIMD4(along, 0), SIMD4(0, 1, 0, 0), SIMD4(z, 0), SIMD4(origin.x, origin.y, origin.z, 1)), + groundY: groundY) + } +} + +/// An axis-aligned box of the map frame, meters. +public struct MapBounds: Sendable, Equatable { + public var min: SIMD3 + public var max: SIMD3 + + public init(min: SIMD3, max: SIMD3) { + precondition(simd_reduce_min(max - min) > 0, "map bounds \(min)...\(max) are empty") + self.min = min + self.max = max + } + + public var size: SIMD3 { max - min } + + public func contains(_ p: SIMD3) -> Bool { + p.x >= min.x && p.y >= min.y && p.z >= min.z && p.x < max.x && p.y < max.y && p.z < max.z + } + + /// Everything the rules can ask about around the meter. The region of interest + /// (`Map3DConfig`) runs `alongExtent` along the wall chain either way from the meter and + /// `outDepth` out from it; a chain can turn at corners, including back behind the meter's + /// wall, so in plan it stays within `alongExtent + outDepth` of the meter in any direction. + /// Heights run from `groundBelow` under the meter's ground to `top` above it. + public static func around(_ config: Map3DConfig, groundY: Float) -> MapBounds { + let r = config.alongExtent + config.outDepth + return MapBounds(min: SIMD3(-r, groundY - config.groundBelow, -r), max: SIMD3(r, groundY + config.top, r)) + } +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Map3D/MeasuredChainAdmission.swift b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/MeasuredChainAdmission.swift new file mode 100644 index 000000000..f8164d514 --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/MeasuredChainAdmission.swift @@ -0,0 +1,99 @@ +import Foundation +import simd + +/// Whether scene.json may describe the 3D map's measured wall chain rather than the walk's +/// tapped wall: only where the two agree. +public enum MeasuredChainAdmission { + /// How far the measured chain may stop short of each end of the walk's baseline and still + /// describe it, meters: two 10 cm voxels. A measured piece ends within half a voxel of its + /// last column of wall evidence, and a marked end is a tap. A guess, not measured. + public static let chainShortfall: Float = 0.2 + /// How far the tapped meter may lie from the line of the measured meter piece, meters. A + /// parallel surface near the meter (a fence, a hedge face, the meter box's own front) faces + /// the same way as the wall and passes the direction check; this keeps it from standing in + /// for the wall. The meter box stands roughly 0.1 to 0.2 m proud of the wall. A guess, not + /// measured. + public static let meterLineDistance: Float = 0.3 + /// How far a measured corner within the baseline may lie from the tapped chain, meters. The + /// homeowner's taps say where the walk's wall runs; a measured corner far from it belongs to + /// something else. A guess, not measured: larger than tap error, smaller than a step. + public static let cornerDistance: Float = 0.5 + + /// Why the measured chain was not admitted. + public struct Refusal: Error, Equatable { + public var reason: String + } + + /// The baseline in `measured`'s s when the chain (laid out as `measured`, + /// `MeasuredWallChain.wallFrame`) may stand for the tapped wall: its meter piece faces within + /// `WallFrame.minCornerAngle` of the tapped wall, the meter's foot falls on it within + /// `meterLineDistance` of its line, every measured corner within the baseline lies within + /// `cornerDistance` of the tapped chain and every tapped corner and baseline end within + /// `cornerDistance` of the measured chain, and the chain reaches both ends of the baseline to + /// within `chainShortfall`. `frame` is the map's frame; `baselineS` is in the tapped wall's s. + public static func admit( + _ chain: MeasuredWallChain, as measured: WallFrame, frame: MapFrame, tapWall: WallFrame, baselineS: ClosedRange + ) -> Result, Refusal> { + guard chain.meterIndex < chain.walls.count, measured.segments.count == chain.walls.count else { + return .failure(Refusal(reason: "no measured wall passes the meter")) + } + let meterPiece = chain.walls[chain.meterIndex] + let outward = frame.worldDirection(SIMD3(meterPiece.outward.x, 0, meterPiece.outward.y)) + let flat = simd_normalize(SIMD3(outward.x, 0, outward.z)) + guard simd_dot(flat, tapWall.outward) >= cos(WallFrame.minCornerAngle) else { + return .failure(Refusal(reason: "the measured meter piece turns more than 30 degrees from the tapped wall")) + } + let meterMap = frame.map(tapWall.meter) + let meterPlan = SIMD2(meterMap.x, meterMap.z) + // The exported wall's origin is the meter's foot on this piece (`wallFrame`), which the + // server must find again by projecting the meter; a foot off the piece would be clamped. + let footT = simd_dot(meterPlan - meterPiece.start, meterPiece.along) + let inset = min(0.01, meterPiece.length / 2) + guard footT >= inset, footT <= meterPiece.length - inset else { + return .failure(Refusal(reason: "the meter's foot falls off the measured meter piece")) + } + let meterOff = abs(simd_dot(meterPlan - meterPiece.start, meterPiece.outward)) + guard meterOff <= meterLineDistance else { + return .failure(Refusal(reason: "the meter is \(meterOff) m from the measured meter piece's line")) + } + + // The baseline's ends are places on the tapped wall; the same places on the measured one. + let low = tapWall.s(baselineS.lowerBound, along: measured) + let high = tapWall.s(baselineS.upperBound, along: measured) + guard low < 0, high > 0 else { return .failure(Refusal(reason: "the baseline's ends fall on one side of the meter on the measured wall")) } + + for corner in measured.leftCorners + measured.rightCorners where corner.s > low && corner.s < high { + let point = measured.world(s: corner.s, height: 0) + let distance = planDistance(point, tapWall.world(s: tapWall.wallPoint(point).s, height: 0)) + guard distance <= cornerDistance else { + return .failure(Refusal(reason: "the measured corner at s=\(corner.s) m is \(distance) m from the tapped wall")) + } + } + + // And the reverse: every tapped corner within the baseline, and both of the baseline's + // ends, lie within `cornerDistance` of the measured chain, measured in plan. Projecting + // onto the measured chain alone ignores how far off it a point lies: a walk that turned a + // corner the map didn't find would map its side wall onto the front wall's line. + let tappedPlaces = (tapWall.leftCorners + tapWall.rightCorners).map(\.s).filter { baselineS.contains($0) } + + [baselineS.lowerBound, baselineS.upperBound] + for s in tappedPlaces { + let point = tapWall.world(s: s, height: 0) + let distance = planDistance(point, measured.world(s: measured.wallPoint(point).s, height: 0)) + guard distance <= cornerDistance else { + return .failure(Refusal(reason: "the tapped wall at s=\(s) m is \(distance) m from the measured chain")) + } + } + + // The chain's own ends in s, as `MeasuredWallChain.wallFrame` lays it out from the meter. + let leftEnd = -footT - chain.walls.prefix(chain.meterIndex).reduce(0) { $0 + $1.length } + let rightEnd = meterPiece.length - footT + chain.walls.suffix(from: chain.meterIndex + 1).reduce(0) { $0 + $1.length } + guard leftEnd <= low + chainShortfall, rightEnd >= high - chainShortfall else { + return .failure(Refusal(reason: "the measured chain spans s=\(leftEnd)...\(rightEnd) m, short of the baseline \(low)...\(high) m")) + } + return .success(low...high) + } + + static func planDistance(_ a: SIMD3, _ b: SIMD3) -> Float { + simd_distance(SIMD2(a.x, a.z), SIMD2(b.x, b.z)) + } +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Map3D/MeasuredWalls.swift b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/MeasuredWalls.swift new file mode 100644 index 000000000..564c76118 --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/MeasuredWalls.swift @@ -0,0 +1,471 @@ +import Foundation +import simd + +/// One straight piece of wall found in the map. Points are plan (x, z) in the map frame. +public struct MeasuredWall: Sendable, Equatable { + /// Ends of the piece, left to right as seen from outside. + public var start: SIMD2 + public var end: SIMD2 + /// Unit, from the wall toward the outside. + public var outward: SIMD2 + /// What it was found from: `.mesh` for LiDAR depth rays or ARKit's mesh, `.plane` for + /// planes ARKit detected on a phone without LiDAR. Never `.tap`. + public var source: WallLineSource + /// Plan cells of vertical surface that support it. + public var support: Int + /// How far the true wall line may lie from this one anywhere along the piece, meters: two + /// standard errors of the fit at the piece's worse end, and never less than half a voxel. + /// scene.json's `plus_minus_ft` for the wall. + public var plusMinus: Float + + public var length: Float { simd_distance(start, end) } + /// Unit, from `start` to `end`: the outward turned 90 degrees, as `WallFrame.along`. + public var along: SIMD2 { SIMD2(outward.y, -outward.x) } +} + +/// The house outline near the meter: pieces of wall found in the map, joined into one chain +/// through the meter's piece. Consecutive pieces share an end, which is where their lines +/// cross (a corner) or the far end of a gap bridged along one line. +public struct MeasuredWallChain: Sendable, Equatable { + /// Left to right as seen from outside. + public var walls: [MeasuredWall] + /// Index of the piece the meter is on. + public var meterIndex: Int + + /// The chain's corners and ends, plan (x, z) in the map frame, left to right. + public var vertices: [SIMD2] { + guard let first = walls.first else { return [] } + return [first.start] + walls.map(\.end) + } + + /// The chain as a `WallFrame` for coverage and scene.json. Its meter piece lies on the + /// measured piece's fitted line, and s = 0 is the meter's foot on that line, at the meter's + /// height: a meter tapped on the face of a box standing proud of the wall leaves the wall + /// where it was measured, so the piece's `plusMinus` still describes it, and the box's + /// offset stays with the meter (`wallPoint(meter).out`). Each measured corner is a turn at + /// the same distance along the chain from the foot, carrying its piece's source; the meter + /// piece's source is the frame's `source`. `meter` is in world, `frame` is the map's frame. + /// Nil when the chain is empty. + public func wallFrame(meter: SIMD3, groundY: Float, frame: MapFrame) -> WallFrame? { + guard meterIndex < walls.count else { return nil } + func worldOutward(_ wall: MeasuredWall) -> SIMD3 { + let d = frame.worldDirection(SIMD3(wall.outward.x, 0, wall.outward.y)) + return simd_normalize(SIMD3(d.x, 0, d.z)) + } + let meterMap = frame.map(meter) + let meterPiece = walls[meterIndex] + // s of the meter's foot on its piece, from the piece's start, kept a centimeter inside + // it so the first corner either way lies beyond the meter as `WallFrame.turn` requires. + let inset = min(0.01, meterPiece.length / 2) + let meterS = min(max(simd_dot(SIMD2(meterMap.x, meterMap.z) - meterPiece.start, meterPiece.along), inset), meterPiece.length - inset) + let foot = meterPiece.start + meterPiece.along * meterS + guard var result = WallFrame( + meter: frame.world(SIMD3(foot.x, meterMap.y, foot.y)), outward: worldOutward(meterPiece), groundY: groundY + ) else { return nil } + result.source = meterPiece.source + var s = meterPiece.length - meterS + for index in walls.indices.dropFirst(meterIndex + 1) { + result.turn(.right, at: WallCorner(s: s, outward: worldOutward(walls[index]), source: walls[index].source)) + s += walls[index].length + } + s = -meterS + for index in walls.indices.prefix(meterIndex).reversed() { + result.turn(.left, at: WallCorner(s: s, outward: worldOutward(walls[index]), source: walls[index].source)) + s -= walls[index].length + } + return result + } +} + +extension Map3D { + /// Walls found in the map, joined into a chain through the meter (map origin). Nil when no + /// piece passes within `cornerJoinDistance` of the meter. + /// + /// A plan cell is wall evidence where its voxels between `wallBottom` and `top` that are + /// surface (or carry a mesh face or a detected plane), have a normal within 30 degrees of horizontal, and are not + /// classified floor, ceiling, table or seat add up to `minWallHeight`. Feature points and + /// estimated depth never make walls: they are too sparse or too uncertain for a wall line. + /// Lines are then taken one at a time, the one with most evidence first, each fitted by + /// least squares to the cells near it that face its way, and split where the evidence + /// breaks for more than `maxWallGap`. + public func measuredWalls() -> MeasuredWallChain? { + chain(wallPieces().filter { !isRelief($0, among: wallPieces()) }) + } + + /// Whether a piece is relief on a longer wall rather than a wall of its own: at most + /// `maxReliefWidth` long and lying wholly in front of another piece at least twice as long, + /// within `reliefDepth` of its line and alongside it. A pilaster's front and its sides are; + /// a bay's sides (0.7 m deep) and a corner's next wall are not. + func isRelief(_ piece: MeasuredWall, among pieces: [MeasuredWall]) -> Bool { + guard piece.length <= config.maxReliefWidth else { return false } + return pieces.contains { wall in + guard wall.length >= 2 * piece.length else { return false } + return [piece.start, piece.end].allSatisfy { p in + let out = simd_dot(p - wall.start, wall.outward) + let along = simd_dot(p - wall.start, wall.along) + return out >= -config.faceBehind && out <= config.reliefDepth + && along >= -config.maxReliefWidth && along <= wall.length + config.maxReliefWidth + } + } + } + + /// Every piece of wall found, unchained. + public func wallPieces() -> [MeasuredWall] { + var cells = wallEvidence() + var pieces: [MeasuredWall] = [] + let minCells = max(2, Int((config.minWallLength / config.voxelSize).rounded(.up))) + let cosTolerance = cos(config.wallNormalTolerance) + for _ in 0..<64 { + guard cells.count >= minCells else { break } + // The line through some cell, along its normal, with the most evidence near it. + let stride = max(1, cells.count / 800) + var best: (weight: Int, point: SIMD2, normal: SIMD2)? + for i in Swift.stride(from: 0, to: cells.count, by: stride) { + let candidate = cells[i] + var weight = 0 + for cell in cells where isInlier(cell, point: candidate.point, normal: candidate.normal, cosTolerance: cosTolerance) { + weight += cell.count + } + if weight > best?.weight ?? 0 { best = (weight, candidate.point, candidate.normal) } + } + guard let seed = best else { break } + let line = robustLine(through: seed, cells: cells, cosTolerance: cosTolerance) + let inliers = cells.filter { isInlier($0, point: line.point, normal: line.normal, cosTolerance: cosTolerance) } + guard inliers.count >= minCells else { break } + cells.removeAll { isInlier($0, point: line.point, normal: line.normal, cosTolerance: cosTolerance) } + pieces += split(inliers, line: line) + } + return pieces + } + + // MARK: Evidence + + struct WallCell { + var point: SIMD2 + var normal: SIMD2 + /// Voxels of vertical surface in the cell's column. + var count: Int + var measured: Int + var planes: Int + } + + func wallEvidence() -> [WallCell] { + struct Column { + var count = 0 + var normal = SIMD2.zero + var measured = 0 + var planes = 0 + } + let excluded: Set = Set([MeshClass.floor, .ceiling, .table, .seat].map { $0.rawValue + 1 }) + let wallSources = VoxelSources([.lidar, .mesh, .plane]).rawValue + let maxVertical = sin(Float.pi / 6) + var columns: [SIMD2: Column] = [:] + grid.forEachStored { g, voxel in + guard voxel.sources & wallSources != 0, !excluded.contains(voxel.meshLabel) else { return } + let height = grid.center(of: g).y - frame.groundY + guard height >= config.wallBottom, height <= config.top else { return } + guard voxel.state(config) == .surface || voxel.sources & (VoxelSources.mesh.rawValue | VoxelSources.plane.rawValue) != 0 else { return } + guard let n = voxel.normal, abs(n.y) <= maxVertical else { return } + let plan = simd_normalize(SIMD2(n.x, n.z)) + let key = SIMD2(g.x, g.z) + var column = columns[key] ?? Column() + // Normals of one wall agree; a neighbour turned the other way is flipped to match. + column.normal += simd_dot(column.normal, plan) < 0 ? -plan : plan + column.count += 1 + if voxel.sources & (VoxelSources.lidar.rawValue | VoxelSources.mesh.rawValue) != 0 { column.measured += 1 } + if voxel.sources & VoxelSources.plane.rawValue != 0 { column.planes += 1 } + columns[key] = column + } + let minCount = Int((config.minWallHeight / config.voxelSize).rounded(.up)) + return columns.compactMap { key, column in + guard column.count >= minCount, simd_length(column.normal) > 0 else { return nil } + let center = grid.center(of: SIMD3(key.x, 0, key.y)) + return WallCell( + point: SIMD2(center.x, center.z), normal: simd_normalize(column.normal), count: column.count, + measured: column.measured, planes: column.planes) + } + .sorted { ($0.point.x, $0.point.y) < ($1.point.x, $1.point.y) } + } + + private func isInlier(_ cell: WallCell, point: SIMD2, normal: SIMD2, cosTolerance: Float, within distance: Float? = nil) -> Bool { + abs(simd_dot(cell.point - point, normal)) <= distance ?? config.wallInlierDistance && abs(simd_dot(cell.normal, normal)) >= cosTolerance + } + + /// The wall's line near a seed, on its dominant face. Relief standing proud of the face, + /// such as cladding or a sill along part of the wall, must stay out of the fit: a + /// least-squares line through it tilts toward wherever the relief is, and refitting from a + /// seed a degree off can settle on face at one end and relief at the other (ETH3D electro's + /// wall came out 1.76 degrees and 4.8 in off that way). So the line is found globally first: + /// every angle within 8 degrees of the seed's normal, in 0.2 degree steps, and at each the + /// offset whose band of `searchBand` either side holds the most evidence, ties going to the + /// angle the band's cells scatter least about. That band is + /// narrower than the 10 cm between voxel layers, so no line scores by taking in the face at + /// one end and relief a layer out at the other. Least squares on the cells within + /// `faceBand` of that line then refines it until those cells stop changing. + private func robustLine(through seed: (weight: Int, point: SIMD2, normal: SIMD2), cells: [WallCell], cosTolerance: Float) -> (point: SIMD2, normal: SIMD2) { + // Cells that could belong to the line at any angle swept: 8 degrees over 4.5 m is 0.6 m. + let reach: Float = 0.6 + let candidates = cells.filter { abs(simd_dot($0.point - seed.point, seed.normal)) <= reach && abs(simd_dot($0.normal, seed.normal)) >= cosTolerance } + let base = atan2(seed.normal.y, seed.normal.x) + var best: (weight: Int, spread: Float, normal: SIMD2, offset: Float)? + for step in -40...40 { + let angle = base + Float(step) * 0.2 * .pi / 180 + let normal = SIMD2(cos(angle), sin(angle)) + let offsets = candidates.map { (offset: simd_dot($0.point - seed.point, normal), weight: $0.count) }.sorted { $0.offset < $1.offset } + var low = 0 + var weight = 0 + var window: (weight: Int, low: Int, high: Int) = (0, 0, -1) + for high in offsets.indices { + weight += offsets[high].weight + while offsets[high].offset - offsets[low].offset > 2 * searchBand { + weight -= offsets[low].weight + low += 1 + } + if weight > window.weight { window = (weight, low, high) } + } + guard window.high >= window.low else { continue } + let slice = offsets[window.low...window.high] + let offset = slice.reduce(Float(0)) { $0 + $1.offset * Float($1.weight) } / Float(window.weight) + let spread = slice.reduce(Float(0)) { $0 + ($1.offset - offset) * ($1.offset - offset) * Float($1.weight) } / Float(window.weight) + // Several angles can hold the same cells; the one they scatter least about is square to the face. + if window.weight > best?.weight ?? 0 || (window.weight == best?.weight && spread < best?.spread ?? .infinity) { + best = (window.weight, spread, normal, offset) + } + } + guard let start = best else { return (seed.point, seed.normal) } + var line = (point: seed.point + start.normal * start.offset, normal: start.normal) + var previous: Set = [] + for _ in 0..<8 { + let face = cells.indices.filter { isInlier(cells[$0], point: line.point, normal: line.normal, cosTolerance: cosTolerance, within: faceBand) } + let members = Set(face) + guard face.count >= 2, members != previous else { break } + previous = members + let faceCells = face.map { cells[$0] } + line = fit(faceCells, weights: faceCells.map { Float($0.count) }, normal: line.normal) + } + return line + } + + /// Cells this close to a line are its face, meters: a plan cell's center lies at most half + /// a voxel's diagonal (7.1 cm) from a line through it, so this keeps the staircase a + /// diagonal wall makes on the grid, with room for noise, and leaves out the next layer of + /// cells a voxel (10 cm) out. + private var faceBand: Float { config.voxelSize * 0.9 } + + /// Half the band the angle search scores, meters: under half the spacing of voxel layers. + private var searchBand: Float { config.voxelSize * 0.45 } + + /// Two standard errors of a fitted line's position at the worse end of a run, meters, from + /// the face cells' scatter about it: offset error sigma / sqrt(n), plus the angle's + /// sigma / sqrt(sum of squared distances along it) times the distance to the end. Never + /// less than half a voxel, the resolution the cells give. + private func positionError(_ face: [(t: Float, offset: Float)], ends: (Float, Float)) -> Float { + let floor = config.voxelSize / 2 + guard face.count >= 3 else { return max(floor, config.wallInlierDistance) } + let n = Float(face.count) + let meanT = face.reduce(0) { $0 + $1.t } / n + let meanOffset = face.reduce(0) { $0 + $1.offset } / n + let variance = face.reduce(0) { $0 + ($1.offset - meanOffset) * ($1.offset - meanOffset) } / (n - 2) + let spread = face.reduce(0) { $0 + ($1.t - meanT) * ($1.t - meanT) } + let far = max(abs(ends.0 - meanT), abs(ends.1 - meanT)) + let standardError = (variance / n + (spread > 0 ? variance / spread * far * far : 0)).squareRoot() + return max(floor, 2 * standardError) + } + + /// Weighted total least squares: the line through the cells' weighted centroid along their + /// principal axis, its normal turned to agree with `normal`. + private func fit(_ cells: [WallCell], weights: [Float], normal: SIMD2) -> (point: SIMD2, normal: SIMD2) { + let total = weights.reduce(0, +) + guard total > 0 else { return (.zero, normal) } + let centroid = zip(cells, weights).reduce(SIMD2.zero) { $0 + $1.0.point * $1.1 } / total + var xx: Float = 0 + var xz: Float = 0 + var zz: Float = 0 + for (cell, w) in zip(cells, weights) { + let d = cell.point - centroid + xx += w * d.x * d.x + xz += w * d.x * d.y + zz += w * d.y * d.y + } + let angle = 0.5 * atan2(2 * xz, xx - zz) + let direction = SIMD2(cos(angle), sin(angle)) + var fitted = SIMD2(-direction.y, direction.x) + if simd_dot(fitted, normal) < 0 { fitted = -fitted } + return (centroid, fitted) + } + + /// The runs of a line's cells without a gap longer than `maxWallGap`, each at least + /// `minWallLength` long, facing the side of the line that was seen free. + private func split(_ cells: [WallCell], line: (point: SIMD2, normal: SIMD2)) -> [MeasuredWall] { + var outward = line.normal + let lineDirection = SIMD2(outward.y, -outward.x) + let sorted = cells.map { (t: simd_dot($0.point - line.point, lineDirection), cell: $0) }.sorted { $0.t < $1.t } + var runs: [[(t: Float, cell: WallCell)]] = [] + for item in sorted { + if let last = runs.last?.last, item.t - last.t - config.voxelSize <= config.maxWallGap { + runs[runs.count - 1].append(item) + } else { + runs.append([item]) + } + } + // The outward side is the one seen free, 0.3 m off the line and 1 m above the ground. + // Depth normals already face the camera; mesh normals may not. + let middle = SIMD3(line.point.x, frame.groundY + 1, line.point.y) + let offset = SIMD3(outward.x, 0, outward.y) * 0.3 + if state(at: middle + offset) != .free, state(at: middle - offset) == .free { outward = -outward } + let rightward = SIMD2(outward.y, -outward.x) + return runs.compactMap { run in + guard let first = run.first, let last = run.last else { return nil } + let half = config.voxelSize / 2 + var a = line.point + lineDirection * (first.t - half) + var b = line.point + lineDirection * (last.t + half) + guard simd_distance(a, b) >= config.minWallLength else { return nil } + if simd_dot(b - a, rightward) < 0 { swap(&a, &b) } + let measured = run.reduce(0) { $0 + $1.cell.measured } + let planes = run.reduce(0) { $0 + $1.cell.planes } + let face = run.map { (t: $0.t, offset: simd_dot($0.cell.point - line.point, line.normal)) }.filter { abs($0.offset) <= faceBand } + return MeasuredWall( + start: a, end: b, outward: outward, source: measured >= planes ? .mesh : .plane, support: run.count, + plusMinus: positionError(face, ends: (first.t - half, last.t + half))) + } + } + + // MARK: Chain + + /// Joins pieces into one chain from the meter's piece outward on each side, until no piece + /// joins or the chain runs `alongExtent` from the meter, where it is cut. + func chain(_ pieces: [MeasuredWall]) -> MeasuredWallChain? { + let origin = SIMD2.zero + func distance(_ wall: MeasuredWall) -> Float { + let t = min(max(simd_dot(origin - wall.start, wall.along), 0), wall.length) + return simd_distance(origin, wall.start + wall.along * t) + } + guard let meterIndex = pieces.indices.min(by: { distance(pieces[$0]) < distance(pieces[$1]) }), + distance(pieces[meterIndex]) <= config.cornerJoinDistance else { return nil } + var used: Set = [meterIndex] + var meterPiece = pieces[meterIndex] + var right: [MeasuredWall] = [] + var left: [MeasuredWall] = [] + for side in [WalkSide.right, .left] { + var current = meterPiece + var joined: [MeasuredWall] = [] + while let (index, joint) = nextJoin(from: current, side: side, pieces: pieces, used: used) { + used.insert(index) + var next = pieces[index] + switch (joint, side) { + case (.straight, .right): + current.plusMinus = max(current.plusMinus, Self.straightJoinError(current, next)) + current.end = current.start + current.along * simd_dot(next.end - current.start, current.along) + current.support += next.support + continue + case (.straight, .left): + current.plusMinus = max(current.plusMinus, Self.straightJoinError(current, next)) + current.start = current.end - current.along * simd_dot(current.end - next.start, current.along) + current.support += next.support + continue + case (.corner(let point), .right): + current.end = point + next.start = point + case (.corner(let point), .left): + current.start = point + next.end = point + } + if joined.isEmpty { meterPiece = current } else { joined[joined.count - 1] = current } + joined.append(next) + current = next + } + if joined.isEmpty { meterPiece = current } else { joined[joined.count - 1] = current } + if side == .right { right = joined } else { left = joined } + } + var walls = left.reversed() + [meterPiece] + right + var meter = left.count + // Cut at alongExtent from the meter's foot along the chain, each side. + let limit = config.alongExtent + let meterT = min(max(simd_dot(origin - meterPiece.start, meterPiece.along), 0), meterPiece.length) + var reach = meterPiece.length - meterT + if reach > limit { walls[meter].end = meterPiece.start + meterPiece.along * (meterT + limit) } + var index = meter + 1 + while index < walls.count { + guard reach < limit else { + walls.removeSubrange(index...) + break + } + if reach + walls[index].length > limit { + walls[index].end = walls[index].start + walls[index].along * (limit - reach) + } + reach += walls[index].length + index += 1 + } + reach = meterT + if reach > limit { walls[meter].start = meterPiece.start + meterPiece.along * (meterT - limit) } + index = meter - 1 + while index >= 0 { + guard reach < limit else { + walls.removeSubrange(...index) + meter -= index + 1 + break + } + if reach + walls[index].length > limit { + walls[index].start = walls[index].end - walls[index].along * (limit - reach) + } + reach += walls[index].length + index -= 1 + } + return MeasuredWallChain(walls: walls, meterIndex: meter) + } + + /// The error `kept`'s line takes on when `joined` is joined to it straight: the joined + /// piece's own error plus how far its ends lie off the kept line. A straight join accepts a + /// piece up to 2 `wallInlierDistance` to the side, and the server takes an exported + /// `plus_minus_ft` instead of its default and drift (server/scene.py, t3/server 9edcd4b), so + /// the error must cover the piece the line now stands for. + static func straightJoinError(_ kept: MeasuredWall, _ joined: MeasuredWall) -> Float { + let offset = [joined.start, joined.end].map { abs(simd_dot($0 - kept.start, kept.outward)) }.max() ?? 0 + return offset + joined.plusMinus + } + + private enum Joint { + /// The next piece continues the same line across a gap. + case straight + /// The pieces' lines cross at this point. + case corner(SIMD2) + } + + /// The unused piece that best continues `current` past its end on `side`. + private func nextJoin(from current: MeasuredWall, side: WalkSide, pieces: [MeasuredWall], used: Set) -> (Int, Joint)? { + let end = side == .right ? current.end : current.start + let sign = side.sign + var best: (index: Int, joint: Joint, score: Float)? + for (index, piece) in pieces.enumerated() where !used.contains(index) { + let near = side == .right ? piece.start : piece.end + let cosine = simd_dot(current.along, piece.along) + let joint: Joint + let score: Float + if cosine >= cos(config.minCornerAngle) { + let offset = near - end + let gap = sign * simd_dot(offset, current.along) + let lateral = abs(simd_dot(offset, current.outward)) + guard lateral <= 2 * config.wallInlierDistance, gap >= -config.wallInlierDistance, gap <= config.maxWallBridge else { continue } + joint = .straight + score = max(gap, 0) + } else { + let cross = current.along.x * piece.along.y - current.along.y * piece.along.x + guard abs(cross) > 1e-4 else { continue } + let t = ((near.x - end.x) * piece.along.y - (near.y - end.y) * piece.along.x) / cross + let point = end + current.along * t + let reach = simd_distance(point, end) + simd_distance(point, near) + guard simd_distance(point, end) <= config.cornerJoinDistance, simd_distance(point, near) <= config.cornerJoinDistance else { continue } + // Moving both ends to the corner must leave each piece pointing the same way and + // at least half the shortest piece long; a piece is never turned round. + let far = side == .right ? piece.end : piece.start + let currentFar = side == .right ? current.start : current.end + let keep = config.minWallLength / 2 + guard sign * simd_dot(point - currentFar, current.along) >= keep, sign * simd_dot(far - point, piece.along) >= keep else { continue } + joint = .corner(point) + score = reach + } + if score < best?.score ?? .infinity { best = (index, joint, score) } + } + return best.map { ($0.index, $0.joint) } + } +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Map3D/MonocularDepth.swift b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/MonocularDepth.swift new file mode 100644 index 000000000..a96e67d23 --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/MonocularDepth.swift @@ -0,0 +1,407 @@ +import Foundation +import simd + +// Metric depth for a phone without LiDAR: a monocular model's relative depth, scaled per frame by +// points ARKit measured in that frame. The model alone gives depth only up to an unknown scale +// and shift; taking its scale from the model was 4 to 12% off (S1's measurement), so the scale +// comes only from ARKit's own geometry, and a frame without enough of it gets no depth. + +/// A monocular model's output for one camera image: inverse depth up to an unknown scale and +/// shift, larger nearer (Depth Anything V2's relative output). It covers the camera image's whole +/// view, stretched to the model's size: pixel (u, v) covers the image fractions u / width to +/// (u + 1) / width across and v / height to (v + 1) / height down, row `v` from the top of the +/// unrotated landscape image. +public struct RelativeInverseDepth: Sendable { + public let width: Int + public let height: Int + /// Row by row from the top. + public let values: [Float] + + public init(width: Int, height: Int, values: [Float]) { + precondition(width > 1 && height > 1 && values.count == width * height, "\(values.count) values for \(width) x \(height)") + self.width = width + self.height = height + self.values = values + } + + /// Bilinear between the four pixel centers around a point given as fractions of the image, + /// clamped at the edges; nil when any of the four is not finite. + public func value(at fraction: SIMD2) -> Float? { + let x = min(max(fraction.x * Float(width) - 0.5, 0), Float(width - 1)) + let y = min(max(fraction.y * Float(height) - 0.5, 0), Float(height - 1)) + let x0 = min(Int(x), width - 2) + let y0 = min(Int(y), height - 2) + let fx = x - Float(x0) + let fy = y - Float(y0) + let a = values[y0 * width + x0] + let b = values[y0 * width + x0 + 1] + let c = values[(y0 + 1) * width + x0] + let d = values[(y0 + 1) * width + x0 + 1] + let value = (a * (1 - fx) + b * fx) * (1 - fy) + (c * (1 - fx) + d * fx) * fy + return value.isFinite ? value : nil + } +} + +/// A point of the frame whose metric depth ARKit measured: a tracked feature point or a point on +/// a detected plane. +public struct DepthAnchor: Sendable, Equatable { + /// Where it lies in the image, as fractions of the image's width and height from its top-left + /// corner. + public var fraction: SIMD2 + /// Meters along the camera's -z. + public var depth: Float + + public init(fraction: SIMD2, depth: Float) { + self.fraction = fraction + self.depth = depth + } + + /// World points (ARKit's `rawFeaturePoints`) that lie in front of `camera`, inside its image + /// and between `minDepth` and `maxDepth`. + public static func anchors(points: [SIMD3], camera: CameraFrame, minDepth: Float = 0.2, maxDepth: Float = 8) -> [DepthAnchor] { + points.compactMap { point in + let depth = -camera.cameraSpace(point).z + guard depth >= minDepth, depth <= maxDepth, let pixel = camera.pixel(of: point), camera.contains(pixel: pixel) else { return nil } + return DepthAnchor(fraction: pixel / camera.imageSize, depth: depth) + } + } + + /// Where rays through a `columns` x `rows` grid of pixels first meet a detected plane inside + /// its extent, between `minDepth` and `maxDepth`, kept only where a feature point (world, + /// `confirmedBy`) within `onPlane` of that plane falls in the same or a neighbouring grid + /// cell of the image. A plane's extent can run behind something standing in front of it, + /// and the fit did not reliably reject such points: in the review of 2f17d67 a box's plane + /// anchors scaled the model's box into the wall behind it. A feature point on the plane + /// there shows the plane itself is in view. + public static func anchors( + planes: [PlaneObservation], confirmedBy points: [SIMD3], camera: CameraFrame, columns: Int = 16, rows: Int = 12, + minDepth: Float = 0.2, maxDepth: Float = 8, onPlane: Float = 0.05 + ) -> [DepthAnchor] { + let surfaces = planes.compactMap { plane -> (point: SIMD3, normal: SIMD3, planeFromWorld: simd_float4x4, boundary: [SIMD2])? in + guard plane.boundary.count >= 3 else { return nil } + let m = plane.worldFromPlane + let normal = SIMD3(m.columns.1.x, m.columns.1.y, m.columns.1.z) + guard simd_length_squared(normal) > 0 else { return nil } + return (SIMD3(m.columns.3.x, m.columns.3.y, m.columns.3.z), simd_normalize(normal), m.inverse, plane.boundary) + } + guard !surfaces.isEmpty, columns > 0, rows > 0 else { return [] } + let cellSize = SIMD2(1 / Float(columns), 1 / Float(rows)) + // Per plane, where in the image points lying on it were seen. + let confirmations = surfaces.map { surface in + points.compactMap { point -> SIMD2? in + guard abs(simd_dot(point - surface.point, surface.normal)) <= onPlane, -camera.cameraSpace(point).z > 0, + let pixel = camera.pixel(of: point), camera.contains(pixel: pixel) else { return nil } + return pixel / camera.imageSize + } + } + var anchors: [DepthAnchor] = [] + for row in 0..= minDepth, depth <= maxDepth else { continue } + anchors.append(DepthAnchor(fraction: fraction, depth: depth)) + } + } + return anchors + } +} + +/// Settings of the scale fit and the per-pixel uncertainty. Each value is a hypothesis unless its +/// comment says what measured it. +public struct MonocularDepthConfig: Sendable, Equatable { + /// A frame with fewer anchors than this gets no depth. Two points fix a scale and shift; 20 + /// leave enough over them for the residuals to say how good the fit is. + public var minAnchors = 20 + /// A relative depth residual within this makes an anchor an inlier of a candidate fit. + public var inlierResidual: Float = 0.1 + /// A fit must explain at least this share of the anchors, or the frame gets no depth: the + /// residual scale is a median over all anchors and means little once most are outliers. + public var minInlierShare: Float = 0.5 + /// Pairs of anchors tried as candidate fits; all pairs when there are fewer. + public var candidatePairs = 300 + /// A pixel takes the residuals of the anchors within this distance, as a fraction of the + /// image's width, as its local check on the model (about 5 degrees on an iPhone's wide + /// camera). One global fit says nothing about a part of the image no anchor fell on: on ETH3D + /// electro, ground with no anchor on it came out more than twice too far from a fit to a wall. + public var supportRadius: Float = 0.08 + /// A pixel with fewer anchors than this within `supportRadius` gets no depth. + public var minSupport = 3 + /// Least relative standard deviation of any pixel. The residual scales can come out small + /// when the anchors lie on one smooth surface the model got right. A guess, not measured. + public var relativeSigmaFloor: Float = 0.02 + /// Least standard deviation of any pixel, meters: about ARKit's plane and feature position + /// noise. A guess, not measured. + public var sigmaFloor: Float = 0.02 + /// Output pixels within this many of a pixel (a 5 x 5 window at 2) widen its deviation to + /// half the largest depth difference among them. Mono models blur depth across edges by a + /// pixel or two of their own, about one output pixel at 518 to 256: a pixel near an edge may + /// belong to either side. + public var edgeRadius = 2 + /// Depths more than this fraction beyond the nearest and farthest inlier anchors are not + /// estimated: the fit was only measured between them. + public var extrapolation: Float = 0.1 + /// Size of the depth frame built, ARKit's LiDAR depth size so Map3D treats both alike. + public var outputWidth = 256 + public var outputHeight = 192 + + public init() {} +} + +/// Inverse depth = `scale` * prediction + `shift`, fitted to one frame's anchors. +public struct InverseDepthFit: Sendable, Equatable { + public var scale: Float + public var shift: Float + public var anchorCount: Int + public var inlierCount: Int + /// Robust standard deviation of the anchors' relative depth residuals: 1.4826 times their + /// median absolute value (the normal distribution's MAD factor), times 1 + 5 / (n - 2), the + /// small-sample correction for a median-based scale of a two-parameter fit (Rousseeuw and + /// Leroy 1987). Taken over every anchor, outliers too, so it errs wide. + public var relativeSigma: Float + /// Covariance of (`scale`, `shift`) from the least-squares fit on the inliers, with + /// `relativeSigma` as the residual deviation: variance of scale, covariance, variance of shift. + public var covariance: SIMD3 + /// Nearest and farthest inlier anchor depths, meters. + public var anchorDepths: ClosedRange + /// Every anchor's place in the image and relative depth residual under the fit. + public var residuals: [Residual] + + public struct Residual: Sendable, Equatable { + public var fraction: SIMD2 + public var residual: Float + } + + /// Metric depth for a prediction, or nil where the fit gives none (at or past infinity). + public func depth(_ prediction: Float) -> Float? { + let inverse = scale * prediction + shift + return inverse > 0 ? 1 / inverse : nil + } + + /// Relative deviation of the depth at `prediction` that comes from the fit's own uncertainty: + /// depth times the deviation of scale * prediction + shift. It grows as a prediction lies + /// farther from the anchors' predictions, where the fit extrapolates. + public func parameterSigma(_ prediction: Float, depth: Float) -> Float { + let variance = covariance.x * prediction * prediction + 2 * covariance.y * prediction + covariance.z + return depth * max(variance, 0).squareRoot() + } +} + +public enum MonocularDepth { + /// The robust affine fit of 1 / depth to the prediction at the anchors, or nil when there + /// are fewer than `minAnchors`, too few inliers, or the fit makes nearer things farther. + /// + /// Residuals are relative depth errors, depth * (scale * p + shift) - 1, so a far anchor + /// counts as much as a near one. Candidate fits through pairs of anchors (RANSAC) pick the + /// inliers; least squares on the inliers then refines it, three times at most. + public static func fit(_ prediction: RelativeInverseDepth, anchors: [DepthAnchor], config: MonocularDepthConfig = MonocularDepthConfig()) -> InverseDepthFit? { + var samples: [(p: Double, z: Double, fraction: SIMD2)] = [] + samples.reserveCapacity(anchors.count) + for anchor in anchors where anchor.depth.isFinite && anchor.depth > 0 { + guard let p = prediction.value(at: anchor.fraction) else { continue } + samples.append((Double(p), Double(anchor.depth), anchor.fraction)) + } + let n = samples.count + guard n >= max(config.minAnchors, 3) else { return nil } + let threshold = Double(config.inlierResidual) + func residual(_ i: Int, _ a: Double, _ b: Double) -> Double { samples[i].z * (a * samples[i].p + b) - 1 } + func inliers(_ a: Double, _ b: Double) -> [Int] { samples.indices.filter { abs(residual($0, a, b)) <= threshold } } + + var best: (a: Double, b: Double, count: Int, cost: Double)? + func consider(_ i: Int, _ j: Int) { + let dp = samples[i].p - samples[j].p + guard abs(dp) > 1e-9 else { return } + let a = (1 / samples[i].z - 1 / samples[j].z) / dp + guard a > 0 else { return } + let b = 1 / samples[i].z - a * samples[i].p + var count = 0 + var cost = 0.0 + for k in 0..= current.cost) { return } + best = (a, b, count, cost) + } + if n * (n - 1) / 2 <= config.candidatePairs { + for i in 0.. simd_double2x2 { + var m = simd_double2x2() + for i in indices { + let row = SIMD2(samples[i].z * samples[i].p, samples[i].z) + m += simd_double2x2(rows: [row * row.x, row * row.y]) + } + return m + } + for _ in 0..<3 { + let m = normal(kept) + guard abs(m.determinant) > 1e-18 else { break } + var y = SIMD2.zero + for i in kept { y += SIMD2(samples[i].z * samples[i].p, samples[i].z) } + let solved = m.inverse * y + guard solved.x > 0 else { break } + let refined = inliers(solved.x, solved.y) + guard refined.count >= kept.count else { break } + (a, b) = (solved.x, solved.y) + let changed = refined != kept + kept = refined + if !changed { break } + } + let m = normal(kept) + guard a > 0, Double(kept.count) >= Double(config.minInlierShare) * Double(n), kept.count >= 3, abs(m.determinant) > 1e-18 else { return nil } + let residuals = (0.. DepthFrame? { + let w = config.outputWidth + let h = config.outputHeight + let near = fit.anchorDepths.lowerBound / (1 + config.extrapolation) + let far = fit.anchorDepths.upperBound * (1 + config.extrapolation) + // Depth everywhere the fit gives one, NaN elsewhere, before the range is applied: the + // edge term needs the depth on both sides of an edge. + var raw = [Float](repeating: .nan, count: w * h) + var predictions = [Float](repeating: .nan, count: w * h) + for v in 0..= near, z <= far, let localSigma = local[v * w + u] else { continue } + var jump: Float = 0 + for dv in -r...r { + let y = v + dv + guard y >= 0, y < h else { continue } + for du in -r...r { + let x = u + du + guard x >= 0, x < w else { continue } + let q = raw[y * w + x] + jump = max(jump, q.isFinite && q <= far ? abs(q - z) : farSigma * 2) + } + } + let modelSigma = max(model, localSigma) + let fitSigma = fit.parameterSigma(predictions[v * w + u], depth: z) + let relative = (modelSigma * modelSigma + fitSigma * fitSigma).squareRoot() + depth[v * w + u] = z + sigma[v * w + u] = max(z * relative, jump / 2, config.sigmaFloor) + any = true + } + } + guard any else { return nil } + return DepthFrame(photo: photo, width: w, height: h, depth: depth, kind: .estimated(sigma: sigma)) + } + + /// Per output pixel, the local relative scale of the model's error: 1.4826 times the median + /// absolute residual of the anchors within `supportRadius`, or nil with fewer than + /// `minSupport` of them. Computed per 4 x 4 block of pixels, at the block's center. + static func localScale(_ residuals: [InverseDepthFit.Residual], width: Int, height: Int, aspect: Float, config: MonocularDepthConfig) -> [Float?] { + let block = 4 + let columns = (width + block - 1) / block + let rows = (height + block - 1) / block + let radius2 = config.supportRadius * config.supportRadius + var cells = [Float?](repeating: nil, count: columns * rows) + var nearby: [Float] = [] + for row in 0..= max(1, config.minSupport) else { continue } + nearby.sort() + let k = nearby.count + let median = k % 2 == 1 ? nearby[k / 2] : (nearby[k / 2 - 1] + nearby[k / 2]) / 2 + cells[row * columns + column] = 1.4826 * median + } + } + return (0..<(width * height)).map { index in cells[(index / width / block) * columns + (index % width) / block] } + } + + /// Deviation for a pixel next to one with no depth, meters: beyond anything `Map3D` measures, + /// so it carves no free space and marks no surface. + public static let farSigma: Float = 100 + + /// The fit and depth frame for one camera image in one call; nil when the fit fails. + public static func estimate(_ prediction: RelativeInverseDepth, anchors: [DepthAnchor], photo: CameraFrame, config: MonocularDepthConfig = MonocularDepthConfig()) -> (fit: InverseDepthFit, frame: DepthFrame)? { + guard let fit = fit(prediction, anchors: anchors, config: config), let frame = depthFrame(prediction, fit: fit, photo: photo, config: config) else { return nil } + return (fit, frame) + } +} + +/// Steele, Lea and Flood's SplitMix64: a small deterministic generator. +struct SplitMix64: RandomNumberGenerator { + private var state: UInt64 + init(seed: UInt64) { state = seed } + mutating func next() -> UInt64 { + state &+= 0x9E37_79B9_7F4A_7C15 + var z = state + z = (z ^ (z >> 30)) &* 0xBF58_476D_1CE4_E5B9 + z = (z ^ (z >> 27)) &* 0x94D0_49BB_1331_11EB + return z ^ (z >> 31) + } +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Map3D/PendingWork.swift b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/PendingWork.swift new file mode 100644 index 000000000..97df3664b --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/PendingWork.swift @@ -0,0 +1,91 @@ +import Foundation + +/// Inputs waiting for one serial worker, at most `limit` of them: a new one pushes out the +/// oldest, so a worker slower than its inputs works on recent ones and its backlog, and the +/// memory the inputs hold, stays bounded. `DepthEstimator` queues kept frames for the depth +/// model through it. +public struct PendingWork: Sendable { + public let limit: Int + private var items: [Item] = [] + /// Inputs pushed out unworked. + public private(set) var dropped = 0 + + public init(limit: Int) { + precondition(limit >= 1, "a work queue needs room for one input, not \(limit)") + self.limit = limit + } + + public var count: Int { items.count } + + public mutating func add(_ item: Item) { + items.append(item) + if items.count > limit { + dropped += items.count - limit + items.removeFirst(items.count - limit) + } + } + + /// The oldest waiting input. + public mutating func take() -> Item? { + items.isEmpty ? nil : items.removeFirst() + } + + public mutating func removeAll() { + items.removeAll() + } +} + +/// Values kept by key within a total cost: setting a key makes it the newest, and the oldest go +/// once the total passes `budget`. A value costing more than the whole budget is not kept. +/// `Map3DSession` holds the mesh chunks and planes ARKit sends before the map starts in it. +public struct BoundedRecent: Sendable { + public let budget: Int + private let cost: @Sendable (Value) -> Int + private var entries: [Key: (value: Value, cost: Int)] = [:] + /// Oldest first. + private var order: [Key] = [] + public private(set) var total = 0 + /// Values pushed out to stay within the budget. + public private(set) var evicted = 0 + + public init(budget: Int, cost: @escaping @Sendable (Value) -> Int) { + self.budget = budget + self.cost = cost + } + + public var keys: [Key] { order } + public var values: [Value] { order.compactMap { entries[$0]?.value } } + public var isEmpty: Bool { order.isEmpty } + + public subscript(key: Key) -> Value? { + get { entries[key]?.value } + set { + if let old = entries.removeValue(forKey: key) { + total -= old.cost + order.removeAll { $0 == key } + } + guard let newValue else { return } + let price = cost(newValue) + guard price <= budget else { + evicted += 1 + return + } + entries[key] = (newValue, price) + order.append(key) + total += price + while total > budget, let oldest = order.first { + order.removeFirst() + if let gone = entries.removeValue(forKey: oldest) { total -= gone.cost } + evicted += 1 + } + } + } + + /// Empties it and restarts `evicted`. + public mutating func removeAll() { + entries.removeAll() + order.removeAll() + total = 0 + evicted = 0 + } +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Map3D/VoxelGrid.swift b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/VoxelGrid.swift new file mode 100644 index 000000000..66c87a022 --- /dev/null +++ b/ios/HouseScanKit/Sources/HouseScanKit/Map3D/VoxelGrid.swift @@ -0,0 +1,503 @@ +import Foundation +import simd + +/// What the map knows about a voxel. +public enum VoxelState: Sendable, Equatable { + /// No ray has reached it, or the evidence either way is too thin to say. + case unknown + /// Rays passed through it. + case free + /// Rays stopped in it: something is there. + case surface +} + +/// Where a voxel's evidence came from. A voxel can have several. +public struct VoxelSources: OptionSet, Sendable, Hashable { + public let rawValue: UInt8 + public init(rawValue: UInt8) { self.rawValue = rawValue } + + /// LiDAR depth rays. + public static let lidar = VoxelSources(rawValue: 1 << 0) + /// ARKit's reconstructed mesh (labels and shape only: a mesh face carries no camera). + public static let mesh = VoxelSources(rawValue: 1 << 1) + /// A detected plane, where a camera without LiDAR saw it (shape only, like the mesh). + public static let plane = VoxelSources(rawValue: 1 << 2) + /// Rays to tracked feature points. + public static let feature = VoxelSources(rawValue: 1 << 3) + /// Rays of estimated (monocular) depth. + public static let estimated = VoxelSources(rawValue: 1 << 4) + /// A LiDAR or feature ray passed through: the voxel's free space was measured, not estimated. + public static let measuredPass = VoxelSources(rawValue: 1 << 5) + /// What a ray that measured the voxel, rather than estimated or outlined it, leaves. + static let measuredHit: VoxelSources = [.lidar, .feature] +} + +/// A voxel's evidence, read out of the map. +public struct VoxelEvidence: Sendable, Equatable { + public var state: VoxelState + /// Frames whose rays stopped in the voxel. + public var hits: Int + /// Frames whose rays passed through it without any stopping there. + public var passes: Int + /// Nearest camera distance of a ray that stopped here, meters; nil without hits. + public var nearestDistance: Float? + /// Smallest angle between a ray that stopped here and the surface normal at that point, + /// radians; nil when no hit had a normal. + public var bestViewAngle: Float? + /// Unit surface normal, averaged over hits; nil when none had one. + public var normal: SIMD3? + /// ARKit's classification of the mesh here; nil where no classified mesh face fell. + public var meshClass: MeshClass? + public var sources: VoxelSources +} + +/// One voxel, packed into 16 bytes. +struct Voxel { + var logOdds: Int16 = 0 + var hits: UInt16 = 0 + var passes: UInt16 = 0 + /// Nearest hit distance in centimeters; `UInt16.max` without hits. + var nearestCm: UInt16 = .max + /// The frame that last updated `logOdds`, so one frame counts once. + var stamp: UInt16 = 0 + /// Largest |cos| between a hit's ray and its normal, times 255. + var bestCos: UInt8 = 0 + /// `MeshClass` raw value plus one; 0 for none recorded. + var meshLabel: UInt8 = 0 + var sources: UInt8 = 0 + /// Unit normal times 127; all zero when unknown. + var nx: Int8 = 0 + var ny: Int8 = 0 + var nz: Int8 = 0 + + var normal: SIMD3? { + guard nx != 0 || ny != 0 || nz != 0 else { return nil } + return simd_normalize(SIMD3(Float(nx), Float(ny), Float(nz))) + } + + /// Leaves the normal as it is when `n` has no direction (two opposite normals averaged). + mutating func setNormal(_ n: SIMD3) { + guard simd_length_squared(n) > 1e-12 else { return } + let q: SIMD3 = (simd_normalize(n) * Float(127)).rounded(.toNearestOrAwayFromZero) + nx = Int8(q.x) + ny = Int8(q.y) + nz = Int8(q.z) + } +} + +/// One ray to integrate, in map coordinates, from the frame's camera. +struct RaySample { + /// Where the ray stopped. + var end: SIMD3 + /// Unit normal of the surface at `end`, facing the camera; zero when unknown. + var normal: SIMD3 + /// Free space is carved along the ray up to this distance from the camera. + var freeLength: Float + /// Whether `end` is marked as a surface. + var hit: Bool + /// |cos| between the ray and the surface normal, when the caller knows it better than the + /// two directions do: an estimated ray's is widened for its normal's error. Nil takes it from + /// `normal` and the ray. + var cosine: Float? +} + +/// Sparse voxels over fixed bounds: a dense index of 8 x 8 x 8 bricks, each brick stored only +/// once a ray reaches it. Memory is bounded by the bounds (`worstCaseBytes`). +struct VoxelGrid { + static let brickShift: Int32 = 3 + static let brickEdge: Int32 = 1 << brickShift + static let brickVolume = Int(brickEdge * brickEdge * brickEdge) + + let origin: SIMD3 + let voxelSize: Float + /// Voxels along each axis, a whole number of bricks. + let dims: SIMD3 + let brickDims: SIMD3 + /// Per brick, its first voxel in `pool`, or -1 while not stored. + private(set) var brickSlot: [Int32] + private(set) var pool: [Voxel] = [] + private var frameStamp: UInt16 = 0 + + /// Covers `bounds` with voxels one of which is centred on `center`, so the planes through it + /// (the meter's ground and wall face) run through voxel centers rather than along voxel + /// faces, where rounding would split one surface between two layers. + init(bounds: MapBounds, voxelSize: Float, center: SIMD3) { + origin = center + (((bounds.min - center) / voxelSize + 0.5).rounded(.down) - 0.5) * voxelSize + self.voxelSize = voxelSize + let voxels = SIMD3(((bounds.max - origin) / voxelSize).rounded(.up)) + brickDims = (voxels &+ (Self.brickEdge &- 1)) &>> Self.brickShift + dims = brickDims &<< Self.brickShift + brickSlot = Array(repeating: -1, count: Int(brickDims.x) * Int(brickDims.y) * Int(brickDims.z)) + } + + /// Bytes held now: the brick index plus stored bricks. + var allocatedBytes: Int { + brickSlot.count * MemoryLayout.stride + pool.capacity * MemoryLayout.stride + } + + /// Bytes with every brick stored. + var worstCaseBytes: Int { + brickSlot.count * MemoryLayout.stride + brickSlot.count * Self.brickVolume * MemoryLayout.stride + } + + // MARK: Addressing + + func coordinate(of p: SIMD3) -> SIMD3? { + let g = SIMD3(((p - origin) / voxelSize).rounded(.down)) + guard all(g .>= 0), all(g .< dims) else { return nil } + return g + } + + func center(of g: SIMD3) -> SIMD3 { + origin + (SIMD3(g) + 0.5) * voxelSize + } + + @inline(__always) + private func brickIndex(_ g: SIMD3) -> Int { + let b = g &>> Self.brickShift + return Int(b.x) + Int(brickDims.x) * (Int(b.y) + Int(brickDims.y) * Int(b.z)) + } + + @inline(__always) + private static func local(_ g: SIMD3) -> Int { + let l = g & (brickEdge &- 1) + return Int(l.x | (l.y << brickShift) | (l.z << (2 * brickShift))) + } + + /// Index into `pool`, or nil when the brick is not stored. + func index(_ g: SIMD3) -> Int? { + let slot = brickSlot[brickIndex(g)] + return slot < 0 ? nil : Int(slot) + Self.local(g) + } + + @inline(__always) + private mutating func storedIndex(_ g: SIMD3) -> Int { + let b = brickIndex(g) + var slot = brickSlot[b] + if slot < 0 { + slot = Int32(pool.count) + if pool.count + Self.brickVolume > pool.capacity { + // Doubling as an array does, but never past every brick of the bounds. + pool.reserveCapacity(min(brickSlot.count * Self.brickVolume, max(2 * pool.capacity, 64 * Self.brickVolume))) + } + pool.append(contentsOf: repeatElement(Voxel(), count: Self.brickVolume)) + brickSlot[b] = slot + } + return Int(slot) + Self.local(g) + } + + func voxel(_ g: SIMD3) -> Voxel? { + index(g).map { pool[$0] } + } + + func voxel(at p: SIMD3) -> Voxel? { + coordinate(of: p).flatMap(voxel) + } + + /// Calls `body` with the coordinate and value of every stored voxel. + func forEachStored(_ body: (SIMD3, Voxel) -> Void) { + for bz in 0..= 0 else { continue } + let base = SIMD3(bx, by, bz) &<< Self.brickShift + for lz in 0.., rays: [RaySample], sources: VoxelSources, config: Map3DConfig) { + frameStamp = frameStamp == .max ? 1 : frameStamp + 1 + let stamp = frameStamp + let measured = !sources.isDisjoint(with: VoxelSources.measuredHit) + for ray in rays where ray.hit { + guard let g = coordinate(of: ray.end) else { continue } + let i = storedIndex(g) + let offset = ray.end - camera + let distance = simd_length(offset) + let cosine = ray.cosine ?? (simd_length_squared(ray.normal) > 0 ? abs(simd_dot(ray.normal, offset / distance)) : 0) + pool[i].recordHit( + stamp: stamp, distance: distance, cosine: cosine, normal: ray.normal, sources: sources.rawValue, measured: measured, config: config) + } + for ray in rays where ray.freeLength > 0 { + carveFree(from: camera, to: ray.end, length: ray.freeLength, stamp: stamp, measured: measured, config: config) + } + } + + /// A surface seen well enough to count as coverage (`Voxel.isWellSeenSurface`) by a ray that + /// measured it: LiDAR or a feature point. Estimated depth never certifies one. The review of + /// 2f17d67 made estimated hits claim walls behind occluders several ways; the fog of war and + /// the next view still read estimated evidence through `Voxel.state`. + func isWellSeenSurface(_ index: Int, config: Map3DConfig) -> Bool { + let voxel = pool[index] + return voxel.isWellSeenSurface(config) && !VoxelSources(rawValue: voxel.sources).isDisjoint(with: VoxelSources.measuredHit) + } + + func isWellSeenSurface(_ g: SIMD3, config: Map3DConfig) -> Bool { + index(g).map { isWellSeenSurface($0, config: config) } ?? false + } + + /// Free, and a measured ray passed through it: what the coverage bands count as free. + func isMeasuredFree(_ g: SIMD3, config: Map3DConfig) -> Bool { + guard let voxel = voxel(g) else { return false } + return voxel.state(config) == .free && voxel.sources & VoxelSources.measuredPass.rawValue != 0 + } + + /// Marks the voxels a segment from `from` toward `to` crosses completely within `length` as + /// passed through (3D DDA, Amanatides and Woo 1987), clipped to the grid. A voxel the + /// segment only enters is left alone: the ray ended somewhere past its free length, which + /// may lie inside that voxel, so only voxels it left again are known empty. + private mutating func carveFree(from: SIMD3, to: SIMD3, length: Float, stamp: UInt16, measured: Bool, config: Map3DConfig) { + let span = to - from + let total = simd_length(span) + guard total > 0 else { return } + let direction = span / total + guard let (start, end) = clip(from: from, direction: direction, length: min(length, total)) else { return } + var g = SIMD3(((from + direction * start - origin) / voxelSize).rounded(.down)) + g = simd_clamp(g, .zero, dims &- 1) + let step = Self.step(direction) + let inverse = 1 / direction + let boundary = origin + SIMD3(g &+ SIMD3(repeating: 0).replacing(with: 1, where: step .> 0)) * voxelSize + // Distance along the ray to the next voxel boundary on each axis, and between boundaries. + var tMax = SIMD3(repeating: .infinity) + var tDelta = SIMD3(repeating: .infinity) + for axis in 0..<3 where step[axis] != 0 { + tMax[axis] = (boundary[axis] - from[axis]) * inverse[axis] + tDelta[axis] = voxelSize * abs(inverse[axis]) + } + let miss = config.missLogOdds + let floor = config.minLogOdds + while true { + let exit = tMax.min() + guard exit <= end else { return } + pool[storedIndex(g)].recordPass(stamp: stamp, miss: miss, floor: floor, surface: config.surfaceLogOdds, measured: measured) + // Every axis whose boundary the ray crosses at this distance advances together: at an + // edge or corner the voxels beside it hold no length of the ray. + for axis in 0..<3 where tMax[axis] == exit { + g[axis] &+= step[axis] + guard g[axis] >= 0, g[axis] < dims[axis] else { return } + tMax[axis] += tDelta[axis] + } + } + } + + @inline(__always) + private static func step(_ direction: SIMD3) -> SIMD3 { + SIMD3(repeating: 0).replacing(with: 1, where: direction .> 0).replacing(with: -1, where: direction .< 0) + } + + /// The part of a ray from `from` along unit `direction`, up to `length`, inside the grid, as + /// distances along it; nil when it misses. + func clip(from: SIMD3, direction: SIMD3, length: Float) -> (Float, Float)? { + let upper = origin + SIMD3(dims) * voxelSize + var near: Float = 0 + var far = length + for axis in 0..<3 { + if direction[axis] == 0 { + guard from[axis] >= origin[axis], from[axis] < upper[axis] else { return nil } + continue + } + let a = (origin[axis] - from[axis]) / direction[axis] + let b = (upper[axis] - from[axis]) / direction[axis] + near = max(near, min(a, b)) + far = min(far, max(a, b)) + } + return near < far ? (near, far) : nil + } + + /// Walks the voxels a ray passes through, from `from` along unit `direction`, up to `length`, + /// calling `body` with each voxel's coordinate and the distance at which the ray enters it + /// until `body` returns false. Voxels not stored are passed as nil. Read-only. + func march(from: SIMD3, direction: SIMD3, length: Float, _ body: (SIMD3, Voxel?, Float) -> Bool) { + guard let (start, end) = clip(from: from, direction: direction, length: length) else { return } + var g = SIMD3(((from + direction * start - origin) / voxelSize).rounded(.down)) + g = simd_clamp(g, .zero, dims &- 1) + let step = Self.step(direction) + let inverse = 1 / direction + let boundary = origin + SIMD3(g &+ SIMD3(repeating: 0).replacing(with: 1, where: step .> 0)) * voxelSize + var tMax = SIMD3(repeating: .infinity) + var tDelta = SIMD3(repeating: .infinity) + for axis in 0..<3 where step[axis] != 0 { + tMax[axis] = (boundary[axis] - from[axis]) * inverse[axis] + tDelta[axis] = voxelSize * abs(inverse[axis]) + } + var entered = start + while true { + guard body(g, voxel(g), entered) else { return } + let axis = tMax.x < tMax.y ? (tMax.x < tMax.z ? 0 : 2) : (tMax.y < tMax.z ? 1 : 2) + guard tMax[axis] < end else { return } + entered = tMax[axis] + g[axis] &+= step[axis] + guard g[axis] >= 0, g[axis] < dims[axis] else { return } + tMax[axis] += tDelta[axis] + } + } + + // MARK: Mesh and planes + + /// Labels the voxels a mesh face falls in and gives them the face's normal where no ray + /// gave one; returns their `pool` indices. `retain` then marks them as mesh. + mutating func markMesh(_ triangle: (SIMD3, SIMD3, SIMD3), meshClass: MeshClass?) -> Set { + let (a, b, c) = triangle + let normal = simd_cross(b - a, c - a) + guard simd_length_squared(normal) > 0 else { return [] } + let longest = max(simd_distance(a, b), simd_distance(b, c), simd_distance(c, a)) + // Samples half a voxel apart, so no voxel the face crosses is skipped. + let n = max(1, Int((longest / (voxelSize / 2)).rounded(.up))) + var marked: Set = [] + for i in 0...n { + for j in 0...(n - i) { + let p = a + (b - a) * (Float(i) / Float(n)) + (c - a) * (Float(j) / Float(n)) + guard let g = coordinate(of: p) else { continue } + let index = storedIndex(g) + guard marked.insert(Int32(index)).inserted else { continue } + pool[index].recordMesh(normal: normal, meshClass: meshClass) + } + } + return marked + } + + /// The voxel a detected plane was seen at, given the plane's normal where no ray gave one. + mutating func markPlane(at point: SIMD3, normal: SIMD3) -> Int32? { + guard let g = coordinate(of: point) else { return nil } + let index = storedIndex(g) + if pool[index].normal == nil { pool[index].setNormal(normal) } + return Int32(index) + } + + /// How many mesh chunks (x) and detected planes (y) mark each voxel. A voxel carries the + /// `.mesh` or `.plane` source while its count is above zero, so replacing or removing one + /// chunk or plane leaves the marks of the others alone. + private(set) var marks: [Int32: SIMD2] = [:] + + mutating func retain(_ indices: some Sequence, as source: VoxelSources) { + let lane = source == .mesh ? 0 : 1 + for index in indices { + var count = marks[index] ?? .zero + count[lane] &+= 1 + marks[index] = count + pool[Int(index)].sources |= source.rawValue + } + } + + /// Undoes `retain`. At zero the source goes, with the mesh label for mesh; a voxel no ray + /// hit and nothing marks any more loses the normal a mark gave it. + mutating func release(_ indices: some Sequence, as source: VoxelSources) { + let lane = source == .mesh ? 0 : 1 + for index in indices { + guard var count = marks[index], count[lane] > 0 else { continue } + count[lane] -= 1 + let i = Int(index) + if count[lane] == 0 { + pool[i].sources &= ~source.rawValue + if source == .mesh { pool[i].meshLabel = 0 } + } + if count == .zero { + marks[index] = nil + if pool[i].hits == 0 { pool[i].nx = 0; pool[i].ny = 0; pool[i].nz = 0 } + } else { + marks[index] = count + } + } + } +} + +extension Voxel { + @inline(__always) + /// A voxel that stops being surface loses its viewing evidence, so it counts as seen again + /// only on a new observation. + mutating func recordPass(stamp: UInt16, miss: Int16, floor: Int16, surface: Int16, measured: Bool) { + guard self.stamp != stamp else { return } + // An estimate can't take back what a measured ray found, such as an occluder LiDAR saw. + guard measured || sources & VoxelSources.measuredHit.rawValue == 0 else { return } + if measured { sources |= VoxelSources.measuredPass.rawValue } + self.stamp = stamp + logOdds = max(floor, logOdds &+ miss) + if passes < .max { passes += 1 } + if logOdds < surface { + nearestCm = .max + bestCos = 0 + } + } + + @inline(__always) + mutating func recordHit( + stamp: UInt16, distance: Float, cosine: Float, normal: SIMD3, sources: UInt8, measured: Bool, config: Map3DConfig + ) { + let measuredBefore = self.sources & VoxelSources.measuredHit.rawValue != 0 + // An estimate adds nothing to what a measured ray found: not its strength, not its normal. + if !measured, measuredBefore { + self.sources |= sources + return + } + if self.stamp != stamp { + self.stamp = stamp + // A hit means something is there now, however long the space was seen empty: start + // from even odds, so one hit is surface, never still free. A voxel's first measured + // hit also starts there whatever estimated hits added before it, so a measured + // surface's strength is measured evidence alone. + let base = measured && !measuredBefore ? 0 : max(logOdds, 0) + logOdds = min(config.maxLogOdds, base &+ config.hitLogOdds) + if simd_length_squared(normal) > 0 { + // A running mean over up to 16 frames, so a normal keeps adapting as views improve. + let weight = Float(min(hits, 16)) + setNormal((self.normal ?? .zero) * weight + normal) + } + if hits < .max { hits += 1 } + } + nearestCm = min(nearestCm, UInt16(min(Float(UInt16.max - 1), distance * 100))) + // The best angle only from measured views within range, so one view is both near enough + // and square enough when the voxel counts as well seen. + if measured, distance <= config.maxViewDistance { + bestCos = max(bestCos, UInt8(min(255, (cosine * 255).rounded(.down)))) + } + self.sources |= sources + } + + mutating func recordMesh(normal: SIMD3, meshClass: MeshClass?) { + if let meshClass, meshClass != .none || meshLabel == 0 { meshLabel = meshClass.rawValue + 1 } + // Mesh winding is not trusted to face the camera, so a mesh normal only fills in a + // voxel no ray has given one. + if self.normal == nil { setNormal(normal) } + } + + func evidence(_ config: Map3DConfig) -> VoxelEvidence { + VoxelEvidence( + state: state(config), hits: Int(hits), passes: Int(passes), + nearestDistance: nearestCm == .max ? nil : Float(nearestCm) / 100, + bestViewAngle: bestCos == 0 ? nil : acos(Float(bestCos) / 255), + normal: normal, meshClass: meshLabel == 0 ? nil : MeshClass(rawValue: meshLabel - 1), + sources: VoxelSources(rawValue: sources)) + } + + func state(_ config: Map3DConfig) -> VoxelState { + if logOdds >= config.surfaceLogOdds, hits > 0 { return .surface } + if logOdds <= config.freeLogOdds, passes > 0 { return .free } + return .unknown + } + + /// A surface seen well enough to count as coverage: by one view from within + /// `maxViewDistance` and within `maxViewAngle` of its normal, a ray that measured it (not a + /// mesh face or a plane, and not a feature point off every plane, which carries no normal). + func isWellSeenSurface(_ config: Map3DConfig) -> Bool { + state(config) == .surface && Float(bestCos) / 255 >= cos(config.maxViewAngle) + } +} diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Replay/ReplayPlanning.swift b/ios/HouseScanKit/Sources/HouseScanKit/Replay/ReplayPlanning.swift index d616f57fc..16f94a936 100644 --- a/ios/HouseScanKit/Sources/HouseScanKit/Replay/ReplayPlanning.swift +++ b/ios/HouseScanKit/Sources/HouseScanKit/Replay/ReplayPlanning.swift @@ -176,6 +176,114 @@ public enum ReplayPlanning { return nil } + /// `heldBackWindow` for a replay whose coverage comes from the 3D map (`-coverage map3d`), which + /// counts what LiDAR depth saw from every frame, not only what kept frames pointed at. `depths` + /// holds each frame's depth, nil where it has none. + /// + /// A window is held back only if the map is left with a gap without it, and the window's own + /// frames close it: a map built from every other frame, in replay order, leaves the planner a + /// gap between the covered extremes, and integrating the frames the app replays for the gap + /// (`gapReplayRange`) satisfies it. Frames with limited tracking add nothing, as in the app. + /// + /// Building a map costs one integration per frame, so windows are tried in order of how many + /// cells their frames' depth alone shows (`CoverageMap.visibleCells(from:depth:)` with nothing + /// else seeing the cell) between the cells the other frames show: ground before wall, nearest + /// the meter first, as `GapPlanner.plan` chooses its gap. A window whose gap would come after + /// one the map keeps with every frame is not tried: the planner would ask for that one. At + /// most `maxTries` are built. Nil when none works, as on a replay whose nearest gap is a + /// stretch no frame's depth reaches. + public static func heldBackWindow( + frames: [PlannedFrame], depths: [DepthImage?], wall: WallFrame, planner: GapPlanner = GapPlanner(), + config: CoverageConfig = CoverageConfig(), mapConfig: Map3DConfig = Map3DConfig(), maxTries: Int = 4 + ) -> HeldBackWindow? { + precondition(depths.count == frames.count, "\(depths.count) depth entries for \(frames.count) frames") + guard frames.count >= 6 else { return nil } + let depthFrames = frames.indices.map { index -> DepthFrame? in + guard frames[index].trackingNormal, let image = depths[index] else { return nil } + return DepthFrame(image: image, pose: frames[index].camera) + } + let empty = CoverageMap(wall: wall, config: config) + struct Cell: Hashable { + var band: SurfaceBand + var index: Int + } + let seen: [Set] = frames.indices.map { index in + guard let image = depths[index], frames[index].trackingNormal else { return [] } + let sightings = empty.visibleCells(from: frames[index].camera, depth: CoverageMap.storedDepth(image)) + return Set(sightings.filter { !$0.rows.isEmpty }.map { Cell(band: $0.band, index: $0.index) }) + } + var seenBy: [Cell: Int] = [:] + for cells in seen { for cell in cells { seenBy[cell, default: 0] += 1 } } + let reach = planner.config.reach + var candidates: [(window: Range, band: Int, distance: Float, only: Int)] = [] + for size in [4, 6, 3, 8, 10] { + for start in 2.. inWindow[cell, default: 0] }.map(\.key.index) + guard let low = outside.min(), let high = outside.max() else { continue } + let only = inWindow.keys.filter { cell in + seenBy[cell] == inWindow[cell] && cell.index > low && cell.index < high && abs(empty.cellRange(cell.index).lowerBound) <= reach + } + // The planner asks for the ground run nearest the meter first, then the wall's, so + // a window is only useful if its gap is nearer than any stretch no frame shows. + let nearest = only.map { cell in + (cell.band == .ground ? 0 : 1, abs(empty.cellRange(cell.index).lowerBound + empty.config.cellWidth / 2)) + }.min { $0 < $1 } + if let nearest, only.count >= 2 { candidates.append((window, nearest.0, nearest.1, only.count)) } + } + } + candidates.sort { a, b in + (a.band, a.distance, -a.only, a.window.count) < (b.band, b.distance, -b.only, b.window.count) + } + + func measured(_ map: Map3D, into coverage: inout CoverageMap) { + coverage.setMeasuredCovered(coverage.cells(seenIn: map.coverage(along: wall))) + } + // A gap the map has with every frame (a stretch behind a bin that no view gets past) is + // one no window removes, and the planner asks for it before any gap farther out or on the + // wall. One map of every frame finds it; only windows whose gap would come first are tried. + var full = Map3D(frame: MapFrame(wall: wall), config: mapConfig) + for depth in depthFrames.compactMap({ $0 }) { full.integrate(depth) } + var fullCoverage = CoverageMap(wall: wall, config: config) + measured(full, into: &fullCoverage) + if let ends = coveredExtremes(fullCoverage) { + fullCoverage.setEnd(.left, at: ends.lowerBound) + fullCoverage.setEnd(.right, at: ends.upperBound) + if let standing = planner.plan(fullCoverage) { + let band = standing.band == .ground ? 0 : 1 + let span = standing.span + let distance: Float = span.contains(0) ? 0 : min(abs(span.lowerBound), abs(span.upperBound)) + candidates.removeAll { ($0.band, $0.distance) >= (band, distance) } + } + } + for candidate in candidates.prefix(maxTries) { + let window = candidate.window + var rest = Map3D(frame: MapFrame(wall: wall), config: mapConfig) + for index in frames.indices where !window.contains(index) { + if let depth = depthFrames[index] { rest.integrate(depth) } + } + var walked = CoverageMap(wall: wall, config: config) + measured(rest, into: &walked) + guard let ends = coveredExtremes(walked) else { continue } + walked.setEnd(.left, at: ends.lowerBound) + walked.setEnd(.right, at: ends.upperBound) + guard let gap = planner.plan(walked) else { continue } + var restored = rest + for index in gapReplayRange(window) { + if let depth = depthFrames[index] { restored.integrate(depth) } + } + var after = walked + measured(restored, into: &after) + guard planner.isSatisfied(gap, after) else { continue } + return HeldBackWindow(frames: window, ends: ends, gap: gap) + } + return nil + } + private static func keptIndices(_ frames: [PlannedFrame], wall: WallFrame, config: CoverageConfig) -> [Int] { var map = CoverageMap(wall: wall, config: config) var capture = AutoCapture() diff --git a/ios/HouseScanKit/Sources/HouseScanKit/Scene/SceneExport.swift b/ios/HouseScanKit/Sources/HouseScanKit/Scene/SceneExport.swift index 05e5b31ec..6c4a8a224 100644 --- a/ios/HouseScanKit/Sources/HouseScanKit/Scene/SceneExport.swift +++ b/ios/HouseScanKit/Sources/HouseScanKit/Scene/SceneExport.swift @@ -231,6 +231,13 @@ public struct SceneInput: Sendable { /// Headroom measured on the LiDAR mesh (`TriangleMesh.overheadSpans`), as `meshFacing`; /// written as `overheads` entries. public var meshOverheads: [ObservedSpan] + /// Position error of each piece's line, meters, one per piece of `wall.chain` in the same + /// order; nil, or an empty list, leaves the server's default for the piece's source. + public var wallPlusMinus: [Float?] + /// Where the meter is, world meters, when it is not on the wall's line: a meter tapped on the + /// face of a box proud of a measured wall. Its foot on the meter's piece must be the chain's + /// origin (s = 0), as the server places it. Nil writes `wall.meter`. + public var meterPosition: SIMD3? /// What the homeowner said the ground along the wall is. With a type, the ground the ground /// coverage saw is sent as patches of it (`SceneWall.groundPatchPolygons`); nil sends none, /// and the server treats the surface as unknown. @@ -240,7 +247,8 @@ public struct SceneInput: Sendable { wall: SceneWall, wallID: String = "wall", baselineS: ClosedRange, wallHeight: Float? = nil, meterPlusMinus: Float? = nil, meterPlane: MeterPlaneSource = .detectedPlane, objectPlusMinus: Float? = nil, features: [SceneFeature] = [], coverage: SceneCoverage, keyframes: [SceneKeyframe] = [], stills: [String: String] = [:], - meshFacing: [ObservedSpan] = [], meshOverheads: [ObservedSpan] = [], groundType: SceneGroundType? = nil + meshFacing: [ObservedSpan] = [], meshOverheads: [ObservedSpan] = [], groundType: SceneGroundType? = nil, + wallPlusMinus: [Float?] = [], meterPosition: SIMD3? = nil ) { self.wall = wall self.wallID = wallID @@ -256,6 +264,8 @@ public struct SceneInput: Sendable { self.meshFacing = meshFacing self.meshOverheads = meshOverheads self.groundType = groundType + self.wallPlusMinus = wallPlusMinus + self.meterPosition = meterPosition } } @@ -273,6 +283,10 @@ public enum SceneExportError: Error, Equatable, CustomStringConvertible { case nonFiniteNumber(String) /// Left corners must run from the meter leftward (s falling below 0), right corners rightward. case cornersOutOfOrder([Float]) + /// A list that must be empty or hold one entry per item of another has neither. + case countMismatch(field: String, expected: Int, actual: Int) + /// `meterPosition` projects onto the chain this far from the origin, meters. + case meterOffChainOrigin(s: Float) public var description: String { switch self { @@ -288,6 +302,8 @@ public enum SceneExportError: Error, Equatable, CustomStringConvertible { case .invalidKeyframe(let id, let r): "keyframe \(id): \(r)" case .nonFiniteNumber(let d): "non-finite number in scene: \(d)" case .cornersOutOfOrder(let s): "corner s values \(s) do not run outward from the meter" + case .countMismatch(let f, let e, let a): "\(f) has \(a) entries, expected \(e)" + case .meterOffChainOrigin(let s): "meterPosition projects onto the wall at s = \(s) m, not at the chain's origin" } } } @@ -377,6 +393,17 @@ public enum SceneExport { if let h = input.wallHeight, !(h > 0) { throw SceneExportError.nonPositiveWallHeight(h) } if let pm = input.meterPlusMinus { try requireNonNegative(pm, "meterPlusMinus") } if let pm = input.objectPlusMinus { try requireNonNegative(pm, "objectPlusMinus") } + // The server puts s = 0 at the meter's projection onto its wall; a millimeter covers + // Float rounding of a point built on the chain. + if let position = input.meterPosition, abs(wall.wallCoordinates(of: position).s) > 0.001 { + throw SceneExportError.meterOffChainOrigin(s: wall.wallCoordinates(of: position).s) + } + guard input.wallPlusMinus.isEmpty || input.wallPlusMinus.count == chain.segments.count else { + throw SceneExportError.countMismatch(field: "wallPlusMinus", expected: chain.segments.count, actual: input.wallPlusMinus.count) + } + for (index, pm) in input.wallPlusMinus.enumerated() { + if let pm { try requireNonNegative(pm, "wallPlusMinus[\(index)]") } + } let objectError = input.objectPlusMinus.map(feet) let meterError: Float? = switch input.meterPlane { case .detectedPlane: input.meterPlusMinus @@ -460,7 +487,7 @@ public enum SceneExport { for (index, item) in spans.enumerated() { try requireNonNegative(item.out, "coverage.\(band)[\(index)].out") } } let corners = chain.segments.dropLast().map(\.span.upperBound) - let writtenWalls = walls(chain.segments, ids: wallIDs, sources: chain.segments.map(\.source), baselineS: input.baselineS, height: input.wallHeight, plan: plan) + let writtenWalls = walls(chain.segments, ids: wallIDs, sources: chain.segments.map(\.source), plusMinus: input.wallPlusMinus, baselineS: input.baselineS, height: input.wallHeight, plan: plan) let meterFeet = point3Feet(wall.meter) // Ground entries stop short of every corner, after all joining. The server draws a ground // entry as the strip in front of the chain over its span and, where the span crosses a @@ -518,7 +545,7 @@ public enum SceneExport { return SceneDocument( schema_version: "1.0", - meter: .init(pos: point3Feet(wall.meter), wall_id: input.wallID, plus_minus_ft: meterError.map(feet)), + meter: .init(pos: point3Feet(input.meterPosition ?? wall.meter), wall_id: input.wallID, plus_minus_ft: meterError.map(feet)), walls: writtenWalls, objects: objects, ground: ground, overheads: overheads.isEmpty ? nil : overheads, facing: facing, coverage: .init( @@ -534,19 +561,20 @@ public enum SceneExport { /// previous one ends (the same numbers, computed once), so the server reads the chain as /// continuous and each corner as a corner. private static func walls( - _ segments: [WallSegment], ids: [String], sources: [WallLineSource], baselineS: ClosedRange, height: Float?, + _ segments: [WallSegment], ids: [String], sources: [WallLineSource], plusMinus: [Float?], baselineS: ClosedRange, height: Float?, plan: (Float, Float) -> [Double] ) -> [SceneDocument.Wall] { - let pieces = segments.indices.compactMap { index -> (id: String, source: WallLineSource, span: ClosedRange)? in + let pieces = segments.indices.compactMap { index -> (id: String, source: WallLineSource, plusMinus: Float?, span: ClosedRange)? in let low = max(segments[index].span.lowerBound, baselineS.lowerBound) let high = min(segments[index].span.upperBound, baselineS.upperBound) - return low < high ? (ids[index], sources[index], low...high) : nil + return low < high ? (ids[index], sources[index], plusMinus.isEmpty ? nil : plusMinus[index], low...high) : nil } let points = ([pieces.first?.span.lowerBound] + pieces.map(\.span.upperBound)).compactMap { $0 }.map { plan($0, 0) } // The source is always written, also `tap`, so the scene says how every line was found. return pieces.enumerated().map { index, piece in SceneDocument.Wall( - id: piece.id, baseline: [points[index], points[index + 1]], height_ft: height.map(feet), source: piece.source.rawValue) + id: piece.id, baseline: [points[index], points[index + 1]], height_ft: height.map(feet), source: piece.source.rawValue, + plus_minus_ft: piece.plusMinus.map(feet)) } } @@ -750,6 +778,7 @@ private struct SceneDocument: Encodable { var baseline: [[Double]] var height_ft: Double? var source: String + var plus_minus_ft: Double? } struct Attrs: Encodable { var operable: Bool diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/ExportedWallTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/ExportedWallTests.swift new file mode 100644 index 000000000..2353b1eb7 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/ExportedWallTests.swift @@ -0,0 +1,62 @@ +import Foundation +import HouseScanKit +import simd +import Testing + +/// The server answers in s along the wall scene.json described; the walk plans and draws along its +/// own. `WallFrame.s(_:along:)` carries a place from one to the other through the world. +@Suite struct ExportedWallTests { + /// The walk's straight wall, and an exported chain with the same line that turns at s = 2 to + /// face +x, so its s past the corner runs along -z behind the walk's line. + static let walk = WallFrame(meter: SIMD3(0, 1.5, 0), outward: SIMD3(0, 0, 1), groundY: 0)! + static var exported: WallFrame { + var wall = walk + wall.turn(.right, at: WallCorner(s: 2, outward: SIMD3(1, 0, 0), source: .mesh)) + return wall + } + + @Test func aServerSpanPastTheExportedCornerIsPlannedWhereTheWalkSeesIt() throws { + let item = try JSONDecoder().decode(PlacementMissingEvidence.self, from: Data(""" + {"kind": "band", "band": "ground", "span_ft": [3.2808, 9.8425], "out_ft": 4, "message": "Show the ground"} + """.utf8)) + let mapped = item.along(Self.walk, from: Self.exported) + // 1 m and 3 m along the exported chain: 1 m is on the shared line; 3 m is 1 m past the + // corner, at (2, 0, -1), whose foot on the walk's straight wall is s = 2. + let span = try #require(mapped.spanFt) + #expect(abs(span.x - 3.2808) < 1e-3) + #expect(abs(span.y - 6.5617) < 1e-3) + #expect(mapped.outFt == 4) + let plan = try #require(GapPlanner().plan(for: mapped, leftEnd: nil, rightEnd: nil)) + #expect(abs(plan.span.lowerBound - 1) < 1e-3 && abs(plan.span.upperBound - 2) < 1e-3) + } + + @Test func theSameWallMapsEverySpanToItself() throws { + let item = try JSONDecoder().decode(PlacementMissingEvidence.self, from: Data(""" + {"kind": "band", "band": "wall", "span_ft": [-2, 5], "message": "Show the wall"} + """.utf8)) + #expect(item.along(Self.walk, from: Self.walk) == item) + } + + /// ARKit refines the meter's anchor after the upload: the walk's wall moves 3 cm along x and its + /// ground is measured 5 cm lower. The exported chain moves the same way, keeping its corner. + @Test func theExportedWallFollowsTheWalksWall() { + var moved = Self.walk + moved.meter += SIMD3(0.03, 0, 0) + moved.groundY -= 0.05 + let exported = Self.exported + let following = exported.following(Self.walk, to: moved) + #expect(simd_distance(following.meter, exported.meter + SIMD3(0.03, 0, 0)) < 1e-6) + #expect(abs(following.groundY - (exported.groundY - 0.05)) < 1e-6) + #expect(following.rightCorners == exported.rightCorners) + #expect(simd_distance(following.world(s: 3, height: 0), exported.world(s: 3, height: 0) + SIMD3(0.03, -0.05, 0)) < 1e-5) + } + + /// When scene.json described the walk's own wall, it is the walk's wall wherever that goes, + /// corners it has turned since included. + @Test func anExportedWalkWallIsTheWalksWall() { + var moved = Self.walk + moved.meter += SIMD3(0.03, 0, 0) + moved.turn(.left, at: WallCorner(s: -3, outward: SIMD3(-1, 0, 0))) + #expect(Self.walk.following(Self.walk, to: moved) == moved) + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/CarriedEvidenceTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/CarriedEvidenceTests.swift new file mode 100644 index 000000000..122b9a026 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/CarriedEvidenceTests.swift @@ -0,0 +1,52 @@ +import HouseScanKit +import simd +import Testing + +/// Walked-facing and confirmed-overhead evidence is measured along the walk's tapped wall; when +/// scene.json describes the measured chain, `ObservedSpan.carried` restates it along that chain. +@Suite struct CarriedEvidenceTests { + static let tapped = WallFrame(meter: SIMD3(0, 1.2, 0), outward: SIMD3(0, 0, 1), groundY: 0)! + + /// The measured wall: its line 0.1 m in front of the tapped one, turned 5 degrees. + static var measured: WallFrame { + let angle: Float = 5 * .pi / 180 + return WallFrame(meter: SIMD3(0, 1.2, 0.1), outward: SIMD3(sin(angle), 0, cos(angle)), groundY: 0)! + } + + @Test func aWalkedReachIsRestatedInsideWhatWasWalked() throws { + let walked = [ObservedSpan(span: -2...2, out: 1.5)] + let carried = try #require(ObservedSpan.carried(walked, depth: nil, from: Self.tapped, to: Self.measured).first) + // The carried claim's corners lie over the walked span, and its far edge within the walked + // reach, in the tapped wall's frame. + for s in [carried.span.lowerBound, carried.span.upperBound] { + for out in [Float(0), carried.out] { + let p = Self.tapped.wallPoint(Self.measured.world(s: s, height: 0, out: out)) + #expect(p.s >= -2 - 1e-4 && p.s <= 2 + 1e-4 && p.out <= 1.5 + 1e-4, "(\(s), \(out)) lands at \(p)") + } + } + // The measured line stands up to 0.1 + 2 sin 5 degrees = 0.27 m in front of the tapped one + // over the span, which comes off the reach; the turn trims the ends. + #expect(carried.out > 1.2 && carried.out < 1.4) + #expect(carried.span.lowerBound > -2 && carried.span.upperBound < 2) + #expect(carried.span.upperBound - carried.span.lowerBound > 3.5) + } + + @Test func overheadKeepsItsHeightOverTheBatterysDepth() throws { + let clear = [ObservedSpan(span: -1...1, out: 2.4)] + let carried = ObservedSpan.carried(clear, depth: 0.5588, from: Self.tapped, to: Self.tapped) + #expect(carried == clear) + // A measured line 0.1 m out moves the battery's depth past what the tilt-up view vouched + // for, so nothing is carried. + #expect(ObservedSpan.carried(clear, depth: 0.5588, from: Self.tapped, to: Self.measured).isEmpty) + } + + /// Past a corner of either wall, the evidence is not carried: only the meter's pieces are + /// straight in both. + @Test func nothingIsCarriedPastACorner() { + var cornered = Self.tapped + cornered.turn(.right, at: WallCorner(s: 1, outward: SIMD3(1, 0, 0))) + let carried = ObservedSpan.carried([ObservedSpan(span: -1...3, out: 1)], depth: nil, from: Self.tapped, to: cornered) + #expect(carried.allSatisfy { $0.span.upperBound <= 1 + 1e-4 }) + #expect(!carried.isEmpty) + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DAttachedReliefTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DAttachedReliefTests.swift new file mode 100644 index 000000000..fe96720c5 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DAttachedReliefTests.swift @@ -0,0 +1,26 @@ +import HouseScanKit +import simd +import Testing + +/// Relief counts as the facade only with positive evidence it is attached: a side face running +/// from its front back to the wall. The review of 2f17d67 found relief accepted wherever no free +/// voxel was seen behind it, so a tall freestanding object whose gap nobody saw passed. +@Suite struct Map3DAttachedReliefTests { + static let wall = standardWall() + + /// A freestanding slab 3 m tall and 1 m wide, standing 0.3 to 0.4 m out over x 1 to 2, walked + /// straight on: no view reaches the gap behind its middle, and none sees the wall there. + @Test func aFreestandingSlabWithAnUnseenGapIsNotRelief() { + let scene = SyntheticScene( + walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(6, 0))], + boxes: [SyntheticScene.Box(min: SIMD3(1, 0, 0.3), max: SIMD3(2, 3, 0.4))]) + var map = Map3D(frame: sceneFrame()) + for camera in bushWalk() { map.integrate(scene.depthFrame(from: camera)) } + let coverage = map.coverage(along: Self.wall) + for index in map.cellIndices where map.cellRange(index).lowerBound >= 1.2 && map.cellRange(index).upperBound <= 1.8 { + #expect(map.wallHeight(cell: index, along: Self.wall) == nil, "wall behind the slab at cell \(index) seen to \(map.wallHeight(cell: index, along: Self.wall) ?? -1)") + let middle = (map.cellRange(index).lowerBound + map.cellRange(index).upperBound) / 2 + #expect(!coverage.wall.contains { $0.contains(middle) }, "wall behind the slab claimed at cell \(index)") + } + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DCaretakerTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DCaretakerTests.swift new file mode 100644 index 000000000..5a0c53d41 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DCaretakerTests.swift @@ -0,0 +1,111 @@ +@testable import HouseScanKit +import Foundation +import simd +import Testing + +/// The #21 caretaker's findings on 2ce7d18, each reproduced before its fix. +@Suite struct Map3DCaretakerTests { + static let config = Map3DConfig() + + /// A 2 m grid of 10 cm voxels with one centred on the origin. + static func grid() -> VoxelGrid { + VoxelGrid(bounds: MapBounds(min: SIMD3(-1, -1, -1), max: SIMD3(1, 1, 1)), voxelSize: 0.1, center: .zero) + } + + static let camera = SIMD3(0, 0, 0.9) + /// A ray that stops square on the origin's voxel. + static let hit = RaySample(end: .zero, normal: SIMD3(0, 0, 1), freeLength: 0, hit: true, cosine: 1) + /// A ray that passes through the origin's voxel and stops far behind it. + static let pass = RaySample(end: SIMD3(0, 0, -0.9), normal: .zero, freeLength: 1.8, hit: false) + + /// A measured hit then a measured pass leave 85 - 40 = 45, under the 50 a surface needs. An + /// estimated hit in between or before must not count toward the measured strength. + @Test(arguments: [[true, false, true], [false, true, true]]) + func estimatedHitsAddNothingToMeasuredStrength(hitsMeasured: [Bool]) { + var grid = Self.grid() + for measured in hitsMeasured.dropLast() { + grid.integrate(camera: Self.camera, rays: [Self.hit], sources: measured ? .lidar : .estimated, config: Self.config) + } + grid.integrate(camera: Self.camera, rays: [Self.pass], sources: .lidar, config: Self.config) + let voxel = grid.voxel(at: .zero) + #expect(voxel?.state(Self.config) != .surface, "log-odds \(voxel?.logOdds ?? 0)") + #expect(!grid.isWellSeenSurface(grid.coordinate(of: .zero)!, config: Self.config)) + } + + /// A ray from a voxel centre diagonally through the corners of the x-y grid spends no length + /// in the voxels beside the diagonal; they stay unknown. + @Test func aRayThroughVoxelCornersLeavesItsNeighboursUnknown() { + var grid = Self.grid() + let end = SIMD3(0.3, 0.3, 0) + grid.integrate(camera: .zero, rays: [RaySample(end: end, normal: .zero, freeLength: simd_length(end), hit: false)], sources: .lidar, config: Self.config) + for beside in [SIMD3(0.1, 0, 0), SIMD3(0, 0.1, 0), SIMD3(0.2, 0.1, 0), SIMD3(0.1, 0.2, 0)] { + #expect((grid.voxel(at: beside)?.passes ?? 0) == 0, "\(beside) passed through") + } + #expect(grid.voxel(at: SIMD3(0.1, 0.1, 0))?.state(Self.config) == .free) + } + + /// A batch holding both a move of the meter's anchor (+0.2 m out) and a mesh chunk ARKit sent + /// in the corrected world: the chunk lands on the depth evidence, which moved with the anchor. + @Test func meshSentWithAMeterMoveLandsOnTheMovedDepth() { + let scene = SyntheticScene(walls: [SyntheticScene.Wall(a: SIMD2(-3, 0), b: SIMD2(3, 0))]) + var map = Map3D(frame: sceneFrame()) + for x in [Float(-0.5), 0, 0.5] { map.integrate(scene.depthFrame(from: lidarCamera(at: SIMD3(x, 1.2, 2), lookingAt: SIMD3(x, 1, 0)))) } + #expect(map.state(at: SIMD3(0, 1, 0)) == .surface) + var move = matrix_identity_float4x4 + move.columns.3 = SIMD4(0, 0, 0.2, 1) + let moved = map.frame.following(anchorMovedFrom: matrix_identity_float4x4, to: move) + // The wall as ARKit now places it, 0.2 m out in world. + let chunk = MeshChunk( + id: UUID(), worldFromChunk: matrix_identity_float4x4, + vertices: [SIMD3(-1, 0.5, 0.2), SIMD3(1, 0.5, 0.2), SIMD3(1, 1.5, 0.2), SIMD3(-1, 1.5, 0.2)], + faces: [SIMD3(0, 1, 2), SIMD3(0, 2, 3)], classes: [.wall, .wall]) + map.apply(frame: moved, planes: [:], chunks: [chunk.id: chunk]) + // In map coordinates the depth wall stays at z = 0; the mesh must be there too. + #expect(map.evidence(at: SIMD3(0, 1, 0)).meshClass == .wall) + #expect(map.evidence(at: SIMD3(0, 1, 0.2)).meshClass == nil) + } + + /// A slow model: more kept frames arrive than the worker takes. At most `limit` wait, the + /// newest, and the rest are counted as dropped. + @Test func pendingWorkKeepsTheNewestUpToItsLimit() { + var work = PendingWork(limit: 2) + for item in 1...10 { work.add(item) } + #expect(work.count == 2) + #expect(work.dropped == 8) + #expect(work.take() == 9) + #expect(work.take() == 10) + #expect(work.take() == nil) + work.add(11) + work.removeAll() + #expect(work.take() == nil) + } + + /// Anchors ARKit sends before the meter is placed wait for the map, within a budget: the + /// oldest go first once their vertices pass it, and an update replaces its anchor's entry. + @Test func waitingAnchorsStayWithinTheirBudget() { + var waiting = BoundedRecent(budget: 10) { $0.count } + for key in 0..<6 { waiting[key] = Array(repeating: 0, count: 3) } + #expect(waiting.total <= 10) + #expect(Set(waiting.values.map { $0.count }) == [3]) + #expect(waiting.keys == [3, 4, 5]) + waiting[3] = Array(repeating: 0, count: 4) + #expect(waiting.keys == [4, 5, 3]) + #expect(waiting.total == 10) + waiting[5] = nil + #expect(waiting.keys == [4, 3]) + // One entry over the whole budget is not kept. + waiting[9] = Array(repeating: 0, count: 11) + #expect(waiting[9] == nil) + } + + /// Two well-fitted parallel pieces 0.2 m apart joined straight: the chain's error must cover + /// both, not only the kept line's own fit. + @Test func aStraightJoinCoversTheJoinedPiecesOffset() throws { + let map = Map3D(frame: sceneFrame()) + let kept = MeasuredWall(start: SIMD2(-2, 0), end: SIMD2(0.5, 0), outward: SIMD2(0, 1), source: .mesh, support: 25, plusMinus: 0.05) + let joined = MeasuredWall(start: SIMD2(0.8, 0.2), end: SIMD2(3, 0.2), outward: SIMD2(0, 1), source: .mesh, support: 22, plusMinus: 0.05) + let chain = try #require(map.chain([kept, joined])) + #expect(chain.walls.count == 1) + #expect(chain.walls[0].plusMinus >= 0.2 + 0.05 - 1e-4, "plus-minus \(chain.walls[0].plusMinus)") + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DEstimatedEvidenceTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DEstimatedEvidenceTests.swift new file mode 100644 index 000000000..0e501a4d0 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DEstimatedEvidenceTests.swift @@ -0,0 +1,72 @@ +@testable import HouseScanKit +import Foundation +import simd +import Testing + +/// Estimated (monocular) depth never certifies coverage: the review of 2f17d67 made it claim +/// walls hidden behind occluders several ways (a plane scaling a model into the wall, regional +/// bias, a hit share taken where few rays could reach, an adjusted view angle dropped). It still +/// tells seen from unknown for the fog of war and the next view. +@Suite struct Map3DEstimatedEvidenceTests { + static let wall = standardWall() + static let cameras = bushWalk() + + /// A depth frame of `scene` from `camera` reported as estimated, with a small honest sigma: + /// the best case an estimate can be. + static func estimated(_ scene: SyntheticScene, _ camera: CameraFrame) -> DepthFrame { + let exact = scene.depthFrame(from: camera) + return DepthFrame( + camera: camera, width: exact.width, height: exact.height, depth: exact.depth, + kind: .estimated(sigma: [Float](repeating: 0.02, count: exact.depth.count))) + } + + static func claims(_ coverage: Map3DCoverage) -> Bool { + !coverage.wall.isEmpty || !coverage.wallHeight.isEmpty || !coverage.ground.isEmpty || !coverage.facing.isEmpty || !coverage.overhead.isEmpty + } + + @Test func estimatedDepthAloneClaimsNothingButClearsFog() { + let scene = SyntheticScene(walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(6, 0))]) + var map = Map3D(frame: sceneFrame()) + let before = map.fogOfWar(along: Self.wall).seen + for camera in Self.cameras { map.integrate(Self.estimated(scene, camera)) } + let coverage = map.coverage(along: Self.wall) + #expect(!Self.claims(coverage), "estimated depth claimed \(coverage)") + #expect(map.cellIndices.allSatisfy { map.facadeOffset(cell: $0, along: Self.wall) == nil }) + #expect(map.fogOfWar(along: Self.wall).seen > before + 0.2) + } + + /// LiDAR shows the wall behind the bush hidden; a model that sees straight through the bush + /// (regional bias, as the review's probes found) adds nothing to what LiDAR claims, and never + /// clears the space the bush fills. + @Test func estimatedFramesAfterLidarClaimNothingLidarDidNot() { + let bush = bushScene() + let throughTheBush = SyntheticScene(walls: bush.walls) + var map = Map3D(frame: sceneFrame()) + for camera in Self.cameras { map.integrate(bush.depthFrame(from: camera)) } + let lidar = map.coverage(along: Self.wall) + for camera in Self.cameras { map.integrate(Self.estimated(throughTheBush, camera)) } + let mixed = map.coverage(along: Self.wall) + func within(_ spans: [ObservedSpan], _ claimed: [ObservedSpan]) -> Bool { + spans.allSatisfy { item in claimed.contains { $0.span.lowerBound <= item.span.lowerBound + 1e-4 && $0.span.upperBound >= item.span.upperBound - 1e-4 && $0.out >= item.out - 1e-4 } } + } + #expect(mixed.wall.allSatisfy { span in lidar.wall.contains { $0.lowerBound <= span.lowerBound + 1e-4 && $0.upperBound >= span.upperBound - 1e-4 } }, "wall \(mixed.wall) past LiDAR's \(lidar.wall)") + #expect(within(mixed.wallHeight, lidar.wallHeight), "wall heights \(mixed.wallHeight) past LiDAR's \(lidar.wallHeight)") + #expect(within(mixed.ground, lidar.ground), "ground \(mixed.ground) past LiDAR's \(lidar.ground)") + #expect(within(mixed.facing, lidar.facing), "facing \(mixed.facing) past LiDAR's \(lidar.facing)") + #expect(within(mixed.overhead, lidar.overhead), "overhead \(mixed.overhead) past LiDAR's \(lidar.overhead)") + // The bush fills x 1 to 2 from 0.4 to 1.0 m out. + #expect(!mixed.facing.contains { $0.span.upperBound > 1.1 && $0.span.lowerBound < 1.9 }, "space the bush fills claimed clear: \(mixed.facing)") + } + + /// The viewing angle a ray carries (an estimated ray's is widened for its normal's error) + /// is the one the voxel keeps. + @Test func aRaysOwnCosineIsTheOneRecorded() throws { + var grid = VoxelGrid(bounds: MapBounds(min: SIMD3(-1, -1, -1), max: SIMD3(1, 1, 1)), voxelSize: 0.1, center: .zero) + let camera = SIMD3(0, 0, 0.9) + let end = SIMD3(0.05, 0.05, 0.05) + let normal = simd_normalize(camera - end) + grid.integrate(camera: camera, rays: [RaySample(end: end, normal: normal, freeLength: 0, hit: true, cosine: 0.5)], sources: .lidar, config: Map3DConfig()) + let angle = try #require(grid.voxel(at: end)?.evidence(Map3DConfig()).bestViewAngle) + #expect(abs(angle - acos(Float(0.5))) < 0.01) + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DEstimatedTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DEstimatedTests.swift new file mode 100644 index 000000000..b48f5a8d3 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DEstimatedTests.swift @@ -0,0 +1,177 @@ +import Foundation +import HouseScanKit +import simd +import Testing + +// Estimated (monocular) depth at the worst noise Map3D's rule lets mark a surface: Gaussian, 7 cm +// per pixel, reported honestly as each pixel's sigma. Every coverage claim is checked against +// exact ray casting of the scene from the same cameras. A monocular model's error is not this +// (it is smooth and regional); independent noise at the limit is the case a per-ray rule alone +// lets through, because one ray in forty lands two deviations too far and a wall voxel behind an +// occluder is never passed through, so nothing takes such a stray hit back. + +/// Whether any point within a voxel (0.1 m) of `point` along `u` and `v` on its surface is truly +/// visible from one of `cameras`: the resolution coverage is judged at. +func nearlyVisible(_ scene: SyntheticScene, _ cameras: [CameraFrame], _ point: SIMD3, normal: SIMD3, u: SIMD3, v: SIMD3) -> Bool { + for du: Float in [-0.1, 0, 0.1] { + for dv: Float in [-0.1, 0, 0.1] { + let p = point + u * du + v * dv + normal * 1e-3 + if cameras.contains(where: { scene.isVisible(p, normal: normal, from: $0) }) { return true } + } + } + return false +} + +@Suite struct Map3DEstimatedTests { + static let sigma: Float = 0.07 + static let wall = standardWall() + + /// A wall along x from -5 to 6 with a box 3.5 m wide (x 1 to 4.5) and 1 m tall whose front + /// face stands `offset` out: the nearest surface in front of the wall there. It is 0.3 m deep, + /// or less where that would reach within 5 cm of the wall. + static func occluderScene(offset: Float) -> SyntheticScene { + SyntheticScene( + walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(6, 0))], + boxes: [SyntheticScene.Box(min: SIMD3(1, 0, max(0.05, offset - 0.3)), max: SIMD3(4.5, 1, offset))]) + } + + /// Every 0.5 m from x = -1.5 to 4.5, the phone 1 m from the box's front, aimed at the wall + /// low and high, straight on and about 35 degrees to either side. + static func walk(offset: Float) -> [CameraFrame] { + let z = offset + 1 + return Swift.stride(from: Float(-1.5), through: 4.5, by: 0.5).flatMap { x in + [Float(0), -0.75 * z, 0.75 * z].flatMap { dx in + [Float(0.5), 1.5].map { y in lidarCamera(at: SIMD3(x, 1.2, z), lookingAt: SIMD3(x + dx, y, 0)) } + } + } + } + + static func map(_ scene: SyntheticScene, _ cameras: [CameraFrame], config: Map3DConfig = Map3DConfig()) -> Map3D { + var map = Map3D(frame: sceneFrame(), config: config) + for (index, camera) in cameras.enumerated() { map.integrate(scene.estimatedFrame(from: camera, sigma: sigma, seed: UInt64(index + 1))) } + return map + } + + static func covers(_ map: Map3D, _ spans: [ClosedRange], _ index: Int) -> Bool { + let middle = (map.cellRange(index).lowerBound + map.cellRange(index).upperBound) / 2 + return spans.contains { $0.contains(middle) } + } + + static func wallSamples(_ map: Map3D, _ index: Int) -> [SIMD3] { + let range = map.cellRange(index) + let width = range.upperBound - range.lowerBound + let heights = Array(Swift.stride(from: Float(0.1), to: 1.9812, by: 0.2)) + [1.9812] + return [0.25, 0.75].flatMap { f in heights.map { SIMD3(range.lowerBound + f * width, $0, 0) } } + } + + /// Cells with a wall sample no camera saw, even within a voxel. + static func hiddenCells(_ map: Map3D, _ scene: SyntheticScene, _ cameras: [CameraFrame]) -> [Int] { + map.cellIndices.filter { index in + wallSamples(map, index).contains { !nearlyVisible(scene, cameras, $0, normal: SIMD3(0, 0, 1), u: SIMD3(1, 0, 0), v: SIMD3(0, 1, 0)) } + } + } + + @Test(arguments: [Float(0.2), 0.5, 1.0]) + func wallHiddenBehindAnOccluderIsNeverClaimed(offset: Float) { + let scene = Self.occluderScene(offset: offset) + let cameras = Self.walk(offset: offset) + let map = Self.map(scene, cameras) + let coverage = map.coverage(along: Self.wall) + let hidden = Self.hiddenCells(map, scene, cameras) + #expect(hidden.contains { abs(map.cellRange($0).lowerBound - 2.75) < 0.3 }, "no hidden cell behind the box: \(hidden)") + let claimed = hidden.filter { Self.covers(map, coverage.wall, $0) } + #expect(claimed.isEmpty, "hidden cells claimed seen: \(claimed.map { map.cellRange($0) })") + // Height too: a wall span with a height claims the face seen up to it. + for span in coverage.wallHeight { + for index in map.cellIndices where span.span.contains((map.cellRange(index).lowerBound + map.cellRange(index).upperBound) / 2) { + let range = map.cellRange(index) + for f: Float in [0.25, 0.75] { + for height in Swift.stride(from: Float(0.1), through: span.out, by: 0.1) { + let p = SIMD3(range.lowerBound + f * (range.upperBound - range.lowerBound), height, 0) + #expect(nearlyVisible(scene, cameras, p, normal: SIMD3(0, 0, 1), u: SIMD3(1, 0, 0), v: SIMD3(0, 1, 0)), "cell \(index) seen to \(span.out) but not at \(height)") + } + } + } + } + } + + /// Estimated evidence never certifies coverage (`VoxelGrid.isWellSeenSurface`, since the + /// review of 2f17d67), so the same walks claim not even the wall away from the box. The + /// tests here then hold vacuously; they stay to catch estimated claims coming back. + @Test(arguments: [Float(0.2), 0.5, 1.0]) + func estimatedDepthClaimsNotEvenTheClearWall(offset: Float) { + let scene = Self.occluderScene(offset: offset) + let map = Self.map(scene, Self.walk(offset: offset)) + let coverage = map.coverage(along: Self.wall) + let clear = map.cellIndices.filter { let r = map.cellRange($0); return r.lowerBound >= -0.8 && r.upperBound <= 0.4 } + let seen = clear.filter { Self.covers(map, coverage.wall, $0) } + #expect(seen.isEmpty, "clear cells claimed from estimated depth: \(seen.count) of \(clear.count)") + } + + @Test(arguments: [Float(0.2), 0.5, 1.0]) + func groundFacingAndOverheadNeverClaimWhatNoCameraSaw(offset: Float) { + let scene = Self.occluderScene(offset: offset) + let cameras = Self.walk(offset: offset) + let map = Self.map(scene, cameras) + let coverage = map.coverage(along: Self.wall) + for span in coverage.ground { + for index in map.cellIndices where span.span.contains((map.cellRange(index).lowerBound + map.cellRange(index).upperBound) / 2) { + let range = map.cellRange(index) + for out in Swift.stride(from: Float(0.1), through: span.out, by: 0.1524) { + let point = SIMD3(range.lowerBound + 0.5 * (range.upperBound - range.lowerBound), 0, out) + #expect(nearlyVisible(scene, cameras, point, normal: SIMD3(0, 1, 0), u: SIMD3(1, 0, 0), v: SIMD3(0, 0, 1)), "ground \(point) claimed seen, reach \(span.out) over \(span.span)") + } + } + } + let box = scene.boxes[0] + for index in map.cellIndices { + let range = map.cellRange(index) + guard range.upperBound > box.min.x, range.lowerBound < box.max.x else { continue } + #expect((map.facingReach(cell: index, along: Self.wall) ?? 0) < box.max.z, "facing at cell \(index) passes the box") + if box.min.z < map.config.overheadDepth { #expect(map.overheadReach(cell: index, along: Self.wall) == nil, "overhead at cell \(index) over the box") } + } + } + + /// The bush scene (0.4 to 1.0 m out, 0.9 m tall) walked 1 m from the bush's front. + @Test func wallBehindTheBushIsNeverClaimed() { + let scene = bushScene() + let cameras = Self.walk(offset: 1.0) + let map = Self.map(scene, cameras) + let coverage = map.coverage(along: Self.wall) + let hidden = Self.hiddenCells(map, scene, cameras) + #expect(!hidden.isEmpty) + let claimed = hidden.filter { Self.covers(map, coverage.wall, $0) } + #expect(claimed.isEmpty, "hidden cells claimed seen: \(claimed.map { map.cellRange($0) })") + } + + /// Pilasters 0.36 m proud: every claimed wall sample has a truly visible facade face, wall + /// or pilaster, straight out from it. The wall beside a pilaster that the pilaster hides + /// from the oblique views must not be claimed from strays. + @Test func pilasterFacadeClaimsOnlyVisibleFaces() { + let scene = pilasterScene() + let cameras = Swift.stride(from: Float(-4), through: 6, by: 0.5).flatMap { x in + [Float(0), -1.2, 1.2].flatMap { dx in [Float(0.5), 1.5].map { y in lidarCamera(at: SIMD3(x, 1.2, 1.6), lookingAt: SIMD3(x + dx, y, 0)) } } + } + let map = Self.map(scene, cameras) + let coverage = map.coverage(along: Self.wall) + var checked = 0 + for index in map.cellIndices where Self.covers(map, coverage.wall, index) { + let range = map.cellRange(index) + for s in [range.lowerBound + 0.03, range.upperBound - 0.03] { + for height: Float in [0.3, 1.0, 1.9] { + checked += 1 + let visible = [Float(0), 0.36].contains { out in + [Float(-0.1), 0, 0.1].contains { ds in + let p = SIMD3(s + ds, height, out + 1e-3) + return scene.intersect(origin: p + SIMD3(0, 0, 0.01), direction: SIMD3(0, 0, -1)).map { $0.t < 0.02 } == true + && cameras.contains { scene.isVisible(p, normal: SIMD3(0, 0, 1), from: $0) } + } + } + #expect(visible, "cell \(index) s \(s) h \(height) claimed without a visible face") + } + } + } + // Estimated evidence certifies nothing, so nothing is claimed at all. + #expect(checked == 0, "\(checked) wall samples claimed from estimated depth") + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DFootTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DFootTests.swift new file mode 100644 index 000000000..6c2a205d5 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DFootTests.swift @@ -0,0 +1,42 @@ +import HouseScanKit +import simd +import Testing + +/// Ground is judged from the first voxel past the facade's face, but scene.json reports it out +/// from the wall's line. The strip between is reported only where the facade shows at its foot. +@Suite struct Map3DFootTests { + /// A straight wall along x with its face at z = `dz` and a box at its foot over x 1 to 2, + /// walked like the bush scene. `dz` 0.05 puts the face mid-voxel, where a low box merges with + /// the facade's voxels. + static func map(box: (min: SIMD3, max: SIMD3)?, dz: Float = 0.05) -> (Map3D, WallFrame) { + let wall = WallFrame(meter: SIMD3(0, 1.2, dz), outward: SIMD3(0, 0, 1), groundY: 0)! + let boxes = box.map { [SyntheticScene.Box(min: $0.min + SIMD3(0, 0, dz), max: $0.max + SIMD3(0, 0, dz))] } ?? [] + let scene = SyntheticScene(walls: [SyntheticScene.Wall(a: SIMD2(-5, dz), b: SIMD2(6, dz))], boxes: boxes) + var map = Map3D(frame: sceneFrame()) + for x in Swift.stride(from: Float(-3), through: 5, by: 0.3) { + for target in [SIMD3(x, 1.0, dz), SIMD3(x, 0, 0.9 + dz)] { + map.integrate(scene.depthFrame(from: lidarCamera(at: SIMD3(x, 1.4, 2.5 + dz), lookingAt: target))) + } + } + return (map, wall) + } + + static func cellsOverTheBox(_ map: Map3D) -> [Int] { + map.cellIndices.filter { map.cellRange($0).lowerBound >= 1.0 && map.cellRange($0).upperBound <= 2.0 } + } + + @Test func aLowObjectAtTheFootLeavesNoGroundReported() { + // 0.1 m tall, 0.05 m out from the wall. + let (map, wall) = Self.map(box: (SIMD3(1, 0, 0), SIMD3(2, 0.1, 0.05))) + for index in Self.cellsOverTheBox(map) { + #expect(map.groundReach(cell: index, along: wall) == nil, "ground claimed over the box at cell \(index)") + } + } + + @Test func aClearFootStillReportsTheGround() { + let (map, wall) = Self.map(box: nil) + for index in Self.cellsOverTheBox(map) { + #expect((map.groundReach(cell: index, along: wall) ?? 0) >= 1.5, "clear ground at cell \(index) not reported") + } + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DInputTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DInputTests.swift new file mode 100644 index 000000000..434a95099 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DInputTests.swift @@ -0,0 +1,231 @@ +import Foundation +import HouseScanKit +import simd +import Testing + +// One kind of input at a time: a single LiDAR frame, frames from a phone without LiDAR, +// estimated depth, and mesh chunks. +@Suite struct Map3DInputTests { + /// One frame of a bare wall from 2.5 m out, aimed straight at it 1.4 m up. + @Test func oneFrameSeparatesUnknownFreeAndSurface() { + let scene = SyntheticScene(walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(5, 0))]) + let camera = lidarCamera(at: SIMD3(0, 1.4, 2.5), lookingAt: SIMD3(0, 1.4, 0)) + var map = Map3D(frame: sceneFrame()) + #expect(map.state(at: SIMD3(0, 1.4, 1.5)) == .unknown) + map.integrate(scene.depthFrame(from: camera)) + // On the face, between the camera and the face, behind the wall, and outside the view + // (26.5 degrees either side across a portrait image: 0.75 m either side 1 m from the camera). + #expect(map.state(at: SIMD3(0, 1.4, 0)) == .surface) + #expect(map.state(at: SIMD3(0.3, 1.0, 0)) == .surface) + #expect(map.state(at: SIMD3(0, 1.4, 1.5)) == .free) + #expect(map.state(at: SIMD3(0, 1.4, 0.3)) == .free) + #expect(map.state(at: SIMD3(0, 1.4, -0.3)) == .unknown) + #expect(map.state(at: SIMD3(-2, 1.4, 1.5)) == .unknown) + // Evidence on the face: one frame, 2.5 m away, seen head on, facing the camera. + let evidence = map.evidence(at: SIMD3(0, 1.4, 0)) + #expect(evidence.hits == 1) + #expect(evidence.passes == 0) + #expect(evidence.nearestDistance.map { abs($0 - 2.5) < 0.05 } == true) + // The cosine is stored in 1/255 steps, rounded down, which near square on is about 5 degrees. + #expect(evidence.bestViewAngle.map { $0 < 6 * .pi / 180 } == true) + #expect(evidence.normal.map { simd_dot($0, SIMD3(0, 0, 1)) > 0.99 } == true) + #expect(evidence.sources == .lidar) + // One frame counts once however many of its rays pass through a voxel. + #expect(map.evidence(at: SIMD3(0, 1.4, 1.5)).passes == 1) + } + + @Test func lowConfidenceAndOutOfRangeDepthIsIgnored() { + let scene = SyntheticScene(walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(5, 0))]) + let camera = lidarCamera(at: SIMD3(0, 1.4, 2.5), lookingAt: SIMD3(0, 1.4, 0)) + let frame = scene.depthFrame(from: camera) + var low = Map3D(frame: sceneFrame()) + low.integrate(DepthFrame(camera: camera, width: frame.width, height: frame.height, depth: frame.depth, kind: .lidar(confidence: [UInt8](repeating: 0, count: frame.depth.count)))) + #expect(low.state(at: SIMD3(0, 1.4, 0)) == .unknown) + #expect(low.state(at: SIMD3(0, 1.4, 1.5)) == .unknown) + var far = Map3D(frame: sceneFrame()) + let distant = lidarCamera(at: SIMD3(0, 1.4, 6), lookingAt: SIMD3(0, 1.4, 0)) + far.integrate(scene.depthFrame(from: distant)) + #expect(far.state(at: SIMD3(0, 1.4, 0)) == .unknown) + } + + /// Without LiDAR: feature points on everything the camera sees (the bush included, one + /// every 5 cm or so) and the wall and ground as detected planes. + @Test func planesAndFeaturePointsMakeAMapThatHonoursTheBush() throws { + let scene = bushScene() + var map = Map3D(frame: sceneFrame()) + // The wall plane: local y (its normal) is +z, local x is +x, so local z is -y. + let wallPlane = PlaneObservation( + id: UUID(), worldFromPlane: simd_float4x4(SIMD4(1, 0, 0, 0), SIMD4(0, 0, 1, 0), SIMD4(0, -1, 0, 0), SIMD4(0, 0, 0, 1)), + alignment: .vertical, center: SIMD2(0.5, -1.5), width: 11, length: 3) + let groundPlane = PlaneObservation( + id: UUID(), worldFromPlane: matrix_identity_float4x4, alignment: .horizontal, center: SIMD2(0.5, 3), width: 13, length: 6) + map.update(wallPlane) + map.update(groundPlane) + let cameras = bushWalk().map { CameraFrame(cameraToWorld: $0.cameraToWorld, intrinsics: SIMD4(1450, 1450, 960, 720), imageSize: SIMD2(1920, 1440)) } + for camera in cameras { + map.integrate(FeatureFrame(camera: camera, points: scene.featurePoints(from: camera, columns: 32, rows: 24))) + } + #expect(map.state(at: SIMD3(-1, 1.4, 0)) == .surface) + #expect(map.evidence(at: SIMD3(-1, 1.4, 0)).sources.contains(.plane)) + #expect(map.state(at: SIMD3(-1, 1.4, 1.5)) == .free) + #expect(map.state(at: SIMD3(-1, 1.4, -0.5)) == .unknown) + + let chain = try #require(map.measuredWalls()) + #expect(chain.walls.count == 1) + #expect(chain.walls.allSatisfy { $0.source == .plane && abs($0.start.y) < 0.06 && abs($0.end.y) < 0.06 }) + + let wall = standardWall() + let coverage = map.coverage(along: wall) + let behindBush = map.cellIndices.filter { map.cellRange($0).lowerBound >= 1.1 && map.cellRange($0).upperBound <= 1.9 } + for index in behindBush { + let middle = (map.cellRange(index).lowerBound + map.cellRange(index).upperBound) / 2 + #expect(!coverage.wall.contains { $0.contains(middle) }, "cell \(index) behind the bush claimed seen") + #expect(map.groundReach(cell: index, along: wall) == nil) + } + let open = map.cellIndices.filter { map.cellRange($0).lowerBound >= -2 && map.cellRange($0).upperBound <= 0.6 } + #expect(open.allSatisfy { index in coverage.wall.contains { $0.contains(map.cellRange(index).lowerBound + 0.07) } }) + } + + /// Estimated depth with 3 % error: a surface only where two deviations are within 15 cm + /// (2.5 m and nearer), free space short of that, and never a wall. + @Test func estimatedDepthCarvesWhatItIsSureOf() { + let scene = SyntheticScene(walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(5, 0))]) + let near = lidarCamera(at: SIMD3(0, 1.4, 2.0), lookingAt: SIMD3(0, 1.4, 0)) + let far = lidarCamera(at: SIMD3(3, 1.4, 4.0), lookingAt: SIMD3(3, 1.4, 0)) + var map = Map3D(frame: sceneFrame()) + for camera in [near, far] { + let lidar = scene.depthFrame(from: camera) + map.integrate(DepthFrame(camera: camera, width: lidar.width, height: lidar.height, depth: lidar.depth, kind: .estimated(sigma: lidar.depth.map { $0 * 0.03 }))) + } + #expect(map.state(at: SIMD3(0, 1.4, 0)) == .surface) + #expect(map.evidence(at: SIMD3(0, 1.4, 0)).sources == .estimated) + #expect(map.state(at: SIMD3(0, 1.4, 1.0)) == .free) + // 4 m away the deviation is 12 cm: no surface, and free space stops 24 cm short. + #expect(map.state(at: SIMD3(3, 1.4, 0)) == .unknown) + #expect(map.state(at: SIMD3(3, 1.4, 0.1)) == .unknown) + #expect(map.state(at: SIMD3(3, 1.4, 1.0)) == .free) + #expect(map.measuredWalls() == nil) + } + + /// A wall and a floor as mesh chunks: the mesh labels voxels and gives them normals but no + /// occupancy, and replacing a chunk clears what its old faces labelled. + @Test func meshChunksLabelVoxelsAndReplaceCleanly() { + var map = Map3D(frame: sceneFrame()) + let id = UUID() + func chunk(atZ z: Float) -> MeshChunk { + MeshChunk( + id: id, worldFromChunk: matrix_identity_float4x4, + vertices: [SIMD3(-1, 0, z), SIMD3(1, 0, z), SIMD3(1, 2, z), SIMD3(-1, 2, z), SIMD3(-1, 0, 1), SIMD3(1, 0, 1)], + faces: [SIMD3(0, 1, 2), SIMD3(0, 2, 3), SIMD3(0, 4, 5), SIMD3(0, 5, 1)], + classes: [.wall, .wall, .floor, .floor]) + } + map.update(chunk(atZ: 0)) + let face = map.evidence(at: SIMD3(0, 1, 0)) + #expect(face.meshClass == .wall) + #expect(face.sources == .mesh) + #expect(face.state == .unknown) + #expect(face.normal.map { abs($0.z) > 0.99 } == true) + #expect(map.evidence(at: SIMD3(0, 0, 0.5)).meshClass == .floor) + map.update(chunk(atZ: -0.5)) + #expect(map.evidence(at: SIMD3(0, 1, 0)).meshClass == nil) + #expect(map.evidence(at: SIMD3(0, 1, -0.5)).meshClass == .wall) + map.removeMeshChunk(id: id) + #expect(map.evidence(at: SIMD3(0, 1, -0.5)).meshClass == nil) + } + + /// The map moves with the meter's anchor: a map point keeps its place relative to it. + @Test func frameFollowsTheMeterAnchor() { + let frame = sceneFrame() + let old = simd_float4x4(SIMD4(1, 0, 0, 0), SIMD4(0, 1, 0, 0), SIMD4(0, 0, 1, 0), SIMD4(0, 1.5, 0, 1)) + // ARKit refines the anchor: 5 cm along x and 2 degrees about +y. + let turn = simd_float4x4(simd_quatf(angle: 2 * .pi / 180, axis: SIMD3(0, 1, 0))) + var new = turn * old + new.columns.3 = SIMD4(0.05, 1.5, 0, 1) + let moved = frame.following(anchorMovedFrom: old, to: new) + let point = SIMD3(2, 1, 1) + let relative = old.inverse * SIMD4(frame.world(point), 1) + let expected = new * relative + #expect(simd_distance(moved.world(point), SIMD3(expected.x, expected.y, expected.z)) < 1e-4) + #expect(abs(moved.worldDirection(SIMD3(0, 1, 0)).y - 1) < 1e-5) + } + + // MARK: Capture packet conventions (packet/README.md on t3/packet) + + /// The map frame is the packet's meter frame: origin at the meter anchor, +y up, +z the + /// wall's outward normal, +x = y × z, and the ground's height below the anchor. + @Test func mapFrameIsThePacketMeterFrame() throws { + let frame = try #require(MapFrame(meter: SIMD3(2, 1.5, -1), outward: SIMD3(1, 0.2, 0), worldGroundY: 0.1)) + #expect(simd_distance(frame.world(.zero), SIMD3(2, 1.5, -1)) < 1e-5) + #expect(abs(frame.groundY - -1.4) < 1e-5) + let x = frame.worldDirection(SIMD3(1, 0, 0)) + let y = frame.worldDirection(SIMD3(0, 1, 0)) + let z = frame.worldDirection(SIMD3(0, 0, 1)) + #expect(simd_distance(z, SIMD3(1, 0, 0)) < 1e-5) + #expect(simd_distance(y, SIMD3(0, 1, 0)) < 1e-5) + #expect(simd_distance(x, simd_cross(y, z)) < 1e-5) + // The packet's meter_anchor fields give the same frame back. + let packet = MapFrame(poseInWorld: frame.poseInWorld, groundY: frame.groundY) + #expect(packet == frame) + } + + /// Where the anchor sits changes nothing the map reports along the wall: the bush scene's + /// coverage with the anchor on the meter 1.5 m up equals it with the anchor on the ground, + /// ground reach to within a row. + @Test func coverageDoesNotDependOnTheAnchorHeight() throws { + let scene = bushScene() + let raised = try #require(MapFrame(meter: SIMD3(0, 1.5, 0), outward: SIMD3(0, 0, 1), worldGroundY: 0)) + var onMeter = Map3D(frame: raised) + var onGround = Map3D(frame: sceneFrame()) + for camera in bushWalk() { + let frame = scene.depthFrame(from: camera) + onMeter.integrate(frame) + onGround.integrate(frame) + } + let a = onMeter.coverage(along: standardWall()) + let b = onGround.coverage(along: standardWall()) + #expect(a.wall == b.wall && a.facing == b.facing && a.overhead == b.overhead) + // Ground at the ends of the walk rests on rays that just cross a voxel or just stop in + // it, which float rounding in either frame can tip: at most one row apart. + for index in onGround.cellIndices { + let reaches = [onMeter, onGround].map { $0.groundReach(cell: index, along: standardWall()) } + #expect(abs((reaches[0] ?? -0.1524) - (reaches[1] ?? -0.1524)) <= 0.1525, "cell \(index): \(reaches)") + } + #expect(onMeter.state(at: SIMD3(-1, -0.1, 0)) == .surface) + } + + /// A depth image aligned to a photo: depth pixel (c, r) covers photo pixels c W / w to + /// (c + 1) W / w, so a point lands on the depth pixel under its photo pixel. + @Test func depthIntrinsicsFollowThePacketAlignment() { + let photo = CameraFrame( + cameraToWorld: lidarCamera(at: SIMD3(0, 1.4, 2.5), lookingAt: SIMD3(0.3, 1.1, 0)).cameraToWorld, + intrinsics: SIMD4(1450, 1452, 962, 718), imageSize: SIMD2(1920, 1440)) + let depth = DepthFrame(photo: photo, width: 256, height: 192, depth: [Float](repeating: 0, count: 256 * 192), kind: .lidar(confidence: nil)) + for point in [SIMD3(0.3, 1.1, 0), SIMD3(-0.4, 0.2, 0), SIMD3(0.8, 2.0, 0)] { + let a = photo.pixel(of: point)! + let b = depth.camera.pixel(of: point)! + #expect(simd_distance(a / 7.5, b) < 1e-3, "photo \(a), depth \(b)") + } + } + + /// A packet's merged mesh is in the meter frame: fed as one chunk placed at the anchor, its + /// faces label the voxels at their meter-frame positions. + @Test func aPacketMeshIsOneChunkInTheMeterFrame() throws { + let frame = try #require(MapFrame(meter: SIMD3(3, 1.5, 2), outward: SIMD3(-1, 0, 0), worldGroundY: 0)) + var map = Map3D(frame: frame) + map.update(MeshChunk( + id: UUID(), worldFromChunk: frame.poseInWorld, + vertices: [SIMD3(-1, -1.5, 0), SIMD3(1, -1.5, 0), SIMD3(1, 0.5, 0), SIMD3(-1, 0.5, 0)], + faces: [SIMD3(0, 1, 2), SIMD3(0, 2, 3)], classes: [.wall, .wall])) + #expect(map.evidence(at: SIMD3(0.5, -0.5, 0)).meshClass == .wall) + #expect(map.evidence(at: SIMD3(0.5, -0.5, 0.3)).meshClass == nil) + } + + @Test func lidarDepthWithoutConfidenceIsTakenAsHigh() { + let scene = SyntheticScene(walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(5, 0))]) + let camera = lidarCamera(at: SIMD3(0, 1.4, 2.5), lookingAt: SIMD3(0, 1.4, 0)) + let frame = scene.depthFrame(from: camera) + var map = Map3D(frame: sceneFrame()) + map.integrate(DepthFrame(camera: camera, width: frame.width, height: frame.height, depth: frame.depth, kind: .lidar(confidence: nil))) + #expect(map.state(at: SIMD3(0, 1.4, 0)) == .surface) + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DOcclusionTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DOcclusionTests.swift new file mode 100644 index 000000000..5586514e3 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DOcclusionTests.swift @@ -0,0 +1,151 @@ +import Foundation +import HouseScanKit +import simd +import Testing + +// The bush scene (SyntheticScene.swift): a straight wall with a bush 0.9 m tall standing 0.4 to +// 1.0 m in front of it between s = 1 and 2, walked every 0.3 m at 2.5 m out, depth off by up to +// 2 cm. Every coverage claim is checked against exact ray casting of the scene from the same +// cameras. +@Suite struct Map3DOcclusionTests { + static let scene = bushScene() + static let cameras = bushWalk() + static let map: Map3D = { + var map = Map3D(frame: sceneFrame()) + for (index, camera) in cameras.enumerated() { + map.integrate(scene.depthFrame(from: camera, noise: 0.02, seed: UInt64(index))) + } + return map + }() + static let wall = standardWall() + static let coverage = map.coverage(along: wall) + + /// Whether any point within a voxel (0.1 m) of `point` along the two directions `u` and `v` + /// on its surface is truly visible from some camera: the resolution coverage is judged at. + static func nearlyVisible(_ point: SIMD3, normal: SIMD3, u: SIMD3, v: SIMD3) -> Bool { + for du: Float in [-0.1, 0, 0.1] { + for dv: Float in [-0.1, 0, 0.1] { + let p = point + u * du + v * dv + normal * 1e-3 + if cameras.contains(where: { scene.isVisible(p, normal: normal, from: $0) }) { return true } + } + } + return false + } + + static func wallSamples(_ index: Int) -> [SIMD3] { + let range = map.cellRange(index) + let width = range.upperBound - range.lowerBound + let heights = Array(Swift.stride(from: Float(0.1), to: 1.9812, by: 0.2)) + [1.9812] + return [0.25, 0.75].flatMap { f in heights.map { SIMD3(range.lowerBound + f * width, $0, 0) } } + } + + static func covers(_ spans: [ClosedRange], _ index: Int) -> Bool { + let middle = (map.cellRange(index).lowerBound + map.cellRange(index).upperBound) / 2 + return spans.contains { $0.contains(middle) } + } + + @Test func everySeenWallCellWasTrulyVisible() { + var checked = 0 + for index in Self.map.cellIndices where Self.covers(Self.coverage.wall, index) { + checked += 1 + for sample in Self.wallSamples(index) { + #expect(Self.nearlyVisible(sample, normal: SIMD3(0, 0, 1), u: SIMD3(1, 0, 0), v: SIMD3(0, 1, 0)), "cell \(index) sample \(sample)") + } + } + #expect(checked >= 40, "only \(checked) wall cells seen") + } + + @Test func wallHiddenBehindTheBushIsNeverSeen() { + // Cells with some sample no camera saw even within a voxel: the band must be seen from + // the ground to headroom, so each of these is unseen. + let hidden = Self.map.cellIndices.filter { index in + Self.wallSamples(index).contains { !Self.nearlyVisible($0, normal: SIMD3(0, 0, 1), u: SIMD3(1, 0, 0), v: SIMD3(0, 1, 0)) } + } + let behindBush = hidden.filter { abs(Self.map.cellRange($0).lowerBound - 1.5) < 1 } + #expect(behindBush.count >= 3, "cells hidden behind the bush: \(behindBush)") + for index in hidden { + #expect(!Self.covers(Self.coverage.wall, index), "cell \(index) has hidden wall but was claimed seen") + } + } + + @Test func wallAwayFromTheBushIsSeen() { + for index in Self.map.cellIndices { + let range = Self.map.cellRange(index) + guard (range.lowerBound >= -2 && range.upperBound <= 0.6) || (range.lowerBound >= 2.6 && range.upperBound <= 4) else { continue } + #expect(Self.covers(Self.coverage.wall, index), "cell \(index) (\(range)) not seen") + } + } + + @Test func groundReachNeverPassesGroundNobodySaw() { + var checked = 0 + for span in Self.coverage.ground { + for index in Self.map.cellIndices where span.span.contains((Self.map.cellRange(index).lowerBound + Self.map.cellRange(index).upperBound) / 2) { + let range = Self.map.cellRange(index) + for out in Swift.stride(from: Float(0.1), through: span.out, by: 0.1524) { + for f: Float in [0.25, 0.75] { + let point = SIMD3(range.lowerBound + f * (range.upperBound - range.lowerBound), 0, out) + #expect(Self.nearlyVisible(point, normal: SIMD3(0, 1, 0), u: SIMD3(1, 0, 0), v: SIMD3(0, 0, 1)), "ground \(point) claimed seen") + checked += 1 + } + } + } + } + #expect(checked > 500) + } + + @Test func groundBehindTheBushIsNotSeenAndGroundElsewhereIs() { + for index in Self.map.cellIndices { + let range = Self.map.cellRange(index) + let reach = Self.map.groundReach(cell: index, along: Self.wall) + if range.lowerBound >= 1.1, range.upperBound <= 1.9 { + #expect(reach == nil, "ground behind the bush at cell \(index) reaches \(reach ?? -1)") + } + if range.lowerBound >= -2, range.upperBound <= 0.6 { + #expect((reach ?? 0) >= 1.5, "ground at cell \(index) reaches only \(reach ?? -1)") + } + } + } + + @Test func facingAndOverheadNeverClaimSpaceTheBushFills() { + let bush = Self.scene.boxes[0] + for index in Self.map.cellIndices { + let range = Self.map.cellRange(index) + let overlapsBush = range.upperBound > bush.min.x && range.lowerBound < bush.max.x + let facing = Self.map.facingReach(cell: index, along: Self.wall) + let overhead = Self.map.overheadReach(cell: index, along: Self.wall) + if overlapsBush { + // The bush fills out 0.4 to 1.0 from 0 to 0.9 m up: a clear reach past 0.4 m out + // (facing) or anything from 0.3 m up over the battery's 0.56 m depth (overhead) + // would claim space it fills. + #expect((facing ?? 0) < bush.min.z, "facing at cell \(index): \(facing ?? -1)") + #expect(overhead == nil, "overhead at cell \(index): \(overhead ?? -1)") + } + if range.lowerBound >= -2, range.upperBound <= 0.6 { + #expect((facing ?? 0) >= 1.0, "facing at cell \(index): \(facing ?? -1)") + #expect((overhead ?? 0) > Self.map.config.headroom, "overhead at cell \(index): \(overhead ?? -1)") + } + } + } + + @Test func coverageFeedsSceneExport() throws { + let scene = SceneCoverage(Self.coverage, leftEndMarked: false, rightEndMarked: false) + #expect(!scene.wall.isEmpty && !scene.ground.isEmpty && !scene.facing.isEmpty && !scene.overhead.isEmpty) + let data = try SceneExport.jsonData(SceneInput(wall: SceneWall(meter: Self.wall.meter, outward: Self.wall.outward, groundY: 0), baselineS: -5...6, coverage: scene)) + #expect(!data.isEmpty) + } + + /// The 2D coverage map counts what the camera pointed at, so it claims the wall behind the + /// bush; the 3D map does not. + @Test func theTwoDimensionalMapClaimsWhatTheBushHides() { + var flat = CoverageMap(wall: Self.wall) + for camera in Self.cameras { flat.observe(camera, trackingNormal: true) } + let hidden = Self.map.cellIndices.filter { index in + let range = Self.map.cellRange(index) + return range.lowerBound >= 1.1 && range.upperBound <= 1.9 + } + let claimedFlat = hidden.filter { Self.covers(flat.coveredIntervals(.wall), $0) } + let claimed3D = hidden.filter { Self.covers(Self.coverage.wall, $0) } + #expect(claimedFlat.count == hidden.count, "2D map claims \(claimedFlat.count) of \(hidden.count)") + #expect(claimed3D.isEmpty) + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DOverClaimTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DOverClaimTests.swift new file mode 100644 index 000000000..cbf8dac7e --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DOverClaimTests.swift @@ -0,0 +1,174 @@ +import Foundation +import HouseScanKit +import simd +import Testing + +// Ways coverage could claim what no ray measured, each reproduced from a review of the map and +// fixed after this test failed. +@Suite struct Map3DOverClaimTests { + static let wall = standardWall() + + static func covers(_ map: Map3D, _ spans: [ClosedRange], _ index: Int) -> Bool { + let middle = (map.cellRange(index).lowerBound + map.cellRange(index).upperBound) / 2 + return spans.contains { $0.contains(middle) } + } + + /// Space seen empty for many frames, then something is measured there: the voxel holding + /// its face is no longer free after the first hit. Five passes saturate the log-odds at + /// -2.0 and one hit adds only 0.85. (Its inside, never measured, keeps what the earlier + /// frames saw: the map assumes nothing moves.) + @Test func aHitEndsFreeSpaceAtOnce() { + let open = SyntheticScene(walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(5, 0))]) + let camera = lidarCamera(at: SIMD3(0, 1, 3), lookingAt: SIMD3(0, 1, 0)) + var map = Map3D(frame: sceneFrame()) + for _ in 0..<6 { map.integrate(open.depthFrame(from: camera)) } + let point = SIMD3(0, 1, 1.8) + #expect(map.state(at: point) == .free) + var blocked = open + blocked.boxes = [SyntheticScene.Box(min: SIMD3(-0.5, 0, 1.5), max: SIMD3(0.5, 2, 1.8))] + map.integrate(blocked.depthFrame(from: camera)) + #expect(map.state(at: point) != .free) + } + + /// A wall seen head on, then many rays grazing along it to a point farther on: the wall's + /// voxels short of that point stay wall, not free. + @Test func grazingRaysDoNotEraseAWall() { + let scene = SyntheticScene(walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(8, 0))]) + var map = Map3D(frame: sceneFrame()) + for x: Float in [1.5, 2.5, 3.5] { map.integrate(scene.depthFrame(from: lidarCamera(at: SIMD3(x, 1, 2), lookingAt: SIMD3(x, 1, 0)))) } + #expect(map.state(at: SIMD3(2.5, 1, 0)) == .surface) + let grazing = lidarCamera(at: SIMD3(0, 1, 0.3), lookingAt: SIMD3(3, 1, 0)) + for _ in 0..<10 { map.integrate(scene.depthFrame(from: grazing)) } + #expect(map.state(at: SIMD3(2.5, 1, 0)) != .free) + #expect(map.state(at: SIMD3(2.0, 1, 0)) != .free) + } + + /// Cell 0 runs from s = 0 to 0.1524; its last 2.4 mm lies in the voxel column centred on + /// 0.2, where the wall has a hole. That column must be checked, so the cell is not seen. + @Test func everyVoxelColumnUnderACellIsChecked() { + let scene = SyntheticScene(walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(0.15, 0)), SyntheticScene.Wall(a: SIMD2(0.35, 0), b: SIMD2(6, 0))]) + var map = Map3D(frame: sceneFrame()) + for camera in bushWalk() { map.integrate(scene.depthFrame(from: camera)) } + let coverage = map.coverage(along: Self.wall) + #expect(Self.covers(map, coverage.wall, -1)) + #expect(!Self.covers(map, coverage.wall, 0)) + } + + /// A box 1.2 m tall whose front is only 14 cm out: its front falls in the voxel layer next + /// to the wall's, and must not stand in for the wall behind it. + @Test func aShallowBoxIsNotTheWall() { + let scene = SyntheticScene( + walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(6, 0))], + boxes: [SyntheticScene.Box(min: SIMD3(1, 0, 0.05), max: SIMD3(2, 1.2, 0.14))]) + var map = Map3D(frame: sceneFrame()) + for camera in bushWalk() { map.integrate(scene.depthFrame(from: camera)) } + let coverage = map.coverage(along: Self.wall) + for index in map.cellIndices where map.cellRange(index).lowerBound >= 1.2 && map.cellRange(index).upperBound <= 1.8 { + #expect(!Self.covers(map, coverage.wall, index), "cell \(index) behind the box claimed seen") + } + #expect(map.cellIndices.filter { map.cellRange($0).lowerBound >= -2 && map.cellRange($0).upperBound <= 0.6 }.allSatisfy { Self.covers(map, coverage.wall, $0) }) + } + + /// The shallow box made 3.5 m wide, wider than the neighbourhood `facadeOffset` takes its + /// mode over (about a meter either side): in its middle more samples meet the box's front + /// than the wall above it, so the mode is the box. No view reaches the wall behind it. + @Test func aWideShallowBoxIsNotTheWall() { + let scene = SyntheticScene( + walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(6, 0))], + boxes: [SyntheticScene.Box(min: SIMD3(1, 0, 0.05), max: SIMD3(4.5, 1.2, 0.14))]) + var map = Map3D(frame: sceneFrame()) + for camera in bushWalk() { map.integrate(scene.depthFrame(from: camera)) } + let coverage = map.coverage(along: Self.wall) + for index in map.cellIndices where map.cellRange(index).lowerBound >= 1.2 && map.cellRange(index).upperBound <= 4.3 { + #expect(!Self.covers(map, coverage.wall, index), "cell \(index) behind the box claimed seen") + #expect(!coverage.wallHeight.contains { $0.span.contains((map.cellRange(index).lowerBound + map.cellRange(index).upperBound) / 2) }, + "cell \(index) behind the box reported with a height") + } + #expect(map.cellIndices.filter { map.cellRange($0).lowerBound >= -2 && map.cellRange($0).upperBound <= 0.6 }.allSatisfy { Self.covers(map, coverage.wall, $0) }) + } + + /// Ground raised 0.2 m under part of the battery's depth and a beam 0.2 to 0.3 m over the + /// rest: the beam is above that ground's clearance, so overhead is not clear from the ground. + @Test func overheadStartsFromEachColumnsOwnGround() { + let scene = SyntheticScene( + walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(6, 0))], + boxes: [ + SyntheticScene.Box(min: SIMD3(1, 0, 0.15), max: SIMD3(2, 0.2, 0.35)), + SyntheticScene.Box(min: SIMD3(1, 0.2, 0.45), max: SIMD3(2, 0.3, 0.6)), + ]) + var map = Map3D(frame: sceneFrame()) + for camera in bushWalk() { map.integrate(scene.depthFrame(from: camera)) } + for index in map.cellIndices where map.cellRange(index).lowerBound >= 1.1 && map.cellRange(index).upperBound <= 1.9 { + let reach = map.overheadReach(cell: index, along: Self.wall) + #expect(reach.map { $0 < 0.2 } ?? true, "cell \(index) overhead \(reach ?? -1)") + } + } + + /// A surface seen square on from beyond range and within range only at a glancing angle: + /// no single view both reached it and saw it well, so it is not seen. + @Test func oneViewMustBeBothNearAndSquareOn() { + let scene = SyntheticScene(walls: [SyntheticScene.Wall(a: SIMD2(-8, 0), b: SIMD2(8, 0))]) + var map = Map3D(frame: sceneFrame()) + // 4.8 m deep on the camera's axis but 5.46 m along the ray to x = 0, 28 degrees off square. + map.integrate(scene.depthFrame(from: lidarCamera(at: SIMD3(2.6, 1, 4.8), lookingAt: SIMD3(2.6, 1, 0)))) + // 2.93 m away but 70 degrees off square. + map.integrate(scene.depthFrame(from: lidarCamera(at: SIMD3(2.75, 1, 1.0), lookingAt: SIMD3(0, 1, 0)))) + let evidence = map.evidence(at: SIMD3(0, 1, 0)) + #expect(evidence.state == .surface) + #expect(!map.isWallSeen(cell: 0, along: Self.wall)) + } + + /// A vertical plane facing +z at `z`, 11 m wide and 3 m tall, as ARKit reports one. + static func wallPlane(id: UUID, z: Float) -> PlaneObservation { + PlaneObservation( + id: id, worldFromPlane: simd_float4x4(SIMD4(1, 0, 0, 0), SIMD4(0, 0, 1, 0), SIMD4(0, -1, 0, 0), SIMD4(0, 0, z, 1)), + alignment: .vertical, center: SIMD2(0.5, -1.5), width: 11, length: 3) + } + + /// Frames from a phone without LiDAR that tracked no points: only the planes speak. + static func emptyFrames() -> [FeatureFrame] { + bushWalk().map { FeatureFrame(camera: CameraFrame(cameraToWorld: $0.cameraToWorld, intrinsics: SIMD4(1450, 1450, 960, 720), imageSize: SIMD2(1920, 1440)), points: []) } + } + + /// ARKit moves a plane from 0.3 m out to the wall: the old position leaves nothing behind. + @Test func aMovedPlaneTakesItsMarksWithIt() { + var map = Map3D(frame: sceneFrame()) + let id = UUID() + map.update(Self.wallPlane(id: id, z: 0.3)) + for frame in Self.emptyFrames() { map.integrate(frame) } + #expect(map.evidence(at: SIMD3(0, 1, 0.3)).sources.contains(.plane)) + map.update(Self.wallPlane(id: id, z: 0)) + for frame in Self.emptyFrames() { map.integrate(frame) } + #expect(!map.evidence(at: SIMD3(0, 1, 0.3)).sources.contains(.plane)) + #expect(map.evidence(at: SIMD3(0, 1, 0)).sources.contains(.plane)) + map.removePlane(id: id) + #expect(!map.evidence(at: SIMD3(0, 1, 0)).sources.contains(.plane)) + } + + /// A plane with nothing measured on it clears no fog: nobody has seen what stands there. + @Test func planesAloneClearNoFog() { + var map = Map3D(frame: sceneFrame()) + map.update(Self.wallPlane(id: UUID(), z: 0)) + for frame in Self.emptyFrames() { map.integrate(frame) } + #expect(map.fogOfWar(along: Self.wall).seen == 0) + #expect(map.state(at: SIMD3(0, 1, 0)) == .unknown) + } + + /// Two mesh chunks cover one voxel; removing one keeps the other's label there. + @Test func removingOneMeshChunkKeepsTheOthers() { + var map = Map3D(frame: sceneFrame()) + func chunk(_ id: UUID, x: Float) -> MeshChunk { + MeshChunk( + id: id, worldFromChunk: matrix_identity_float4x4, + vertices: [SIMD3(x - 0.5, 0, 0), SIMD3(x + 0.5, 0, 0), SIMD3(x + 0.5, 2, 0), SIMD3(x - 0.5, 2, 0)], + faces: [SIMD3(0, 1, 2), SIMD3(0, 2, 3)], classes: [.wall, .wall]) + } + let a = UUID() + map.update(chunk(a, x: 0)) + map.update(chunk(UUID(), x: 0.4)) + map.removeMeshChunk(id: a) + #expect(map.evidence(at: SIMD3(0.2, 1, 0)).meshClass == .wall) + #expect(map.evidence(at: SIMD3(-0.3, 1, 0)).meshClass == nil) + } +} + diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DPerformanceTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DPerformanceTests.swift new file mode 100644 index 000000000..72e43afc2 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DPerformanceTests.swift @@ -0,0 +1,96 @@ +import Darwin +import Foundation +import HouseScanKit +import simd +import Testing + +// Timing and memory. Times are printed as MAP3D_PERF lines; the frame-time bound is only held in +// optimized builds (swift test -c release), since a debug build is many times slower. +@Suite(.serialized) struct Map3DPerformanceTests { + static func milliseconds(_ duration: Duration) -> Double { + Double(duration.components.seconds) * 1000 + Double(duration.components.attoseconds) / 1e15 + } + + static let optimized: Bool = { + #if DEBUG + false + #else + true + #endif + }() + + /// Median and fastest of `runs` calls. + static func time(runs: Int, _ body: () -> Void) -> (median: Double, fastest: Double) { + let clock = ContinuousClock() + let times = (0.. Void)] { + let result = Self.time(runs: Self.optimized ? 3 : 1, body) + print("MAP3D_PERF \(name) after \(bushWalk().count) frames: median \(String(format: "%.1f", result.median)) ms") + } + } + + /// A 30 x 10 x 5 m region (x, z, y), filled by frames from all over it looking four ways: + /// storage never passes the bounds' worst case, and integrating frames again stores + /// nothing more. + @Test func memoryIsBoundedForA30By10By5Region() { + let bounds = MapBounds(min: SIMD3(-15, -1, -1), max: SIMD3(15, 4, 9)) + var map = Map3D(frame: sceneFrame(), bounds: bounds) + let worst = map.worstCaseBytes + #expect(worst <= 32 << 20, "worst case \(worst) bytes") + let scene = SyntheticScene( + walls: SyntheticScene.chain([SIMD2(-15, 9), SIMD2(-15, 0), SIMD2(15, 0), SIMD2(15, 9), SIMD2(-15, 9)]), + boxes: [SyntheticScene.Box(min: SIMD3(-8, 0, 3), max: SIMD3(-6, 1.5, 5)), SyntheticScene.Box(min: SIMD3(5, 0, 6), max: SIMD3(7, 2, 7))]) + let cameras = Swift.stride(from: Float(-12), through: 12, by: 6).flatMap { x in + [Float(2), 5, 8].flatMap { z in + [SIMD3(1, 0, 0), SIMD3(-1, 0, 0), SIMD3(0, 0, 1), SIMD3(0, 0, -1)].map { direction in + lidarCamera(at: SIMD3(x, 1.4, z), lookingAt: SIMD3(x, 1.0, z) + direction) + } + } + } + let frames = cameras.map { scene.depthFrame(from: $0) } + for frame in frames { map.integrate(frame) } + let first = map.allocatedBytes + for frame in frames.prefix(12) { map.integrate(frame) } + #expect(map.allocatedBytes == first) + #expect(first <= worst) + var info = task_vm_info_data_t() + var count = mach_msg_type_number_t(MemoryLayout.size / MemoryLayout.size) + let status = withUnsafeMutablePointer(to: &info) { + $0.withMemoryRebound(to: integer_t.self, capacity: Int(count)) { task_info(mach_task_self_, task_flavor_t(TASK_VM_INFO), $0, &count) } + } + let footprint = status == KERN_SUCCESS ? "\(info.phys_footprint >> 20) MB" : "unknown" + print("MAP3D_PERF 30x10x5 m region, \(frames.count) frames: map holds \(first >> 20) MB (\(first) bytes), worst case \(worst >> 20) MB (\(worst) bytes); test process footprint \(footprint)") + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DReliefTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DReliefTests.swift new file mode 100644 index 000000000..5361d2ac9 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DReliefTests.swift @@ -0,0 +1,125 @@ +import Foundation +import HouseScanKit +import simd +import Testing + +// A facade with pilasters 0.36 m proud (ETH3D electro's), walked with up to 2 cm of depth error. +// The wall behind a pilaster can't be seen from anywhere and nothing can be mounted there: the +// pilaster's own face covers it. A freestanding hedge with a gap behind it does not. +@Suite struct Map3DReliefTests { + static let wall = standardWall() + static let scene = pilasterScene() + static let map: Map3D = { + var map = Map3D(frame: sceneFrame()) + for (index, camera) in pilasterWalk().enumerated() { map.integrate(scene.depthFrame(from: camera, noise: 0.02, seed: UInt64(index))) } + return map + }() + + static func covers(_ map: Map3D, _ spans: [ClosedRange], _ index: Int) -> Bool { + let middle = (map.cellRange(index).lowerBound + map.cellRange(index).upperBound) / 2 + return spans.contains { $0.contains(middle) } + } + + /// Every cell between the first and last pilaster is seen, those behind pilasters included: + /// no view can exist of the wall behind them, so a request for one could never be met. + @Test func wallBehindPilastersIsCoveredByTheirFaces() { + let coverage = Self.map.coverage(along: Self.wall) + let cells = Self.map.cellIndices.filter { Self.map.cellRange($0).lowerBound >= -4.25 && Self.map.cellRange($0).upperBound <= 5.75 } + let unseen = cells.filter { !Self.covers(Self.map, coverage.wall, $0) } + #expect(unseen.isEmpty, "unseen cells \(unseen.map { Self.map.cellRange($0) })") + } + + /// Every claimed wall sample has a truly visible facade surface, wall face or pilaster face, + /// straight out from it (within a voxel). + @Test func everyClaimedSampleHasAVisibleFacadeFace() { + let coverage = Self.map.coverage(along: Self.wall) + let cameras = pilasterWalk() + for index in Self.map.cellIndices where Self.covers(Self.map, coverage.wall, index) { + let range = Self.map.cellRange(index) + for s in [range.lowerBound + 0.03, range.upperBound - 0.03] { + for height: Float in [0.3, 1.0, 1.9] { + let visible = [Float(0), 0.36].contains { out in + [Float(-0.1), 0, 0.1].contains { ds in + let p = SIMD3(s + ds, height, out + 1e-3) + return Self.scene.intersect(origin: p + SIMD3(0, 0, 0.01), direction: SIMD3(0, 0, -1)).map { $0.t < 0.02 } == true + && cameras.contains { Self.scene.isVisible(p, normal: SIMD3(0, 0, 1), from: $0) } + } + } + #expect(visible, "cell \(index) s \(s) h \(height) claimed without a visible face") + } + } + } + } + + /// The wall's line runs along the face, not between face and pilasters: within 0.5 degrees + /// and 3 cm at the meter, with an error bar that covers that and is not wildly larger. + @Test func wallLineFollowsTheFaceNotThePilasters() throws { + let chain = try #require(Self.map.measuredWalls()) + let piece = chain.walls[chain.meterIndex] + let angle = abs(atan2(piece.along.y, piece.along.x)) * 180 / .pi + #expect(angle < 0.5, "angle \(angle) degrees") + let offset = simd_dot(SIMD2.zero - piece.start, piece.outward) + #expect(abs(offset) < 0.03, "line \(offset) m off the face at the meter") + #expect(piece.plusMinus >= abs(offset) && piece.plusMinus <= 0.2, "plus or minus \(piece.plusMinus)") + #expect(abs(piece.length - 12) < 0.3, "length \(piece.length)") + } + + /// A hedge 2.5 m tall standing 0.25 to 0.45 m out, with the gap behind it seen from the side: + /// it is not part of the facade, so the wall behind it is not seen. + @Test func aFreestandingHedgeWithAGapIsNotRelief() { + let hedge = SyntheticScene( + walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(7, 0))], + boxes: [SyntheticScene.Box(min: SIMD3(1, 0, 0.25), max: SIMD3(2, 2.5, 0.45))]) + var map = Map3D(frame: sceneFrame()) + let side = [SIMD3(3.2, 1.2, 0.35), SIMD3(-0.2, 1.2, 0.35)].map { lidarCamera(at: $0, lookingAt: SIMD3(1.5, 1.0, 0.1)) } + for (index, camera) in (pilasterWalk() + side).enumerated() { map.integrate(hedge.depthFrame(from: camera, seed: UInt64(index))) } + let coverage = map.coverage(along: Self.wall) + for index in map.cellIndices where map.cellRange(index).lowerBound >= 1.2 && map.cellRange(index).upperBound <= 1.8 { + #expect(!Self.covers(map, coverage.wall, index), "cell \(index) behind the hedge claimed seen") + } + } + + /// Stone cladding 12 cm proud and 1.3 m tall along the right two thirds of a 9.4 m wall + /// (30.8 ft, electro's length): the line still runs on the wall's face. A least-squares + /// line through face and cladding tilts toward the cladding's end. + @Test func claddingAlongPartOfTheWallDoesNotTiltItsLine() throws { + let scene = SyntheticScene( + walls: [SyntheticScene.Wall(a: SIMD2(-3, 0), b: SIMD2(6.4, 0))], + boxes: [SyntheticScene.Box(min: SIMD3(0.4, 0, 0), max: SIMD3(6.4, 1.3, 0.12))]) + var map = Map3D(frame: sceneFrame()) + let cameras = Swift.stride(from: Float(-2.5), through: 6, by: 0.3).flatMap { x in + [Float(0), -1.4, 1.4].map { dx in lidarCamera(at: SIMD3(x, 1.4, 2.5), lookingAt: SIMD3(x + dx, 1.0, 0)) } + } + for (index, camera) in cameras.enumerated() { map.integrate(scene.depthFrame(from: camera, noise: 0.02, seed: UInt64(index))) } + let chain = try #require(map.measuredWalls()) + let piece = chain.walls[chain.meterIndex] + let angle = abs(atan2(piece.along.y, piece.along.x)) * 180 / .pi + let offset = simd_dot(SIMD2.zero - piece.start, piece.outward) + #expect(angle < 0.3, "angle \(angle) degrees") + #expect(abs(offset) < 0.03, "line \(offset) m off the face at the meter") + } + + /// A box 0.3 m proud mounted on the wall from 1.0 to 1.5 m up: the wall is seen up to just + /// below it, and the rest of the wall up to the top of the map. Pilasters' faces carry the + /// height on up. + @Test func wallHeightStopsBelowABoxOnTheWall() { + let scene = SyntheticScene( + walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(7, 0))], + boxes: [SyntheticScene.Box(min: SIMD3(1, 1.0, 0), max: SIMD3(2, 1.5, 0.3))]) + var map = Map3D(frame: sceneFrame()) + for (index, camera) in pilasterWalk().enumerated() { map.integrate(scene.depthFrame(from: camera, noise: 0.02, seed: UInt64(index))) } + for index in map.cellIndices { + let range = map.cellRange(index) + let height = map.wallHeight(cell: index, along: Self.wall) + if range.lowerBound >= 1.2, range.upperBound <= 1.8 { + #expect(height.map { $0 >= 0.8 && $0 < 1.0 } == true, "cell \(index) seen to \(height ?? -1)") + } + if range.lowerBound >= -2, range.upperBound <= 0.6 { + #expect((height ?? 0) >= 2.3, "cell \(index) seen to \(height ?? -1)") + } + } + let behind = Self.map.cellIndices.filter { Self.map.cellRange($0).lowerBound >= -4.2 && Self.map.cellRange($0).upperBound <= -3.8 } + #expect(behind.allSatisfy { (Self.map.wallHeight(cell: $0, along: Self.wall) ?? 0) >= 2.3 }) + } +} + diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DReplayComparisonTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DReplayComparisonTests.swift new file mode 100644 index 000000000..c159f9f6f --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DReplayComparisonTests.swift @@ -0,0 +1,283 @@ +import Foundation +import HouseScanKit +import simd +import Testing + +// The 2D coverage map and the 3D map fed the same whole replay walk, read along the declared +// wall, and printed side by side as REPLAY_COMPARE lines. On the synthetic fixtures each claim +// is judged against exact ray casting of the scene the fixture was rendered from +// (ios/Tools/make-synthetic-replay.swift, header), from the same camera poses. Only the 3D map +// is asserted on; the 2D map's numbers are printed for comparison. +@Suite struct Map3DReplayComparisonTests { + static let fixtures = URL(fileURLWithPath: #filePath) + .deletingLastPathComponent() // Map3D + .deletingLastPathComponent() // HouseScanKitTests + .deletingLastPathComponent() // Tests + .deletingLastPathComponent() // HouseScanKit + .deletingLastPathComponent() // ios + .appendingPathComponent("HouseScanUITests/Fixtures", isDirectory: true) + + static let advioFolder = URL( + fileURLWithPath: ProcessInfo.processInfo.environment["HOUSESCAN_ADVIO_REPLAY"] ?? "/Users/samgu/house-scanning-data/replays/advio-20-0040-0075", + isDirectory: true) + static var advioPresent: Bool { FileManager.default.fileExists(atPath: advioFolder.appendingPathComponent("session.json").path) } + + /// The generator's scene: the wall on z = 0 facing +z, x in [-6, 6], 3 m tall, the ground at + /// y = 0, and with --lidar the bin. The meters, door and window are painted flush on the wall + /// and hide nothing, so they are left out. + static func scene(bin: Bool) -> SyntheticScene { + SyntheticScene( + walls: [SyntheticScene.Wall(a: SIMD2(-6, 0), b: SIMD2(6, 0), height: 3)], + boxes: bin ? [SyntheticScene.Box(min: SIMD3(1.2, 0, 0.5), max: SIMD3(1.8, 1.1, 1.1))] : []) + } + + @Test func syntheticLidarWalk() throws { + let walk = try Walk(folder: Self.fixtures.appendingPathComponent("synthetic-wall-lidar")) + let truth = Truth(scene: Self.scene(bin: true), cameras: walk.frames.map(\.photo), wallExtent: -6...6, wall: walk.wall) + let models = walk.feed() + let rows = report("synthetic-wall-lidar", walk: walk, models: models, truth: truth) + + #expect(rows.map3DWall.falseLength == 0, "Map3D claims \(rows.map3DWall.falseLength) m of wall no camera saw") + #expect(rows.map3DGround.falseLength == 0, "Map3D claims \(rows.map3DGround.falseLength) m of ground past what any camera saw") + // Behind the bin means cells whose middle lies in s 1.3..1.7, as Map3DOcclusionTests.covers + // judges a cell. Any overlap would be too strict: the cell from 1.676 to 1.829 is truly + // seen at every height (1 to 7 cameras at s 1.65 and 1.75 by exact ray casting), and + // overlaps the stretch by 2.4 cm. + let width = Map3DConfig().cellWidth + let behindBin = (0..<20).map { (Float($0) + 0.5) * width }.filter { (1.3...1.7).contains($0) } + let claimedBehindBin = behindBin.filter { middle in models.map.wall.contains { $0.contains(middle) } } + let gridOverlap = Truth.grid.filter { s in (1.3...1.7).contains(s) && models.map.wall.contains { $0.contains(s) } } + print(String( + format: "REPLAY_COMPARE fixture=synthetic-wall-lidar model=Map3D note=behind the bin: %d of %d cells with middles in s 1.3..1.7 claimed; claimed 2 cm samples in 1.3..1.7: %@", + claimedBehindBin.count, behindBin.count, gridOverlap.map { String(format: "%.2f", $0) }.joined(separator: ","))) + #expect(!behindBin.isEmpty) + #expect(claimedBehindBin.isEmpty, "Map3D claims the wall behind the bin at cell middles \(claimedBehindBin)") + } + + @Test func syntheticPhotoWalk() throws { + let walk = try Walk(folder: Self.fixtures.appendingPathComponent("synthetic-wall")) + let truth = Truth(scene: Self.scene(bin: false), cameras: walk.frames.map(\.photo), wallExtent: -6...6, wall: walk.wall) + report("synthetic-wall", walk: walk, models: walk.feed(), truth: truth) + } + + @Test(.enabled(if: advioPresent, "no ADVIO replay at HOUSESCAN_ADVIO_REPLAY (default /Users/samgu/house-scanning-data/replays/advio-20-0040-0075); skipped")) + func advioWalk() throws { + let walk = try Walk(folder: Self.advioFolder) + report("advio-20-0040-0075", walk: walk, models: walk.feed(), truth: nil) + } + + // MARK: Feeding + + struct Walk { + struct Frame { + var photo: CameraFrame + var depth: DepthImage? + var time: Double + var trackingNormal: Bool + } + + var frames: [Frame] + var wall: WallFrame + var wallSource: String + + init(folder: URL) throws { + let session = try ReplaySession.load(folder: folder) + frames = try session.frames.map { frame in + Frame( + photo: CameraFrame(cameraToWorld: frame.cameraToWorld, intrinsics: frame.intrinsics, imageSize: SIMD2(Float(frame.width), Float(frame.height))), + depth: try ReplaySession.loadDepth(for: frame, folder: folder), time: frame.timestamp, trackingNormal: frame.trackingNormal) + } + if let declared = session.declaredWall { + wall = try #require(WallFrame(meter: declared.meter, outward: declared.outward, groundY: declared.groundY)) + wallSource = "declared" + } else { + // No wall was tapped (ADVIO): the wall the app's replay player assumes from the trajectory. + let planned = frames.map { PlannedFrame(camera: $0.photo, timestamp: $0.time, trackingNormal: $0.trackingNormal) } + let assumed = try #require(ReplayPlanning.assumedWall(frames: planned), "no wall can be assumed from \(folder.path)") + wall = assumed.wall + wallSource = String(format: "assumed by ReplayPlanning.assumedWall, %.2f m from the walk's centroid", assumed.offset) + } + } + + struct Models { + var flat: CoverageMap + var map: Map3DCoverage + var flatOverheadFrames: Int + var map3DDepthFrames: Int + } + + /// Every frame, as ScanEngine keeps them: the 2D map observes each with its tracking state + /// (limited tracking only breaks the walked path) and its depth; the 3D map integrates the + /// depth of each frame with normal tracking. The 2D map's overhead band holds only views + /// the homeowner confirmed clear; frames tilted more than 15 degrees up stand in for them. + func feed() -> Models { + var flat = CoverageMap(wall: wall) + var map = Map3D(frame: MapFrame(wall: wall)) + var overheadFrames = 0 + var depthFrames = 0 + for frame in frames { + flat.observe(frame.photo, trackingNormal: frame.trackingNormal, time: frame.time, depth: frame.depth) + if frame.trackingNormal, frame.photo.forward.y > sin(15 * Float.pi / 180), + !flat.recordOverhead(frame.photo, trackingNormal: true).isEmpty { + overheadFrames += 1 + } + if frame.trackingNormal, let depth = frame.depth { + map.integrate(Self.depthFrame(depth, pose: frame.photo)) + depthFrames += 1 + } + } + return Models(flat: flat, map: map.coverage(along: wall), flatOverheadFrames: overheadFrames, map3DDepthFrames: depthFrames) + } + + /// Mirrors Map3DFeed.depthFrame(_:pose:) in the app target (HouseScan/Runtime/Map3DFeed.swift). + static func depthFrame(_ image: DepthImage, pose: CameraFrame) -> DepthFrame { + let camera = CameraFrame( + cameraToWorld: pose.cameraToWorld, intrinsics: image.intrinsics, + imageSize: SIMD2(Float(image.width), Float(image.height))) + return DepthFrame( + camera: camera, width: image.width, height: image.height, + depth: image.millimeters.map { Float($0) / 1000 }, kind: .lidar(confidence: image.confidence)) + } + } + + // MARK: Truth + + /// What the cameras truly saw of a synthetic scene. A point counts as seen when, within a + /// voxel (0.1 m) along the surface, some camera has it in view within 5 m and 65 degrees of + /// its normal with nothing in between (SyntheticScene.isVisible), as + /// Map3DOcclusionTests.nearlyVisible judges it. Different heights may be seen by different + /// cameras, as the 3D map accumulates them. + struct Truth { + /// s samples every 2 cm, at their centers, over the 3D map's whole extent. + static let step: Float = 0.02 + static let grid: [Float] = (0..<800).map { -8 + step * (Float($0) + 0.5) } + static let wallHeights = Array(Swift.stride(from: Float(0.1), to: 1.9812, by: 0.2)) + [1.9812] + static let groundStep: Float = 0.1524 + static let groundOuts = Array(Swift.stride(from: Float(0.1), through: 5.2, by: groundStep)) + + var scene: SyntheticScene + var cameras: [CameraFrame] + var wallExtent: ClosedRange + var wall: WallFrame + + func nearlyVisible(_ point: SIMD3, normal: SIMD3, u: SIMD3, v: SIMD3) -> Bool { + for du: Float in [-0.1, 0, 0.1] { + for dv: Float in [-0.1, 0, 0.1] { + let p = point + u * du + v * dv + normal * 1e-3 + if cameras.contains(where: { scene.isVisible(p, normal: normal, from: $0) }) { return true } + } + } + return false + } + + /// Seen at every sampled height of the wall band. Past the wall's ends there is no wall. + func wallSeen(_ s: Float) -> Bool { + guard wallExtent.contains(s) else { return false } + return Self.wallHeights.allSatisfy { height in + nearlyVisible(wall.world(s: s, height: height), normal: wall.outward, u: wall.along, v: WallFrame.up) + } + } + + /// The farthest ground sample out from the wall with every sample up to it seen; nil when + /// the first is not. + func groundReach(_ s: Float) -> Float? { + var reach: Float? + for out in Self.groundOuts { + guard nearlyVisible(wall.world(s: s, height: 0, out: out), normal: WallFrame.up, u: wall.along, v: wall.outward) else { break } + reach = out + } + return reach + } + } + + // MARK: Report + + struct Row { + var claimedLength: Float + var falseLength: Float? + var missedLength: Float? + } + + struct Rows { + var map3DWall: Row + var map3DGround: Row + } + + @discardableResult + func report(_ fixture: String, walk: Walk, models: Walk.Models, truth: Truth?) -> Rows { + let grid = Truth.grid + let step = Truth.step + let trueWall = truth.map { t in grid.map(t.wallSeen) } + let trueGround = truth.map { t in grid.map(t.groundReach) } + + func length(_ count: Int) -> Float { Float(count) * step } + func meters(_ value: Float?) -> String { + guard let value else { return "n/a" } + return String(format: "%.2fm(%.1fft)", value, value / 0.3048) + } + func reach(_ outs: [Float]) -> String { + guard !outs.isEmpty else { return "reach=none" } + let sorted = outs.sorted() + return String(format: "reach(min/median/max)=%.2f/%.2f/%.2fm", sorted[0], sorted[sorted.count / 2], sorted[sorted.count - 1]) + } + func line(_ model: String, _ band: String, _ row: Row, _ extra: String = "") { + print("REPLAY_COMPARE fixture=\(fixture) model=\(model) band=\(band) claimed=\(meters(row.claimedLength)) false=\(meters(row.falseLength)) missed=\(meters(row.missedLength))\(extra.isEmpty ? "" : " " + extra)") + } + + func wallRow(_ intervals: [ClosedRange]) -> Row { + let claimed = grid.map { s in intervals.contains { $0.contains(s) } } + return Row( + claimedLength: length(claimed.filter { $0 }.count), + falseLength: trueWall.map { seen in length(grid.indices.filter { claimed[$0] && !seen[$0] }.count) }, + missedLength: trueWall.map { seen in length(grid.indices.filter { !claimed[$0] && seen[$0] }.count) }) + } + func outs(_ spans: [ObservedSpan]) -> [Float?] { + grid.map { s in spans.filter { $0.span.contains(s) }.map(\.out).max() } + } + /// Ground is claimed falsely where the claim reaches more than one sample step past the + /// last truly seen sample, or where nothing at the wall's foot was seen. + func groundRow(_ spans: [ObservedSpan]) -> (Row, [Float]) { + let claimed = outs(spans) + let row = Row( + claimedLength: length(claimed.compactMap { $0 }.count), + falseLength: trueGround.map { t in + length(grid.indices.filter { i in claimed[i].map { out in t[i].map { out > $0 + Truth.groundStep } ?? true } ?? false }.count) + }, + missedLength: trueGround.map { t in length(grid.indices.filter { claimed[$0] == nil && t[$0] != nil }.count) }) + return (row, claimed.compactMap { $0 }) + } + func untruthedRow(_ spans: [ObservedSpan]) -> (Row, [Float]) { + let claimed = outs(spans).compactMap { $0 } + return (Row(claimedLength: length(claimed.count)), claimed) + } + + print("REPLAY_COMPARE fixture=\(fixture) note=\(walk.frames.count) frames (\(walk.frames.filter(\.trackingNormal).count) normal), \(walk.frames.filter { $0.depth != nil }.count) with depth; wall \(walk.wallSource); lengths on a \(Int(step * 100)) cm s grid over [-8, 8] m; no ends marked") + + if let trueWall, let trueGround { + line("truth", "wall", Row(claimedLength: length(trueWall.filter { $0 }.count))) + line("truth", "ground", Row(claimedLength: length(trueGround.compactMap { $0 }.count)), reach(trueGround.compactMap { $0 })) + } + + let flatWall = wallRow(models.flat.coveredIntervals(.wall)) + let (flatGround, flatGroundOuts) = groundRow(models.flat.groundDepthSpans()) + let (flatFacing, flatFacingOuts) = untruthedRow(models.flat.facingSpans()) + let (flatOverhead, flatOverheadOuts) = untruthedRow(models.flat.overheadSpans()) + line("CoverageMap", "wall", flatWall) + line("CoverageMap", "ground", flatGround, reach(flatGroundOuts)) + line("CoverageMap", "facing", flatFacing, reach(flatFacingOuts)) + line("CoverageMap", "overhead", flatOverhead, reach(flatOverheadOuts) + " (\(models.flatOverheadFrames) tilted-up frames recorded as confirmed clear)") + + let mapWall = wallRow(models.map.wall) + let (mapGround, mapGroundOuts) = groundRow(models.map.ground) + let (mapFacing, mapFacingOuts) = untruthedRow(models.map.facing) + let (mapOverhead, mapOverheadOuts) = untruthedRow(models.map.overhead) + line("Map3D", "wall", mapWall) + line("Map3D", "ground", mapGround, reach(mapGroundOuts)) + line("Map3D", "facing", mapFacing, reach(mapFacingOuts)) + line("Map3D", "overhead", mapOverhead, reach(mapOverheadOuts)) + if models.map3DDepthFrames == 0 { + print("REPLAY_COMPARE fixture=\(fixture) model=Map3D note=no depth and no feature points in this replay, so Map3D integrated nothing and is expected to claim nothing") + } + return Rows(map3DWall: mapWall, map3DGround: mapGround) + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DReviewCaseTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DReviewCaseTests.swift new file mode 100644 index 000000000..f897ff0ba --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DReviewCaseTests.swift @@ -0,0 +1,104 @@ +import Foundation +import HouseScanKit +import simd +import Testing + +// Cases where coverage could claim more than a ray reached: something standing just in front of +// the wall, a strip of ground with no usable depth, an obstacle a phone without LiDAR tracked no +// points on. And a chain whose meter sits at the very end of its piece. +@Suite struct Map3DReviewCaseTests { + static let wall = standardWall() + + static func covers(_ map: Map3D, _ spans: [ClosedRange], _ index: Int) -> Bool { + let middle = (map.cellRange(index).lowerBound + map.cellRange(index).upperBound) / 2 + return spans.contains { $0.contains(middle) } + } + + static func cells(_ map: Map3D, from low: Float, to high: Float) -> [Int] { + map.cellIndices.filter { map.cellRange($0).lowerBound >= low && map.cellRange($0).upperBound <= high } + } + + /// A box 1.2 m tall standing 5 to 30 cm out from the wall hides the wall behind it; its + /// front is not the wall face. + @Test func aBoxAgainstTheWallHidesTheWall() { + let scene = SyntheticScene( + walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(6, 0))], + boxes: [SyntheticScene.Box(min: SIMD3(1, 0, 0.05), max: SIMD3(2, 1.2, 0.3))]) + var map = Map3D(frame: sceneFrame()) + for (index, camera) in bushWalk().enumerated() { map.integrate(scene.depthFrame(from: camera, noise: 0.02, seed: UInt64(index))) } + let coverage = map.coverage(along: Self.wall) + for index in Self.cells(map, from: 1.2, to: 1.8) { + #expect(!Self.covers(map, coverage.wall, index), "cell \(index) behind the box claimed seen") + #expect(map.facingReach(cell: index, along: Self.wall) == nil) + #expect(map.overheadReach(cell: index, along: Self.wall) == nil) + } + #expect(Self.cells(map, from: -2, to: 0.6).allSatisfy { Self.covers(map, coverage.wall, $0) }) + } + + /// Ground 0.35 to 0.45 m out comes back low confidence (a dark hose, a drain): ground is + /// never claimed across it. + @Test func groundReachStopsAtGroundWithNoDepth() { + let scene = SyntheticScene(walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(6, 0))]) + var map = Map3D(frame: sceneFrame()) + for camera in bushWalk() { + let frame = scene.depthFrame(from: camera) + var confidence = [UInt8](repeating: 2, count: frame.depth.count) + for v in 0..= 0.35, ground.z <= 0.45, abs(t - simd_distance(ground, ray.origin)) < 0.01 { confidence[v * frame.width + u] = 0 } + } + } + map.integrate(DepthFrame(camera: camera, width: frame.width, height: frame.height, depth: frame.depth, kind: .lidar(confidence: confidence))) + } + for index in Self.cells(map, from: -2, to: 4) { + let reach = map.groundReach(cell: index, along: Self.wall) ?? 0 + #expect(reach < 0.35, "cell \(index) reaches \(reach) across unmeasured ground") + } + } + + /// Without LiDAR, a box with no feature points on it: the planes behind it are not taken as + /// seen, and its volume is not taken as free. + @Test func withoutLidarAnUntrackedBoxIsNotSeenThrough() { + let scene = bushScene() + var map = Map3D(frame: sceneFrame()) + map.update(PlaneObservation( + id: UUID(), worldFromPlane: simd_float4x4(SIMD4(1, 0, 0, 0), SIMD4(0, 0, 1, 0), SIMD4(0, -1, 0, 0), SIMD4(0, 0, 0, 1)), + alignment: .vertical, center: SIMD2(0.5, -1.5), width: 11, length: 3)) + map.update(PlaneObservation( + id: UUID(), worldFromPlane: matrix_identity_float4x4, alignment: .horizontal, center: SIMD2(0.5, 3), width: 13, length: 6)) + let bush = scene.boxes[0] + for camera in bushWalk() { + let color = CameraFrame(cameraToWorld: camera.cameraToWorld, intrinsics: SIMD4(1450, 1450, 960, 720), imageSize: SIMD2(1920, 1440)) + let points = scene.featurePoints(from: color, columns: 32, rows: 24).filter { p in + !(p.x >= bush.min.x - 1e-3 && p.x <= bush.max.x + 1e-3 && p.z >= bush.min.z - 1e-3 && p.z <= bush.max.z + 1e-3 && p.y <= bush.max.y + 1e-3) + } + map.integrate(FeatureFrame(camera: color, points: points)) + } + let coverage = map.coverage(along: Self.wall) + for index in Self.cells(map, from: 1.1, to: 1.9) { + #expect(!Self.covers(map, coverage.wall, index)) + #expect(map.groundReach(cell: index, along: Self.wall) == nil) + #expect((map.facingReach(cell: index, along: Self.wall) ?? 0) < bush.min.z) + } + #expect(map.state(at: SIMD3(1.5, 0.5, 0.7)) != .free) + } + + /// The meter projects past the end of its piece, where the next piece starts round a + /// corner: the wall frame still builds, with the corner just past the meter. + @Test func meterAtThePieceEndStillMakesAWallFrame() throws { + let truth: [SIMD2] = [SIMD2(-3, 0), SIMD2(0, 0), SIMD2(0, -3)] + let scene = SyntheticScene(walls: SyntheticScene.chain(truth)) + var map = Map3D(frame: sceneFrame()) + for camera in Map3DWallTests.walk(truth) { map.integrate(scene.depthFrame(from: camera)) } + let chain = try #require(map.measuredWalls()) + #expect(chain.walls.count == 2) + for meter in [SIMD3(0, 1.5, 0), SIMD3(0.05, 1.5, 0.05), SIMD3(-0.05, 1.5, 0.05)] { + let frame = try #require(chain.wallFrame(meter: meter, groundY: 0, frame: sceneFrame())) + #expect(frame.segments.count == 2) + } + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DSpotTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DSpotTests.swift new file mode 100644 index 000000000..2ba9a5687 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DSpotTests.swift @@ -0,0 +1,86 @@ +import Foundation +import HouseScanKit +import simd +import Testing + +// The chosen-spot check: what the map says about the battery's volume at a stretch of wall, +// and how that combines with the homeowner's answer. +@Suite struct Map3DSpotTests { + static let wall = standardWall() + static let bush: Map3D = { + var map = Map3D(frame: sceneFrame()) + for (index, camera) in bushWalk().enumerated() { map.integrate(bushScene().depthFrame(from: camera, noise: 0.02, seed: UInt64(index))) } + return map + }() + + /// The bush stands 0.4 to 1.0 m out between s = 1 and 2, inside the battery's 0.56 m depth. + @Test func aMeasuredBushIsAnOccluder() { + #expect(Self.bush.spotView(span: 1.2...1.8, along: Self.wall) == .occluder) + } + + @Test func openWallIsClear() { + #expect(Self.bush.spotView(span: -1.5...(-0.7), along: Self.wall) == .clear) + } + + /// Beyond the walk, and in an empty map: nothing measured either way. + @Test func unmeasuredSpansAreUnknown() { + #expect(Self.bush.spotView(span: 6.5...7.3, along: Self.wall) == .unknown) + #expect(Map3D(frame: sceneFrame()).spotView(span: -0.4...0.4, along: Self.wall) == .unknown) + } + + /// Estimated depth alone never decides the spot, occluder or clear. + @Test func estimatedDepthAloneDecidesNothing() { + var map = Map3D(frame: sceneFrame()) + for camera in bushWalk() { + let lidar = bushScene().depthFrame(from: camera) + map.integrate(DepthFrame(camera: camera, width: lidar.width, height: lidar.height, depth: lidar.depth, kind: .estimated(sigma: lidar.depth.map { $0 * 0.01 }))) + } + #expect(map.spotView(span: 1.2...1.8, along: Self.wall) == .unknown) + #expect(map.spotView(span: -1.5...(-0.7), along: Self.wall) == .unknown) + } + + /// The bush seen in a single frame (from x = 1.5 m, straight on) is hit once: not enough to + /// be an occluder, and not clear either. + @Test func aSurfaceHitInOneFrameIsNotEnough() { + var map = Map3D(frame: sceneFrame()) + let camera = bushWalk()[30] + #expect(abs(camera.position.x - 1.5) < 1e-4) + map.integrate(bushScene().depthFrame(from: camera)) + // The part of the bush inside the battery's volume is its top, 0.9 m up, 0.4 to 0.56 m out. + #expect(map.evidence(at: SIMD3(1.5, 0.9, 0.5)).hits == 1) + #expect(map.spotView(span: 1.2...1.8, along: Self.wall) == .unknown) + map.integrate(bushScene().depthFrame(from: bushWalk()[28])) + #expect(map.spotView(span: 1.2...1.8, along: Self.wall) == .occluder) + } + + @Test func theHomeownerCannotOverrideAnOccluder() { + for answer in [true, false, nil] as [Bool?] { + #expect(SpotDecision.decide(.occluder, homeownerSaysClear: answer) == .needsAnotherView) + } + } + + @Test func unknownFallsBackToTheHomeowner() { + #expect(SpotDecision.decide(.unknown, homeownerSaysClear: true) == .clear) + #expect(SpotDecision.decide(.unknown, homeownerSaysClear: false) == .obstacle) + #expect(SpotDecision.decide(.unknown, homeownerSaysClear: nil) == .needsConfirmation) + #expect(SpotDecision.decide(.clear, homeownerSaysClear: nil) == .clear) + #expect(SpotDecision.decide(.clear, homeownerSaysClear: false) == .obstacle) + } + + /// ARKit refines the meter anchor 5 cm along the wall and 2 degrees about +y: the map moves + /// with it, the walk's wall is re-derived from the moved anchor, and the answers stay. + @Test func theAnswerFollowsTheMeterAnchor() throws { + var map = Self.bush + let old = map.frame.poseInWorld + let turn = simd_float4x4(simd_quatf(angle: 2 * .pi / 180, axis: SIMD3(0, 1, 0))) + var new = turn * old + new.columns.3 = old.columns.3 + SIMD4(0.05, 0, 0, 0) + let moved = map.frame.following(anchorMovedFrom: old, to: new) + map.reanchor(moved) + let meter = moved.world(.zero) + let outward = moved.worldDirection(SIMD3(0, 0, 1)) + let wall = try #require(WallFrame(meter: meter, outward: outward, groundY: meter.y + moved.groundY)) + #expect(map.spotView(span: 1.2...1.8, along: wall) == .occluder) + #expect(map.spotView(span: -1.5...(-0.7), along: wall) == .clear) + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DViewTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DViewTests.swift new file mode 100644 index 000000000..6524dc023 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DViewTests.swift @@ -0,0 +1,71 @@ +import Foundation +import HouseScanKit +import simd +import Testing + +// Fog of war and the next view, on the bush scene with a small region of interest (3 m either +// way along the wall, 2 m out) walked 3 m out so the whole region is in front of the cameras. +@Suite struct Map3DViewTests { + static let config: Map3DConfig = { + var config = Map3DConfig() + config.alongExtent = 3 + config.outDepth = 2 + return config + }() + static let scene = bushScene() + static let wall = standardWall() + + /// Every 0.3 m from x = -4 to 4 at 3 m out: at the wall, at the ground and up past the eaves. + static let walk: [CameraFrame] = Swift.stride(from: Float(-4), through: 4, by: 0.3).flatMap { x in + [SIMD3(x, 1.0, 0), SIMD3(x, 0, 1.0), SIMD3(x, 2.8, 0)].map { lidarCamera(at: SIMD3(x, 1.4, 3), lookingAt: $0) } + } + + static let walked: Map3D = { + var map = Map3D(frame: sceneFrame(), config: config) + for (index, camera) in walk.enumerated() { map.integrate(scene.depthFrame(from: camera, noise: 0.02, seed: UInt64(index))) } + return map + }() + + @Test func anEmptyMapIsAllFogWithNoViewToSuggest() { + let map = Map3D(frame: sceneFrame(), config: Self.config) + let fog = map.fogOfWar(along: Self.wall) + #expect(fog.seen == 0) + #expect(!fog.cells.isEmpty) + #expect(fog.cells.allSatisfy { $0.unknown == 1 }) + #expect(map.nextBestView(along: Self.wall) == nil) + } + + @Test func walkingClearsFogExceptBehindTheBush() { + let fog = Self.walked.fogOfWar(along: Self.wall) + #expect(fog.seen > 0.9, "seen \(fog.seen)") + // Fog cells are 0.3 m cubes; the unknown left is behind and inside the bush. + let heavy = fog.cells.filter { $0.unknown > 0.5 } + #expect(!heavy.isEmpty) + #expect(heavy.allSatisfy { $0.center.x > 0.5 && $0.center.x < 2.5 && $0.center.z < 1.3 && $0.center.y < 1.2 }, "\(heavy.map(\.center))") + } + + @Test func theNextViewLooksBehindTheBushAndRevealsIt() throws { + let map = Self.walked + let view = try #require(map.nextBestView(along: Self.wall)) + // The target is on the unseen region's boundary behind the bush. + #expect(view.target.x > 0.8 && view.target.x < 2.2, "target \(view.target)") + #expect(view.target.z > -0.1 && view.target.z < 1.1, "target \(view.target)") + #expect(view.inSight > 0) + // It stands in front of the wall, not in the bush, and at eye height. + #expect(view.stand.z > 0.25) + #expect(!(view.stand.x > 1 && view.stand.x < 2 && view.stand.z > 0.4 && view.stand.z < 1.0)) + #expect(view.eye.y == map.config.eyeHeight) + + // Taking the view shrinks the unseen region. + var after = map + after.integrate(Self.scene.depthFrame(from: lidarCamera(at: view.eye, lookingAt: view.target), noise: 0.02, seed: 99)) + let before = map.fogOfWar(along: Self.wall) + let later = after.fogOfWar(along: Self.wall) + #expect(later.seen > before.seen, "seen \(before.seen) -> \(later.seen)") + let region = { (fog: FogOfWar) in + fog.cells.filter { $0.center.x > view.regionMin.x && $0.center.x < view.regionMax.x && $0.center.z < view.regionMax.z } + .reduce(Float(0)) { $0 + $1.unknown } + } + #expect(region(later) < region(before), "unknown behind the bush \(region(before)) -> \(region(later))") + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DWallTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DWallTests.swift new file mode 100644 index 000000000..437fe95cc --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/Map3DWallTests.swift @@ -0,0 +1,108 @@ +import Foundation +import HouseScanKit +import simd +import Testing + +// Walls measured from depth frames of non-straight houses, with up to 2 cm of depth error on +// every pixel. Tolerances: every piece's direction within 3 degrees and its length within 0.2 m +// of the truth, every corner within 0.15 m. A voxel is 0.1 m, so a piece's end is placed to +// within half a voxel plus fitting error. +@Suite struct Map3DWallTests { + static let angleTolerance: Float = 3 * .pi / 180 + static let lengthTolerance: Float = 0.2 + static let cornerTolerance: Float = 0.15 + + /// Cameras 2.2 m in front of each piece every 0.4 m along it, aimed at the wall 1 m up, + /// plus one on the bisector of each corner looking into it. + static func walk(_ points: [SIMD2]) -> [CameraFrame] { + var cameras: [CameraFrame] = [] + for (a, b) in zip(points, points.dropFirst()) { + let along = simd_normalize(b - a) + let outward = SIMD2(-along.y, along.x) + let length = simd_distance(a, b) + for t in Swift.stride(from: Float(0.2), through: length - 0.2, by: 0.4) { + let foot = a + along * t + let eye = foot + outward * 2.2 + cameras.append(lidarCamera(at: SIMD3(eye.x, 1.4, eye.y), lookingAt: SIMD3(foot.x, 1, foot.y))) + } + } + for index in points.indices.dropFirst().dropLast() { + let before = simd_normalize(points[index] - points[index - 1]) + let after = simd_normalize(points[index + 1] - points[index]) + let outward = simd_normalize(SIMD2(-before.y, before.x) + SIMD2(-after.y, after.x)) + let eye = points[index] + outward * 2.5 + cameras.append(lidarCamera(at: SIMD3(eye.x, 1.4, eye.y), lookingAt: SIMD3(points[index].x, 1, points[index].y))) + } + return cameras + } + + static func measure(_ points: [SIMD2]) -> MeasuredWallChain? { + let scene = SyntheticScene(walls: SyntheticScene.chain(points)) + var map = Map3D(frame: sceneFrame()) + for (index, camera) in walk(points).enumerated() { + map.integrate(scene.depthFrame(from: camera, noise: 0.02, seed: UInt64(index))) + } + return map.measuredWalls() + } + + static func check(_ chain: MeasuredWallChain?, against truth: [SIMD2], meterPiece: Int) { + guard let chain else { + Issue.record("no chain measured") + return + } + #expect(chain.walls.count == truth.count - 1, "measured \(chain.vertices) for \(truth)") + #expect(chain.meterIndex == meterPiece) + guard chain.walls.count == truth.count - 1 else { return } + for (index, wall) in chain.walls.enumerated() { + let a = truth[index] + let b = truth[index + 1] + let angle = acos(min(1, simd_dot(wall.along, simd_normalize(b - a)))) + #expect(angle <= angleTolerance, "piece \(index) is \(angle * 180 / .pi) degrees off") + #expect(abs(wall.length - simd_distance(a, b)) <= lengthTolerance, "piece \(index) is \(wall.length) m, truth \(simd_distance(a, b))") + #expect(wall.source == .mesh) + // Outward is the truth's direction turned 90 degrees, facing where the cameras were. + let outward = simd_normalize(SIMD2(-(b - a).y, (b - a).x)) + #expect(simd_dot(wall.outward, outward) > 0.99) + } + for (measured, point) in zip(chain.vertices.dropFirst().dropLast(), truth.dropFirst().dropLast()) { + #expect(simd_distance(measured, point) <= cornerTolerance, "corner \(measured), truth \(point)") + } + } + + /// An L: a wing juts out left of the meter's wall (an inside corner at (-3, 0), an outside + /// one at (-3, 2.5)) and the house turns away at an outside corner at (4, 0). + @Test func lShapedHouse() { + let truth: [SIMD2] = [SIMD2(-4.5, 2.5), SIMD2(-3, 2.5), SIMD2(-3, 0), SIMD2(4, 0), SIMD2(4, -3)] + Self.check(Self.measure(truth), against: truth, meterPiece: 2) + } + + /// A bay window right of the meter: sides at 45 degrees, 0.99 m long. + @Test func angledBay() { + let truth: [SIMD2] = [SIMD2(-2, 0), SIMD2(1.5, 0), SIMD2(2.2, 0.7), SIMD2(4.2, 0.7), SIMD2(4.9, 0), SIMD2(7, 0)] + Self.check(Self.measure(truth), against: truth, meterPiece: 0) + } + + @Test func chainBecomesAWallFrameWithTheSameCorners() throws { + let truth: [SIMD2] = [SIMD2(-4.5, 2.5), SIMD2(-3, 2.5), SIMD2(-3, 0), SIMD2(4, 0), SIMD2(4, -3)] + let chain = try #require(Self.measure(truth)) + let frame = sceneFrame() + let wall = try #require(chain.wallFrame(meter: SIMD3(0, 1.5, 0), groundY: 0, frame: frame)) + #expect(wall.segments.count == chain.walls.count) + #expect(wall.meterSegmentIndex == chain.meterIndex) + // Each measured corner sits where the wall frame puts it. + let s: [Float] = [-5.5, -3, 4] + for (corner, point) in zip(s, [truth[1], truth[2], truth[3]]) { + let world = wall.world(s: corner, height: 0, out: 0) + #expect(simd_distance(SIMD2(world.x, world.z), point) <= 2 * Self.cornerTolerance, "s \(corner) at \(world), truth \(point)") + } + } + + @Test func aBushIsNotAWall() { + let scene = bushScene() + var map = Map3D(frame: sceneFrame()) + for camera in bushWalk() { map.integrate(scene.depthFrame(from: camera)) } + let chain = map.measuredWalls() + #expect(chain?.walls.count == 1, "\(chain?.vertices ?? [])") + #expect(map.wallPieces().allSatisfy { abs($0.start.y) < 0.15 && abs($0.end.y) < 0.15 }, "\(map.wallPieces())") + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/MeasuredChainAdmissionTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/MeasuredChainAdmissionTests.swift new file mode 100644 index 000000000..4b857c13e --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/MeasuredChainAdmissionTests.swift @@ -0,0 +1,72 @@ +import Foundation +@testable import HouseScanKit +import simd +import Testing + +/// When the export may describe the 3D map's measured wall chain instead of the walk's tapped +/// wall (`MeasuredChainAdmission`): each refusal on its own, and the tapped corner the chain +/// doesn't have. +@Suite struct MeasuredChainAdmissionTests { + /// The walk's wall: the meter at (0, 1.2, 0), facing +z, ground at 0. + static let tapped = WallFrame(meter: SIMD3(0, 1.2, 0), outward: SIMD3(0, 0, 1), groundY: 0)! + static let frame = MapFrame(wall: tapped) + + static func piece(_ a: SIMD2, _ b: SIMD2, outward: SIMD2 = SIMD2(0, 1)) -> MeasuredWall { + MeasuredWall(start: a, end: b, outward: outward, source: .mesh, support: 30, plusMinus: 0.03) + } + + static func admit(_ chain: MeasuredWallChain, tapped: WallFrame = tapped, baseline: ClosedRange = -2...2) -> Result, MeasuredChainAdmission.Refusal> { + let measured = chain.wallFrame(meter: tapped.meter, groundY: tapped.groundY, frame: frame)! + return MeasuredChainAdmission.admit(chain, as: measured, frame: frame, tapWall: tapped, baselineS: baseline) + } + + static func refused(_ result: Result, MeasuredChainAdmission.Refusal>, _ words: String) -> Bool { + if case .failure(let refusal) = result { return refusal.reason.contains(words) } + return false + } + + @Test func aChainOnTheTappedWallIsAdmitted() throws { + let result = Self.admit(MeasuredWallChain(walls: [Self.piece(SIMD2(-3, 0), SIMD2(3, 0))], meterIndex: 0)) + let baseline = try result.get() + #expect(abs(baseline.lowerBound + 2) < 1e-3 && abs(baseline.upperBound - 2) < 1e-3) + } + + /// The walk turned a right corner at s = 2 and went 1 m along the side wall; the map found + /// only the front wall, straight on past x = 2. The side wall's end projects back onto the + /// front wall at s = 2, but lies 1 m from it. + @Test func aTappedCornerTheChainLacksKeepsTheTappedWall() { + var cornered = Self.tapped + cornered.turn(.right, at: WallCorner(s: 2, outward: SIMD3(1, 0, 0))) + let chain = MeasuredWallChain(walls: [Self.piece(SIMD2(-3, 0), SIMD2(3.5, 0))], meterIndex: 0) + #expect(Self.refused(Self.admit(chain, tapped: cornered, baseline: -2...3), "tapped")) + } + + @Test func aMeterFarFromTheMeasuredLineIsRefused() { + let chain = MeasuredWallChain(walls: [Self.piece(SIMD2(-3, -0.5), SIMD2(3, -0.5))], meterIndex: 0) + #expect(Self.refused(Self.admit(chain), "meter is")) + } + + @Test func aMeterPieceTurnedFromTheTappedWallIsRefused() { + let turn: Float = 45 * .pi / 180 + let along = SIMD2(cos(turn), -sin(turn)) + let outward = SIMD2(sin(turn), cos(turn)) + let chain = MeasuredWallChain(walls: [Self.piece(-3 * along, 3 * along, outward: outward)], meterIndex: 0) + #expect(Self.refused(Self.admit(chain), "turns")) + } + + @Test func aChainShortOfTheBaselineIsRefused() { + let chain = MeasuredWallChain(walls: [Self.piece(SIMD2(-1, 0), SIMD2(3, 0))], meterIndex: 0) + #expect(Self.refused(Self.admit(chain), "short of the baseline")) + } + + @Test func aMeasuredCornerFarFromTheTappedWallIsRefused() { + // The chain steps 1.5 m out at x = 0.5 (a bump-out the walk never tapped) and runs on + // there; the tapped wall runs straight on. + let chain = MeasuredWallChain( + walls: [ + Self.piece(SIMD2(-3, 0), SIMD2(0.5, 0)), Self.piece(SIMD2(0.5, 0), SIMD2(0.5, 1.5), outward: SIMD2(-1, 0)), + Self.piece(SIMD2(0.5, 1.5), SIMD2(4, 1.5)), + ], meterIndex: 0) + #expect(Self.refused(Self.admit(chain, baseline: -2...3), "measured corner")) + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/MeasuredWallFrameTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/MeasuredWallFrameTests.swift new file mode 100644 index 000000000..c52b5ad34 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/MeasuredWallFrameTests.swift @@ -0,0 +1,24 @@ +@testable import HouseScanKit +import simd +import Testing + +/// `MeasuredWallChain.wallFrame` puts the wall on the fitted line, wherever the meter was tapped. +@Suite struct MeasuredWallFrameTests { + /// The map frame is the world here: a straight measured wall along x at z = 0, facing +z. + static let frame = MapFrame(poseInWorld: matrix_identity_float4x4, groundY: -1.2) + static let chain = MeasuredWallChain( + walls: [MeasuredWall(start: SIMD2(-3, 0), end: SIMD2(3, 0), outward: SIMD2(0, 1), source: .mesh, support: 60, plusMinus: 0.02)], + meterIndex: 0) + + @Test func aMeterTappedOnABoxProudOfTheWallLeavesTheWallOnItsFittedLine() throws { + // Tapped on the face of a meter box 0.25 m out from the wall, 0.5 m right of the origin. + let meter = SIMD3(0.5, 0, 0.25) + let wall = try #require(Self.chain.wallFrame(meter: meter, groundY: -1.2, frame: Self.frame)) + // The wall's line is the fit's, z = 0, so the box stands 0.25 m in front of it. + #expect(abs(wall.meter.z) < 1e-5) + #expect(abs(wall.wallPoint(meter).out - 0.25) < 1e-5) + // s = 0 is the meter's foot on the fitted line, at the meter's height. + #expect(simd_distance(wall.meter, SIMD3(0.5, 0, 0)) < 1e-5) + #expect(wall.source == .mesh) + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/MonocularDepthTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/MonocularDepthTests.swift new file mode 100644 index 000000000..a3479a866 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/MonocularDepthTests.swift @@ -0,0 +1,255 @@ +import Foundation +import HouseScanKit +import simd +import Testing + +// The scale fit and per-pixel sigma of MonocularDepth, on predictions made from known depth: +// prediction = (1 / depth - shift) / scale, the relation the fit inverts. + +/// A photo camera (1920 x 1440, ARKit's wide camera) at `position` looking at `target`, landscape. +func photoCamera(at position: SIMD3, lookingAt target: SIMD3) -> CameraFrame { + let f = simd_normalize(target - position) + let r = simd_normalize(simd_cross(f, SIMD3(0, 1, 0))) + let u = simd_cross(r, f) + let m = simd_float4x4(SIMD4(r, 0), SIMD4(u, 0), SIMD4(-f, 0), SIMD4(position, 1)) + return CameraFrame(cameraToWorld: m, intrinsics: SIMD4(1450, 1450, 960, 720), imageSize: SIMD2(1920, 1440)) +} + +@Suite struct MonocularDepthTests { + static let scale: Float = 0.5 + static let shift: Float = 0.1 + + /// A 64 x 48 prediction of a depth field given per image fraction. + static func prediction(width: Int = 64, height: Int = 48, _ depth: (SIMD2) -> Float) -> RelativeInverseDepth { + var values: [Float] = [] + for v in 0..) -> Float { 2 + 2 * f.x } + + /// Anchors on a `columns` x `rows` grid at pixel centers of the 64 x 48 prediction, so the + /// bilinear value there is exact. + static func anchors(columns: Int, rows: Int, _ depth: (SIMD2) -> Float) -> [DepthAnchor] { + (0..= z * fit.relativeSigma && sigma[middle] < z * fit.relativeSigma * 1.2, "sigma \(sigma[middle]) at \(z) m") + } + + /// Exact anchors: every pixel's sigma is the floor, 2% of depth and at least 2 cm. + @Test func exactAnchorsGiveTheFloor() throws { + let prediction = Self.prediction(Self.ramp) + let fit = try #require(MonocularDepth.fit(prediction, anchors: Self.anchors(columns: 10, rows: 6, Self.ramp))) + let frame = try #require(MonocularDepth.depthFrame(prediction, fit: fit, photo: photoCamera(at: .zero, lookingAt: SIMD3(0, 0, -1)))) + guard case .estimated(let sigma) = frame.kind else { Issue.record("not estimated"); return } + for index in Swift.stride(from: 0, to: frame.depth.count, by: 97) where frame.depth[index] > 0 { + #expect(abs(sigma[index] - max(0.02 * frame.depth[index], 0.02)) < 1e-3, "sigma \(sigma[index]) at \(frame.depth[index])") + } + } + + /// A box 1 m nearer than the wall behind it in the middle of the image: pixels within two + /// of its edge are uncertain by half the jump; pixels well away from it are not. + @Test func sigmaWidensAtADepthEdge() throws { + func scene(_ f: SIMD2) -> Float { f.x > 0.4 && f.x < 0.6 && f.y > 0.3 && f.y < 0.7 ? 2 : 3 } + let prediction = Self.prediction(width: 256, height: 192, scene) + // Anchors at pixel centers of the 256 x 192 prediction, 16 across and 12 down. + var anchors: [DepthAnchor] = [] + for row in 0..<12 { + for column in 0..<16 { + let pixel = SIMD2(Float(column * 16 + 8) + 0.5, Float(row * 16 + 8) + 0.5) + let fraction = pixel / SIMD2(256, 192) + anchors.append(DepthAnchor(fraction: fraction, depth: scene(fraction))) + } + } + let fit = try #require(MonocularDepth.fit(prediction, anchors: anchors)) + let frame = try #require(MonocularDepth.depthFrame(prediction, fit: fit, photo: photoCamera(at: .zero, lookingAt: SIMD3(0, 0, -1)))) + guard case .estimated(let sigma) = frame.kind else { Issue.record("not estimated"); return } + // The box starts at output column 102 (its center, 102.5 / 256, is past 0.4). + let row = 96 * 256 + let wallBeside: Float = sigma[row + 101] + let boxBeside: Float = sigma[row + 103] + let wallAway: Float = sigma[row + 60] + let boxAway: Float = sigma[row + 128] + #expect(wallBeside >= 0.499, "wall beside the edge: \(wallBeside)") + #expect(boxBeside >= 0.499, "box beside the edge: \(boxBeside)") + #expect(wallAway < 0.1, "wall away from the edge: \(wallAway)") + #expect(boxAway < 0.1, "box away from the edge: \(boxAway)") + } + + /// Anchors only on the left half: the right half gets no depth, since nothing there checked + /// the model. + @Test func noDepthWhereNoAnchorIsNear() throws { + let prediction = Self.prediction(Self.ramp) + let left = Self.anchors(columns: 10, rows: 6, Self.ramp).filter { $0.fraction.x < 0.5 } + let fit = try #require(MonocularDepth.fit(prediction, anchors: left + left.map { DepthAnchor(fraction: $0.fraction + SIMD2(0.02, 0.02), depth: Self.ramp($0.fraction + SIMD2(0.02, 0.02))) })) + let frame = try #require(MonocularDepth.depthFrame(prediction, fit: fit, photo: photoCamera(at: .zero, lookingAt: SIMD3(0, 0, -1)))) + #expect(frame.depth[96 * 256 + 40] > 0) + #expect(frame.depth[96 * 256 + 250] == 0) + } + + /// Anchors that agree with the model only up to 3.2 m (the right two fifths are outliers): + /// pixels the fit puts past 10% beyond that get no depth, and the last pixel before them is + /// as uncertain as can be. + @Test func noDepthBeyondTheAnchorsDepths() throws { + let prediction = Self.prediction(Self.ramp) + let near = Self.anchors(columns: 16, rows: 12, Self.ramp).map { anchor in + anchor.depth <= 3.2 ? anchor : DepthAnchor(fraction: anchor.fraction, depth: anchor.depth * 0.5) + } + let fit = try #require(MonocularDepth.fit(prediction, anchors: near)) + #expect(fit.anchorDepths.upperBound <= 3.2) + let frame = try #require(MonocularDepth.depthFrame(prediction, fit: fit, photo: photoCamera(at: .zero, lookingAt: SIMD3(0, 0, -1)))) + guard case .estimated(let sigma) = frame.kind else { Issue.record("not estimated"); return } + let farthest = frame.depth.max() ?? 0 + #expect(farthest <= fit.anchorDepths.upperBound * 1.1 + 1e-3, "\(farthest)") + let edge = (0..<256).last { frame.depth[96 * 256 + $0] > 0 }! + #expect(sigma[96 * 256 + edge] == MonocularDepth.farSigma) + } + + @Test func anchorsFromPointsKeepThoseInView() { + let camera = photoCamera(at: SIMD3(0, 1.4, 2), lookingAt: SIMD3(0, 1.4, 0)) + let anchors = DepthAnchor.anchors(points: [SIMD3(0, 1.4, 0), SIMD3(0.5, 1.0, 0.5), SIMD3(0, 1.4, 3), SIMD3(20, 1.4, 0)], camera: camera) + #expect(anchors.count == 2) + #expect(abs(anchors[0].depth - 2) < 1e-4 && simd_distance(anchors[0].fraction, SIMD2(0.5, 0.5)) < 1e-4) + #expect(abs(anchors[1].depth - 1.5) < 1e-4) + } + + @Test func anchorsFromAPlaneLieOnIt() { + let camera = photoCamera(at: SIMD3(0, 1.4, 2), lookingAt: SIMD3(0, 1.4, 0)) + // A wall plane at z = 0 facing +z: plane-local y along world +z. + let wall = PlaneObservation( + id: UUID(), worldFromPlane: simd_float4x4(SIMD4(1, 0, 0, 0), SIMD4(0, 0, 1, 0), SIMD4(0, -1, 0, 0), SIMD4(0, 1.4, 0, 1)), + alignment: .vertical, center: .zero, width: 2, length: 2) + // Feature points all over the plane's square confirm it is in view everywhere. + let points = Swift.stride(from: Float(-0.95), through: 0.95, by: 0.1).flatMap { x in + Swift.stride(from: Float(0.45), through: 2.35, by: 0.1).map { y in SIMD3(x, y, 0) } + } + let anchors = DepthAnchor.anchors(planes: [wall], confirmedBy: points, camera: camera) + #expect(!anchors.isEmpty) + for anchor in anchors { #expect(abs(anchor.depth - 2) < 1e-3) } + // A 2 m square seen from 2 m covers the middle of a 67 x 53 degree view, not its corners. + #expect(!anchors.contains { $0.fraction.x < 0.1 || $0.fraction.x > 0.9 }) + } + + /// A plane's extent can run behind something standing in front of it (the review's probe: a + /// box before the wall, the wall's plane scaling the model into the wall behind the box). + /// A plane point anchors the fit only where a feature point on the plane shows it is in view + /// there; points in front of the plane confirm nothing. + @Test func planeAnchorsNeedAFeaturePointOnThePlaneNearby() { + let camera = photoCamera(at: SIMD3(0, 1.4, 2), lookingAt: SIMD3(0, 1.4, 0)) + let wall = PlaneObservation( + id: UUID(), worldFromPlane: simd_float4x4(SIMD4(1, 0, 0, 0), SIMD4(0, 0, 1, 0), SIMD4(0, -1, 0, 0), SIMD4(0, 1.4, 0, 1)), + alignment: .vertical, center: .zero, width: 2, length: 2) + // One point on the wall left of center, one on a box 1 m in front of the wall's right half. + let onWall = SIMD3(-0.5, 1.4, 0) + let onBox = SIMD3(0.5, 1.4, 1) + let anchors = DepthAnchor.anchors(planes: [wall], confirmedBy: [onWall, onBox], camera: camera) + #expect(!anchors.isEmpty) + let seen = camera.pixel(of: onWall)! / camera.imageSize + for anchor in anchors { + #expect(abs(anchor.fraction.x - seen.x) <= 1.0 / 16 + 1e-4 && abs(anchor.fraction.y - seen.y) <= 1.0 / 12 + 1e-4, "unconfirmed anchor at \(anchor.fraction)") + } + #expect(DepthAnchor.anchors(planes: [wall], confirmedBy: [onBox], camera: camera).isEmpty) + } + + /// End to end on a wall 1.5 m away: a prediction rendered at the model's 518 x 392 from the + /// photo's view, anchors from the scene's surface points, and the depth frame it builds agree + /// with the scene pixel for pixel, so the image fractions and intrinsics line up. + @Test func depthFrameMatchesTheSceneItWasFittedTo() throws { + let scene = SyntheticScene(walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(5, 0))]) + let photo = photoCamera(at: SIMD3(0.3, 1.4, 1.5), lookingAt: SIMD3(-0.4, 0.8, 0)) + func depth(_ f: SIMD2) -> Float { + let ray = photo.ray(throughPixel: f * photo.imageSize) + guard let hit = scene.intersect(origin: ray.origin, direction: ray.direction) else { return 50 } + return hit.t * simd_dot(ray.direction, photo.forward) + } + let prediction = Self.prediction(width: 518, height: 392, depth) + var points: [SIMD3] = [] + for row in 0..<12 { + for column in 0..<16 { + let fraction = SIMD2((Float(column) + 0.5) / 16, (Float(row) + 0.5) / 12) + let ray = photo.ray(throughPixel: fraction * photo.imageSize) + if let hit = scene.intersect(origin: ray.origin, direction: ray.direction) { points.append(ray.at(hit.t)) } + } + } + let estimate = try #require(MonocularDepth.estimate(prediction, anchors: DepthAnchor.anchors(points: points, camera: photo), photo: photo)) + let frame = estimate.frame + #expect(frame.width == 256 && frame.height == 192) + var checked = 0 + for v in Swift.stride(from: 10, to: 182, by: 17) { + for u in Swift.stride(from: 10, to: 246, by: 19) where frame.depth[v * 256 + u] > 0 { + let ray = frame.camera.ray(throughPixel: SIMD2(Float(u) + 0.5, Float(v) + 0.5)) + let hit = try #require(scene.intersect(origin: ray.origin, direction: ray.direction)) + let truth: Float = hit.t * simd_dot(ray.direction, frame.camera.forward) + let estimated: Float = frame.depth[v * 256 + u] + #expect(abs(estimated - truth) < 0.005 * truth, "pixel \(u), \(v): \(estimated) vs \(truth)") + checked += 1 + } + } + #expect(checked > 100, "\(checked) pixels with depth") + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/SyntheticScene.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/SyntheticScene.swift new file mode 100644 index 000000000..28e183ec8 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3D/SyntheticScene.swift @@ -0,0 +1,207 @@ +import Foundation +import HouseScanKit +import simd + +// Synthetic scenes for the 3D map tests: walls, boxes and the ground, rendered into depth frames +// by exact ray casting, with the same ray casting as a ground-truth visibility oracle. World and +// map frame coincide: the meter's foot is the origin, its wall runs along +x and faces +z. + +/// fx = fy = 193, cx = 128, cy = 96 on 256 x 192: ARKit's wide camera (about 1450 px focal +/// length on 1920 x 1440) scaled to the LiDAR depth size, 67 by 53 degrees. +let lidarIntrinsics = SIMD4(193, 193, 128, 96) +let lidarSize = SIMD2(256, 192) + +/// A portrait phone camera at `position` looking at `target`, level side to side, with the +/// LiDAR depth intrinsics. Camera +y is the world's right and camera +x points down the world, +/// as in `makeCamera`. +func lidarCamera(at position: SIMD3, lookingAt target: SIMD3) -> CameraFrame { + let f = simd_normalize(target - position) + let r = simd_normalize(simd_cross(f, SIMD3(0, 1, 0))) + let u = simd_cross(r, f) + let m = simd_float4x4(SIMD4(-u, 0), SIMD4(r, 0), SIMD4(-f, 0), SIMD4(position, 1)) + return CameraFrame(cameraToWorld: m, intrinsics: lidarIntrinsics, imageSize: lidarSize) +} + +struct SyntheticScene { + /// A vertical wall from plan point `a` to `b` ((x, z)), from the ground to `height`. + struct Wall { + var a: SIMD2 + var b: SIMD2 + var height: Float = 3 + } + + struct Box { + var min: SIMD3 + var max: SIMD3 + } + + var walls: [Wall] + var boxes: [Box] = [] + + /// Walls joined end to end through `points`, left to right as seen from outside. + static func chain(_ points: [SIMD2]) -> [Wall] { + zip(points, points.dropFirst()).map { Wall(a: $0, b: $1) } + } + + /// The nearest surface along a ray and its normal facing the ray's origin. + func intersect(origin: SIMD3, direction: SIMD3, maxT: Float = 50) -> (t: Float, normal: SIMD3)? { + var best: (t: Float, normal: SIMD3)? + func consider(_ t: Float, _ normal: SIMD3) { + guard t > 1e-4, t < maxT, t < best?.t ?? .infinity else { return } + best = (t, simd_dot(normal, direction) > 0 ? -normal : normal) + } + // The ground, y = 0. + if direction.y < 0 { consider(-origin.y / direction.y, SIMD3(0, 1, 0)) } + for wall in walls { + let along = wall.b - wall.a + let length = simd_length(along) + let normal = SIMD3(-along.y, 0, along.x) / length + let denominator = simd_dot(normal, direction) + guard abs(denominator) > 1e-6 else { continue } + let t = simd_dot(normal, SIMD3(wall.a.x, 0, wall.a.y) - origin) / denominator + let p = origin + direction * t + let u = simd_dot(SIMD2(p.x, p.z) - wall.a, along / length) + guard u >= 0, u <= length, p.y >= 0, p.y <= wall.height else { continue } + consider(t, normal) + } + for box in boxes { + var near: Float = -.infinity + var far: Float = .infinity + var axisHit = 0 + var missed = false + for axis in 0..<3 { + if direction[axis] == 0 { + if origin[axis] < box.min[axis] || origin[axis] > box.max[axis] { missed = true } + continue + } + let a = (box.min[axis] - origin[axis]) / direction[axis] + let b = (box.max[axis] - origin[axis]) / direction[axis] + if min(a, b) > near { + near = min(a, b) + axisHit = axis + } + far = min(far, max(a, b)) + } + guard !missed, near <= far, near > 0 else { continue } + var normal = SIMD3.zero + normal[axisHit] = 1 + consider(near, normal) + } + return best + } + + /// Whether `point`, on a surface, is in view of `camera` with nothing in between: in front + /// of it, inside its image, within `maxDistance` and within `maxAngle` of `normal`. + func isVisible(_ point: SIMD3, normal: SIMD3, from camera: CameraFrame, maxDistance: Float = 5, maxAngle: Float = 65 * .pi / 180) -> Bool { + guard let pixel = camera.pixel(of: point), camera.contains(pixel: pixel) else { return false } + let offset = point - camera.position + let distance = simd_length(offset) + guard distance <= maxDistance, simd_dot(-offset / distance, normal) >= cos(maxAngle) else { return false } + guard let hit = intersect(origin: camera.position, direction: offset / distance) else { return true } + return hit.t >= distance - 2e-3 + } + + /// A LiDAR depth frame: every pixel's z-depth through its center, full confidence; 0 where + /// the ray meets nothing. `noise` adds a uniform error of up to that many + /// meters, the same for a given seed (ARKit's LiDAR is within about 1 cm at 1 m and a few + /// cm at 5 m; 2 cm everywhere is on the rough side). + func depthFrame(from camera: CameraFrame, noise: Float = 0, seed: UInt64 = 1) -> DepthFrame { + var state = seed &* 6364136223846793005 &+ 1442695040888963407 + func uniform() -> Float { + state = state &* 6364136223846793005 &+ 1442695040888963407 + return Float(state >> 40) / Float(1 << 24) * 2 - 1 + } + let width = Int(camera.imageSize.x) + let height = Int(camera.imageSize.y) + var depth = [Float](repeating: 0, count: width * height) + let forward = camera.forward + for v in 0.. DepthFrame { + var state = seed &* 6364136223846793005 &+ 1442695040888963407 + func uniform() -> Float { + state = state &* 6364136223846793005 &+ 1442695040888963407 + return (Float(state >> 40) + 0.5) / Float(1 << 24) + } + let exact = depthFrame(from: camera) + let depth = exact.depth.map { d -> Float in + guard d > 0 else { return 0 } + // Box-Muller. + let gaussian = (-2 * log(uniform())).squareRoot() * cos(2 * .pi * uniform()) + return max(0, d + sigma * gaussian) + } + return DepthFrame(camera: camera, width: exact.width, height: exact.height, depth: depth, kind: .estimated(sigma: [Float](repeating: sigma, count: depth.count))) + } + + /// Surface points under a `columns` x `rows` grid of pixels, standing in for the feature + /// points ARKit would track in the frame. + func featurePoints(from camera: CameraFrame, columns: Int, rows: Int) -> [SIMD3] { + var points: [SIMD3] = [] + for row in 0.. MapFrame { + MapFrame(meter: .zero, outward: SIMD3(0, 0, 1), worldGroundY: 0)! +} + +/// A straight wall along x from -5 to 6 with a bush in front of it: x 1 to 2, out 0.4 to 1.0, +/// 0.9 m tall. +func bushScene() -> SyntheticScene { + SyntheticScene( + walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(6, 0))], + boxes: [SyntheticScene.Box(min: SIMD3(1, 0, 0.4), max: SIMD3(2, 0.9, 1.0))]) +} + +/// The bush scene's walk: every 0.3 m from x = -3 to 5, 2.5 m out, the phone at chest height, +/// once aimed at the wall and once down at the ground in front of it. +func bushWalk() -> [CameraFrame] { + Swift.stride(from: Float(-3), through: 5, by: 0.3).flatMap { x in + [ + lidarCamera(at: SIMD3(x, 1.4, 2.5), lookingAt: SIMD3(x, 1.0, 0)), + lidarCamera(at: SIMD3(x, 1.4, 2.5), lookingAt: SIMD3(x, 0, 0.9)), + ] + } +} + +/// A facade with pilasters: a wall along x from -5 to 7 with pilasters 0.5 m wide standing +/// 0.36 m proud, as ETH3D electro's do, full height, every 2.5 m, none at the meter (x = 0). +func pilasterScene() -> SyntheticScene { + SyntheticScene( + walls: [SyntheticScene.Wall(a: SIMD2(-5, 0), b: SIMD2(7, 0))], + boxes: [Float(-4), -1.5, 1, 3.5, 6].map { x in SyntheticScene.Box(min: SIMD3(x - 0.25, 0, 0), max: SIMD3(x + 0.25, 3, 0.36)) }) +} + +/// The pilaster facade's walk: every 0.3 m from x = -4 to 6, 2.5 m out, aimed at the wall +/// straight on, and 30 degrees to either side, so the faces between pilasters are seen. +func pilasterWalk() -> [CameraFrame] { + Swift.stride(from: Float(-4), through: 6, by: 0.3).flatMap { x in + [Float(0), -1.4, 1.4].map { dx in lidarCamera(at: SIMD3(x, 1.4, 2.5), lookingAt: SIMD3(x + dx, 1.0, 0)) } + } +} + diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/Map3DHooksTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3DHooksTests.swift new file mode 100644 index 000000000..b03fa3a61 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/Map3DHooksTests.swift @@ -0,0 +1,162 @@ +import Foundation +@testable import HouseScanKit +import Testing +import simd + +/// The two HouseScanKit changes the app's 3D map needs: CoverageMap's covered state taken from +/// the map, and scene.json's per-wall source and error. +@Suite struct MeasuredCoveredTests { + // `wallCamera(s:)` sees every wall row of the cells whose lower edge L has c in + // [L - 0.7881, L + 0.9405]; two positions 0.3 m apart cover cells -2 to 7. + static func coveredByCamera() -> CoverageMap { + var map = CoverageMap(wall: standardWall()) + map.observe(wallCamera(s: 0.3), trackingNormal: true) + map.observe(wallCamera(s: 0.6), trackingNormal: true) + return map + } + + @Test func cameraCoverageIsUntouchedWithoutMeasuredCells() { + let map = Self.coveredByCamera() + #expect(map.measuredCovered == nil) + #expect(map.level(.wall, 0) == .covered) + #expect(map.coveredIntervals(.wall).count == 1) + } + + @Test func measuredCellsAloneAreCovered() { + var map = Self.coveredByCamera() + let before = map.revision + map.setMeasuredCovered([.wall: [2, 3, 12], .ground: []]) + #expect(map.revision == before + 1) + // Covered by the cameras but not by the map: back to seen, never covered. + #expect(map.level(.wall, 0) == .seen) + #expect(map.level(.wall, 2) == .covered) + // Seen from one position only, so the cameras alone never covered it either. + #expect(map.level(.wall, 8) == .seen) + // Covered by the map where no camera looked (cell 12 starts 1.83 m along, out of both views). + #expect(map.level(.wall, 12) == .covered) + #expect(map.level(.wall, 13) == .unseen) + #expect(map.coveredIntervals(.wall) == [map.cellRange(2).lowerBound...map.cellRange(3).upperBound, map.cellRange(12)]) + #expect(map.coveredCount == 3) + #expect(map.coveredFraction(.wall, in: map.cellRange(2).lowerBound...map.cellRange(3).upperBound) == 1) + } + + @Test func settingTheSameCellsIsNoChange() { + var map = CoverageMap(wall: standardWall()) + map.setMeasuredCovered([.wall: [1]]) + let revision = map.revision + map.setMeasuredCovered([.wall: [1]]) + #expect(map.revision == revision) + } + + @Test func measuredCellsPastAMarkedEndAreNotCovered() { + var map = CoverageMap(wall: standardWall()) + map.setEnd(.right, at: map.cellRange(3).upperBound) + map.setMeasuredCovered([.wall: [3, 4]]) + #expect(map.level(.wall, 3) == .covered) + #expect(map.level(.wall, 4) == .unseen) + #expect(map.coveredIntervals(.wall) == [map.cellRange(3)]) + } + + @Test func skippingLeavesMeasuredCoveredCellsCovered() { + var map = CoverageMap(wall: standardWall()) + map.setMeasuredCovered([.ground: [1]]) + map.markSkipped(.ground, map.cellRange(0).lowerBound...map.cellRange(2).upperBound) + #expect(map.level(.ground, 0) == .skipped) + #expect(map.level(.ground, 1) == .covered) + #expect(map.level(.ground, 2) == .skipped) + } + + /// The 3D map's wall cell counts as covered once its face was seen to the height the walk + /// asks for (`wallWalkHeight`, 4.5 ft), as the camera coverage map's does, not only to + /// headroom; the export still reports each stretch's own height. + @Test func mapWallCellsAreCoveredAtTheWalksHeight() { + let map = CoverageMap(wall: standardWall()) + let walk = map.config.wallWalkHeight + let coverage = Map3DCoverage( + wall: [], wallHeight: [ObservedSpan(span: map.cellRange(2).lowerBound...map.cellRange(3).upperBound, out: walk + 0.05), + ObservedSpan(span: map.cellRange(4), out: walk - 0.1)], + ground: [], facing: [], overhead: []) + #expect(map.cells(seenIn: coverage)[.wall] == [2, 3]) + } + + @Test func measuredCellsCountAsSeenExtent() { + var map = CoverageMap(wall: standardWall()) + #expect(map.seenExtent == nil) + map.setMeasuredCovered([.wall: [5]]) + #expect(map.seenExtent == map.cellRange(5)) + } +} + +/// What a scene exported from the 3D map adds: walls found on the LiDAR mesh with their line's +/// error, and wall spans with the height the map saw each to. +@Suite struct Map3DSceneTests { + /// A measured chain with one right corner at s = 2: two mesh pieces. + static func meshWall() -> SceneWall { + SceneWall( + meter: SIMD3(0, 1.2, 0), outward: SIMD3(0, 0, 1), groundY: 0, + rightCorners: [WallCorner(s: 2, outward: SIMD3(1, 0, 0), source: .mesh)], source: .mesh) + } + + static func input(plusMinus: [Float?] = [0.03048, nil]) -> SceneInput { + SceneInput( + wall: meshWall(), baselineS: -1...3, + coverage: SceneCoverage( + leftEndMarked: false, rightEndMarked: true, + wall: [ObservedSpan(span: -1...1, out: 0.9), ObservedSpan(span: 1...3, out: 2.0)], + ground: [ObservedSpan(span: -1...3, out: 1.2)], facing: [ObservedSpan(span: -1...3, out: 0.6)], + overhead: [ObservedSpan(span: -1...1, out: 2.4)]), + wallPlusMinus: plusMinus) + } + + @Test func aMap3DSceneValidatesAgainstTheSchema() throws { + let data = try SceneExport.jsonData(Self.input()) + #expect(try SceneSchemas.scene().validate(data) == []) + let v = try JSONSchemaValidator.Value.parse(data) + let walls = try #require(v["walls"]?.array) + #expect(walls.map { $0["source"]?.string } == ["mesh", "mesh"]) + #expect(walls[0]["plus_minus_ft"] == .number(0.1)) + #expect(walls[1]["plus_minus_ft"] == nil) + let wallEntries = (v["coverage"]?["observed"]?.array ?? []).filter { $0["band"] == .string("wall") } + // 0.9 m is 2.95276 ft and 2.0 m is 6.56168 ft, each rounded down to four decimals. + #expect(wallEntries.map { $0["out_ft"]?.number } == [2.9527, 6.5616]) + } + + /// A meter tapped on a box 0.25 m proud of a measured wall: the wall stays on its line and + /// the meter's position is where it was tapped, projecting onto the wall at s = 0. + @Test func theMeterKeepsItsOwnPositionOffAMeasuredWall() throws { + var input = Self.input() + input.meterPosition = input.wall.world(s: 0, height: 1.2, out: 0.25) + let data = try SceneExport.jsonData(input) + #expect(try SceneSchemas.scene().validate(data) == []) + let v = try JSONSchemaValidator.Value.parse(data) + // (0, 1.2, 0.25) m in feet; the wall's baseline stays on z = 0. + #expect(v["meter"]?["pos"]?.numbers == [0, 3.937, 0.8202]) + let baseline = try #require(v["walls"]?.array?.first?["baseline"]?.array) + #expect(baseline.compactMap { $0.numbers?[1] } == [0, 0]) + } + + @Test func aMeterPositionAwayFromTheChainsOriginIsRefused() { + var input = Self.input() + input.meterPosition = input.wall.world(s: 0.5, height: 1.2, out: 0.25) + #expect(throws: SceneExportError.meterOffChainOrigin(s: 0.5)) { try SceneExport.jsonData(input) } + } + + @Test func noErrorsWriteNoPlusMinus() throws { + let data = try SceneExport.jsonData(Self.input(plusMinus: [])) + let walls = try #require(try JSONSchemaValidator.Value.parse(data)["walls"]?.array) + #expect(walls.count == 2) + #expect(walls.allSatisfy { $0["plus_minus_ft"] == nil }) + } + + @Test func errorsOfTheWrongCountAreRefused() { + #expect(throws: SceneExportError.countMismatch(field: "wallPlusMinus", expected: 2, actual: 1)) { + try SceneExport.jsonData(Self.input(plusMinus: [0.1])) + } + } + + @Test func aNegativeErrorIsRefused() { + #expect(throws: SceneExportError.negativeValue(field: "wallPlusMinus[1]", value: -0.1)) { + try SceneExport.jsonData(Self.input(plusMinus: [nil, -0.1])) + } + } +} diff --git a/ios/HouseScanKit/Tests/HouseScanKitTests/ReplayPlanningMap3DTests.swift b/ios/HouseScanKit/Tests/HouseScanKitTests/ReplayPlanningMap3DTests.swift new file mode 100644 index 000000000..cbfeb3947 --- /dev/null +++ b/ios/HouseScanKit/Tests/HouseScanKitTests/ReplayPlanningMap3DTests.swift @@ -0,0 +1,108 @@ +import Foundation +import HouseScanKit +import Testing +import simd + +/// The frames the autopilot holds back for the gap loop when the 3D map is the coverage model +/// (`ReplayPlanning.heldBackWindow(frames:depths:wall:)`), on the UI tests' two synthetic replays. +@Suite struct ReplayPlanningMap3DTests { + static let fixtures = URL(fileURLWithPath: #filePath).deletingLastPathComponent() + .appendingPathComponent("../../../HouseScanUITests/Fixtures").standardizedFileURL + + struct Replay { + var frames: [PlannedFrame] + var wall: WallFrame + } + + static func load(_ name: String) throws -> (Replay, ReplaySession, URL) { + let folder = fixtures.appendingPathComponent(name) + let session = try ReplaySession.load(folder: folder) + let frames = session.frames.map { + PlannedFrame( + camera: CameraFrame(cameraToWorld: $0.cameraToWorld, intrinsics: $0.intrinsics, imageSize: SIMD2(Float($0.width), Float($0.height))), + timestamp: $0.timestamp, trackingNormal: $0.trackingNormal) + } + let declared = try #require(session.declaredWall) + let wall = try #require(WallFrame(meter: declared.meter, outward: declared.outward, groundY: declared.groundY)) + return (Replay(frames: frames, wall: wall), session, folder) + } + + /// The gap planner's view of a map built from `indices`, with the window's ends marked. + static func coverage(_ replay: Replay, depths: [DepthImage?], indices: [Int], ends: ClosedRange) -> CoverageMap { + var map = Map3D(frame: MapFrame(wall: replay.wall)) + for index in indices where replay.frames[index].trackingNormal { + if let depth = depths[index] { map.integrate(DepthFrame(image: depth, pose: replay.frames[index].camera)) } + } + var coverage = CoverageMap(wall: replay.wall) + coverage.setEnd(.left, at: ends.lowerBound) + coverage.setEnd(.right, at: ends.upperBound) + coverage.setMeasuredCovered(coverage.cells(seenIn: map.coverage(along: replay.wall))) + return coverage + } + + /// Depth a LiDAR sensor would report for the synthetic wall's scene: the wall's plane above + /// the ground and the ground's plane in front of the wall, nothing else. 256 x 192 pixels, + /// ARKit's depth size: at 128 x 96 the facade's lowest row gets too few rays to count as seen + /// (`Map3D.groundReach` needs it), and at 64 x 48 nothing does. + static func renderedDepth(_ camera: CameraFrame, wall: WallFrame) -> DepthImage { + let width = 256 + let height = 192 + let intrinsics = DepthImage.intrinsics(scaling: camera.intrinsics, from: camera.imageSize, toWidth: width, height: height) + let m = camera.cameraToWorld + let rotation = simd_float3x3( + SIMD3(m.columns.0.x, m.columns.0.y, m.columns.0.z), SIMD3(m.columns.1.x, m.columns.1.y, m.columns.1.z), + SIMD3(m.columns.2.x, m.columns.2.y, m.columns.2.z)) + let origin = SIMD3(m.columns.3.x, m.columns.3.y, m.columns.3.z) + var meters = [Float](repeating: 0, count: width * height) + for v in 0.. 0, (origin + ray * t).y >= wall.groundY { nearest = min(nearest, t) } + } + if ray.y < 0 { + let t = (wall.groundY - origin.y) / ray.y + if t > 0, simd_dot(origin + ray * t - wall.meter, wall.outward) >= 0 { nearest = min(nearest, t) } + } + if nearest.isFinite { meters[v * width + u] = nearest } + } + } + return DepthImage(meters: meters, width: width, height: height, confidence: nil, intrinsics: intrinsics) + } + + @Test func aWindowOnADepthReplayLeavesAGapOnlyItsFramesClose() throws { + let (replay, _, _) = try Self.load("synthetic-wall") + let depths = replay.frames.map { Optional(Self.renderedDepth($0.camera, wall: replay.wall)) } + let held = try #require(ReplayPlanning.heldBackWindow(frames: replay.frames, depths: depths, wall: replay.wall)) + + // Checked again from scratch, as the app would see it: every other frame leaves the gap + // open, and adding the frames replayed for it closes it. + let rest = replay.frames.indices.filter { !held.frames.contains($0) } + let planner = GapPlanner() + let walked = Self.coverage(replay, depths: depths, indices: rest, ends: held.ends) + #expect(planner.plan(walked) == held.gap) + #expect(!planner.isSatisfied(held.gap, walked)) + let replayed = rest + Array(ReplayPlanning.gapReplayRange(held.frames)) + #expect(planner.isSatisfied(held.gap, Self.coverage(replay, depths: depths, indices: replayed, ends: held.ends))) + } + + /// The LiDAR fixture's bin hides ground no frame's depth gets past. That stretch is the gap + /// the planner asks for first whatever is held back, and no replayed frame closes it, so no + /// window is chosen and the autopilot's gap loop runs on the bin's gap instead. + @Test func theBinsShadowLeavesNoWindowOnTheLidarReplay() throws { + let (replay, session, folder) = try Self.load("synthetic-wall-lidar") + let depths = try session.frames.map { try ReplaySession.loadDepth(for: $0, folder: folder) } + #expect(depths.allSatisfy { $0 != nil }) + #expect(ReplayPlanning.heldBackWindow(frames: replay.frames, depths: depths, wall: replay.wall) == nil) + + let all = Self.coverage(replay, depths: depths, indices: Array(replay.frames.indices), ends: -5.9...5.9) + let standing = try #require(GapPlanner().plan(all)) + // The bin stands 4 to 6 ft (1.22 to 1.83 m) right of the meter. + #expect(standing.band == .ground) + #expect(standing.span.lowerBound <= 1.22 && standing.span.upperBound >= 1.83) + } +} diff --git a/ios/HouseScanUITests/FullFlowUITests.swift b/ios/HouseScanUITests/FullFlowUITests.swift index d75105122..6b7388c2f 100644 --- a/ios/HouseScanUITests/FullFlowUITests.swift +++ b/ios/HouseScanUITests/FullFlowUITests.swift @@ -33,10 +33,29 @@ final class FullFlowUITests: XCTestCase { static var environment: [String: String] { ProcessInfo.processInfo.environment } + /// On the synthetic replay, which has no depth, the 3D map (the default) sees nothing, so the + /// export takes the camera coverage map's path (`ScanEngine.exportGeometry`): the scene reports + /// the wall the walk saw rather than the map's empty coverage. @MainActor func testFullFlowFromReplay() throws { let replay = Self.environment["HOUSESCAN_REPLAY"].flatMap { $0.isEmpty ? nil : $0 } ?? Self.fixture - try runFlow(replay: replay) + var wallEntries = 0 + try runFlow(replay: replay, onScene: { data in + let scene = try XCTUnwrap(JSONSerialization.jsonObject(with: data) as? [String: Any]) + let coverage = try XCTUnwrap(scene["coverage"] as? [String: Any]) + let observed = try XCTUnwrap(coverage["observed"] as? [[String: Any]]) + wallEntries = observed.filter { $0["band"] as? String == "wall" }.count + }) + if replay == Self.fixture { + XCTAssertGreaterThan(wallEntries, 0, "a replay without depth exported the 3D map's empty coverage") + } + } + + /// The same flow with the camera coverage map alone (`-coverage legacy`), the model the 3D map + /// replaced as the default. + @MainActor + func testFullFlowFromReplayLegacyCoverage() throws { + try runFlow(replay: Self.fixture, extraArguments: ["-coverage", "legacy"]) } /// The flow from the LiDAR fixture. Depth must show the bin in front of the wall: the wall map @@ -44,8 +63,19 @@ final class FullFlowUITests: XCTestCase { /// gap loop, on the gap request. @MainActor func testFullFlowFromLidarReplay() throws { + try runLidarFlow(coverage: nil) + } + + @MainActor + func testFullFlowFromLidarReplayLegacyCoverage() throws { + try runLidarFlow(coverage: "legacy") + } + + /// `coverage` is the `-coverage` launch argument; nil leaves the app's default. + @MainActor + private func runLidarFlow(coverage: String?) throws { var showedHidden = false - try runFlow(replay: Self.lidarFixture) { app, phase in + try runFlow(replay: Self.lidarFixture, extraArguments: coverage.map { ["-coverage", $0] } ?? []) { app, phase in guard !showedHidden else { return } switch phase { // At the autopilot's 3x the walk plays in about 6 s. No other frame sees the wall diff --git a/ios/README.md b/ios/README.md index 444c80429..9ccb8d28e 100644 --- a/ios/README.md +++ b/ios/README.md @@ -8,12 +8,66 @@ Native iPhone app for the AR capture walk. The homeowner marks the electric mete - `HouseScan/Runtime/` is the engine: ARKit and replay frame sources, the state machine behind `ScanActions`, keyframe storage, upload and the autopilot. - `HouseScan/UI/` holds the screens. They read `ScanViewState` and call `ScanActions`; `HouseScan/Contract/ScanContract.swift` is the boundary between the two. +## 3D map + +`HouseScanKit/Sources/HouseScanKit/Map3D/` keeps a live occupancy map of the space around the meter. A ray from the camera marks the voxels it passes through as free and the voxel it stops in as surface. Voxels no ray reached stay unknown, so a wall behind a bush stays unseen until some view gets past the bush. The map uses 10 cm voxels in 8³ bricks, stored only once a ray reaches them. The default bounds hold at most 32 MB. `MapFrame` is the capture packet's meter frame (`packet/README.md`): origin at the meter anchor, +z the wall's outward normal, +y up, and the ground's height below the anchor as `groundY`. `DepthFrame` uses the packet's depth encoding, and a packet's mesh is one `MeshChunk` placed at `MapFrame.poseInWorld`. + +`Runtime/Map3DFeed.swift` converts ARKit data into the map's inputs on the AR delegate queue. `Runtime/Map3DSession.swift` owns one map for the app. The delegate queue only converts and hands inputs over, keeping the newest pending frame. The session integrates on its own queue and publishes a `Map3DSnapshot` (coverage, measured walls, fog, next view) at most twice a second. `Runtime/Map3DCoverageSource.swift` is what the export reads: the measured wall chain with each piece's `source` and `plus_minus_ft` when it matches the walk (its meter piece within 0.3 m of the meter, its corners within 0.5 m of the tapped chain, reaching both ends), otherwise the tapped wall. Each wall span goes out with the height its face was seen to. `UI/Camera/Map3DOverlay.swift` draws the fog and the next-view cue. Integrate only frames with normal tracking. + +The 3D map is the coverage model by default (`-coverage map3d`); `-coverage legacy` keeps the camera coverage map alone. Under map3d: + +- scene.json's coverage, walls and wall sources come from the map once measured depth went into it (LiDAR or a replay's). Estimated depth doesn't count, since it certifies nothing. Without measured depth the export and the walk use the camera coverage map, and the export logs which path it took. The walk's own facing (the walked path) and overhead (tilt-up views confirmed clear) are merged in, taking the larger reach. When the measured chain is written they are first restated along it, on the meter's piece only (`ObservedSpan.carried`). +- On a phone or replay with depth, the strip and the gap planner count a cell covered only where the map saw it (`CoverageMap.setMeasuredCovered`). Without depth the map is too sparse to walk by, and the camera sightings keep deciding. +- The result is placed along the wall the scene described, which can be the measured chain. +- A replay's frames go into the map only on the walk and in a gap request: for the close-up a replay plays its whole recording. + +| Phone | Session setting | Feed | +| --- | --- | --- | +| LiDAR | `frameSemantics.insert(.sceneDepth)`, `sceneReconstruction = .meshWithClassification` | `depthFrame` → `Map3D.integrate`; `meshChunk` of each added or updated `ARMeshAnchor` → `Map3D.update`, removed → `removeMeshChunk` | +| No LiDAR | plane detection (already on) | `featureFrame` → `Map3D.integrate`; `planes` → `Map3D.update` | + +Outputs, all against a `WallFrame` (the walk's, or one built from the map's own walls): + +- `coverage(along:)`: wall, ground, facing and overhead spans for `SceneCoverage(_:leftEndMarked:rightEndMarked:)`. + - A span is seen only where rays reached it. A voxel is free only where a ray crossed it completely and ended beyond it. + - The wall is judged against where the facade was measured around each cell, not the chain's line. Its face counts within 7.5 cm, and a recessed face up to 0.5 m behind also counts. + - Attached relief up to 0.5 m proud, such as a pilaster, counts as the facade, because nothing can be mounted behind it. It must reach headroom, no gap may have been seen behind it, and one of its side faces must show from its front back to the wall: an unseen gap is not evidence it is attached. + - Anything else standing in front hides the wall: a box, a shrub, or the meter itself, so the few cells behind the meter stay unseen. + - Facing reaches shorter than the battery's depth are not reported. +- `measuredWalls()`: the outline near the meter as a chain of straight pieces with real corners. + - Each piece is marked `mesh` or `plane` and carries `plusMinus`, its line's position error. The server takes both as `walls[].source` and `plus_minus_ft`. + - Lines are found by an angle sweep over the densest band of wall evidence, so relief along part of a wall doesn't tilt them. + - `wallFrame(meter:groundY:frame:)` turns the chain into a `WallFrame`. +- `fogOfWar(along:)`: 0.3 m cells of the region of interest that are still unknown, in the map frame, to draw from the meter anchor. +- `nextBestView(along:)`: the largest unseen region that borders seen space, plus where to stand and aim to see it. +- `spotView(span:along:)`: whether the battery's volume in front of a chosen span is blocked (`occluder`: a measured surface hit at least twice), seen `clear` (wall and ground seen, every voxel measured free), or `unknown`. `SpotDecision.decide` turns that and the homeowner's answer into the next step: an occluder always asks for another view and the homeowner can't override it, while unknown asks the homeowner. `Map3DCoverageSource.spotDecision` runs it on the live map; nothing in the app calls it yet, because the confirmation screen belongs to the engine. + +Without LiDAR, only feature-point rays count as seen. A point on a detected plane takes that plane's normal. Detected planes and mesh chunks mark voxels by reference count and add no occupancy: updating or removing one takes its marks with it, and planes alone clear no fog. Coverage without LiDAR is therefore sparse, and a replay without depth gives the map nothing. + +Estimated depth (`-estimatedDepth on`, off by default) runs Apple's Core ML Depth Anything V2 Small on each kept keyframe of a phone without LiDAR (`Runtime/DepthEstimator.swift`). The model gives inverse depth up to an unknown scale and shift, so each frame is scaled by its own ARKit geometry: `MonocularDepth` fits 1/depth = a · prediction + b to the frame's feature points and to points on detected planes, robustly. A plane point counts only where a feature point on that plane shows it in view nearby: a plane's extent can run behind something standing in front of it. A frame with too few of them gets no depth. Each pixel gets a standard deviation that grows where nothing checked the model: + +- the fit's robust residual scale; +- the local scale of anchors within 8% of the image width; +- the fit's own uncertainty where it extrapolates; +- half of any depth jump within two pixels. + +Pixels with no anchor nearby get no depth. `Map3D` then treats a pixel as a ray with that uncertainty: + +- It carves free space to two deviations short of the depth. +- It marks a surface only when two deviations are within 15 cm. + +That tells seen from unknown for the fog of war and the next view, and nothing more: estimated evidence never certifies coverage. A surface counts as seen only where a LiDAR or feature ray measured it, space counts as free for the facing and overhead bands only where one passed through it, and an estimated ray never lowers a voxel a measured ray hit (`VoxelGrid.integrate`). A review of the first version made estimated hits claim walls behind occluders several ways (`Map3DEstimatedEvidenceTests`). Estimated depth never makes walls. On ETH3D electro, photos only, it claimed nothing: at the photos' 3.8 m median distance two deviations are about 60 cm. The model is not in the repository. The locator looks for it in the app bundle, then in Application Support/Models. + +On an M4 Pro, one 256×192 LiDAR frame integrates in about 6 ms (release build, every second pixel), and reading coverage takes about 25 ms. `swift test -c release --filter Map3DPerformanceTests` prints the current numbers. The map assumes nothing moves: an object that appears in space seen empty earlier becomes surface where its face is measured, but its inside keeps the earlier free reading. + ## Launch arguments | Argument | Effect | | --- | --- | | `-replay ` | Plays a measure-lab-session v2 folder instead of the camera. A replay without wall taps gets a wall assumed from its trajectory, logged as an assumption. Keyframes with a `depth` entry (`{file, confidenceFile, w, h}`, Float32 meters) play their LiDAR depth into coverage as a LiDAR phone would. | -| `-autopilot` | Drives every step on a replay, answering the ground question with mulch. It holds some walk frames back to close one gap request before the upload. After each upload it also drives the requests the server's answer raises, closing each with the replay's frames, until the result shows. A request the frames don't close gets "I can't get there", as a homeowner would answer. | +| `-coverage map3d\|legacy` | Where coverage comes from (default `map3d`): see "3D map". Any other value stops the app. | +| `-estimatedDepth on\|off` | On a live phone without LiDAR under map3d, runs the depth model on kept keyframes into the 3D map, where it clears the fog of war and feeds the next view. It certifies no coverage, so the strip, the planners and the export keep using the camera coverage map on such a phone (default `off`, see "3D map"). Without the model it stays off. Any other value stops the app. | +| `-autopilot` | Drives every step on a replay, answering the ground question with mulch. It holds some walk frames back to close one gap request before the upload: under map3d on a replay with depth, frames whose depth alone shows a gap the map keeps without them (`ReplayPlanning.heldBackWindow(frames:depths:wall:)`), otherwise frames the camera coverage map needs. On the LiDAR fixture no window qualifies under map3d: the bin's shadow is always the nearest gap, and the autopilot answers it "I can't get there". Under map3d it waits for the map to catch up before each step that reads coverage. After each upload it also drives the requests the server's answer raises, closing each with the replay's frames, until the result shows. A request the frames don't close gets "I can't get there", as a homeowner would answer. | | `-serverURL ` | Uploads the scan to this server: `POST /v1/placements` with scene.json as `application/json`. Without it the app uses `HOUSESCAN_SERVER_URL` from `Config/Shared.xcconfig` (https://house-scanning-server.vercel.app), carried in Info.plist as `HouseScanServerURL`. Photos stay on the phone unless someone uses Share scan, which shares the scan as a capture packet (see "Scan bundle"). | | `-sampleResult` | Answers with the bundled sample result, which the result screen must label as a sample, even when a server is configured. The UI tests pass it so they run offline. It is also the fallback when `HOUSESCAN_SERVER_URL` is empty. | | `-autopilotHold ` | How long the autopilot leaves each screen up (default 1.2 s). |