diff --git a/CppDependencies/PoissonRecon/Sources/include/PoissonRecon/MeshingOperation.h b/CppDependencies/PoissonRecon/Sources/include/PoissonRecon/MeshingOperation.h index c9c024ff..ec39ce67 100644 --- a/CppDependencies/PoissonRecon/Sources/include/PoissonRecon/MeshingOperation.h +++ b/CppDependencies/PoissonRecon/Sources/include/PoissonRecon/MeshingOperation.h @@ -41,4 +41,7 @@ */ @property (nonatomic) BOOL closed; +/** Set after -main if a stage failed (vs. completed or was cancelled). nil on success. */ +@property (nonatomic, readonly, nullable) NSString *failureReason; + @end diff --git a/CppDependencies/PoissonRecon/Sources/src/MeshingOperation.mm b/CppDependencies/PoissonRecon/Sources/src/MeshingOperation.mm index d4275331..b2f36cd1 100644 --- a/CppDependencies/PoissonRecon/Sources/src/MeshingOperation.mm +++ b/CppDependencies/PoissonRecon/Sources/src/MeshingOperation.mm @@ -30,6 +30,11 @@ @implementation MeshingOperation { NSString *_outputFilePath; } +static unsigned long long FileSizeAtPath(NSString *path) { + NSDictionary *attrs = [[NSFileManager defaultManager] attributesOfItemAtPath:path error:NULL]; + return attrs ? [attrs[NSFileSize] unsignedLongLongValue] : 0; +} + - (instancetype)initWithInputFilePath:(NSString *)inputPath outputFilePath:(NSString *)outputPath { @@ -52,43 +57,61 @@ - (void)main NSString *tempPoissonOutputPathString = [NSTemporaryDirectory() stringByAppendingFormat:@"/poisson-%@.ply", [[NSUUID UUID] UUIDString]]; const char *poissonOutputPath = [tempPoissonOutputPathString UTF8String]; const char *surfaceTrimmerOutputPath = [_outputFilePath UTF8String]; - + + // Treat a non-existent or trivially small PLY as silent failure from the + // PoissonRecon/SurfaceTrimmer C++ layer. A well-formed PLY header alone is + // ~100 bytes; anything below this means the writer never produced geometry. + static const unsigned long long kMinValidPLYBytes = 256; + PoissonReconParameters poissonParams; poissonParams.Depth = (int)remapAndClamp(_resolution, 1, 10, 4, 14); poissonParams.SamplesPerNode = (int)remapAndClamp(_smoothness, 1, 10, 1, 15); - + SurfaceTrimmerParameters surfaceTrimmerParams; surfaceTrimmerParams.Trim = (int)remapAndClamp(_surfaceTrimmingAmount, 1, 10, 1, 10); - + __weak MeshingOperation *weakSelf = self; auto progressHandler = _progressHandler; - + if (![weakSelf isCancelled]) { PoissonReconExecute(inputPath, poissonOutputPath, _closed, poissonParams, [weakSelf, progressHandler](float progress) { float adjustedProgress = remapAndClamp(progress, 0, 1, 0, kPoissonProgressFraction); - progressHandler(adjustedProgress); - - BOOL shouldContinue = [weakSelf isCancelled]; - return !shouldContinue; + BOOL cancelled = [weakSelf isCancelled]; + return !cancelled; }); + + if (![weakSelf isCancelled] && FileSizeAtPath(tempPoissonOutputPathString) < kMinValidPLYBytes) { + _failureReason = @"PoissonRecon produced no output (NULL solver result). The input point cloud is likely too sparse, has invalid normals, or is degenerate (all points coplanar)."; + } } - - if (![weakSelf isCancelled]) { + + if (![weakSelf isCancelled] && _failureReason == nil) { if (_surfaceTrimmingAmount <= 0) { // Trimming disabled, just move the file to the destination - [[NSFileManager defaultManager] moveItemAtPath:tempPoissonOutputPathString toPath:_outputFilePath error:NULL]; + NSError *moveError = nil; + [[NSFileManager defaultManager] moveItemAtPath:tempPoissonOutputPathString toPath:_outputFilePath error:&moveError]; + if (moveError) { + _failureReason = [NSString stringWithFormat:@"Failed to move Poisson output to destination: %@", moveError.localizedDescription]; + } } else { - SurfaceTrimmerExecute(poissonOutputPath, surfaceTrimmerOutputPath, surfaceTrimmerParams, [weakSelf, progressHandler](float progress) { + int trimResult = SurfaceTrimmerExecute(poissonOutputPath, surfaceTrimmerOutputPath, surfaceTrimmerParams, [weakSelf, progressHandler](float progress) { float adjustedProgress = remapAndClamp(progress, 0, 1, kPoissonProgressFraction, 1); progressHandler(adjustedProgress); - - BOOL shouldContinue = [weakSelf isCancelled]; - return !shouldContinue; + BOOL cancelled = [weakSelf isCancelled]; + return !cancelled; }); + + if (![weakSelf isCancelled]) { + if (trimResult != 0) { + _failureReason = @"SurfaceTrimmer failed: PLY header unreadable, missing density values, or Poisson output corrupt."; + } else if (FileSizeAtPath(_outputFilePath) < kMinValidPLYBytes) { + _failureReason = @"SurfaceTrimmer produced no geometry (empty mesh after trimming)."; + } + } } } - + [[NSFileManager defaultManager] removeItemAtPath:tempPoissonOutputPathString error:NULL]; } diff --git a/StandardCyborgFusion/Sources/StandardCyborgFusion/Algorithm/PBFModel.cpp b/StandardCyborgFusion/Sources/StandardCyborgFusion/Algorithm/PBFModel.cpp index 875f0b88..a55c8c28 100644 --- a/StandardCyborgFusion/Sources/StandardCyborgFusion/Algorithm/PBFModel.cpp +++ b/StandardCyborgFusion/Sources/StandardCyborgFusion/Algorithm/PBFModel.cpp @@ -169,7 +169,9 @@ PBFAssimilatedFrameMetadata PBFModel::assimilate(ProcessedFrame& frame, ICPConfiguration icpConfig, SurfelFusionConfiguration surfelFusionConfiguration, double currentTime, - const std::vector* screenSpaceLandmarks) + const std::vector* screenSpaceLandmarks, + const Eigen::Matrix4f* headPoseDelta, + float headPoseConfidence) { // Summary of algorithm: // The first frame is defined to be identity for the world coordinates @@ -178,7 +180,19 @@ PBFAssimilatedFrameMetadata PBFModel::assimilate(ProcessedFrame& frame, // That resulting transform is multiplied into the world transform // The surfel index map is drawn from the point of view of the incoming frame (inverse world transform) // Points in the new frame are un-projected into 3D based on the world transform - + // + // Head-pose-prior extension (Phase B/C of head-pose-aware scan rework): + // When a caller (e.g. ARFaceCameraManager) provides headPoseDelta with high + // confidence, we trust it and skip ICP entirely -- this lets us fuse frames + // when ICP would otherwise reject them due to head motion that ICP can't + // distinguish from camera motion. At lower confidence we still run ICP but + // could (future) seed it with the prior; for now low-conf falls through to + // pure ICP. + + // Thresholds (tuned conservatively; high-conf bypass requires ARFaceAnchor + // .isTracked == true which Swift maps to confidence == 1.0f). + static const float kHighConfidenceThreshold = 0.9f; + // Get the current most recent metadata (haven't pushed this frame's metadata yet) PBFAssimilatedFrameMetadata* previousFrameMeta = _nthMostRecentValidFrameMetadata(0); @@ -192,44 +206,59 @@ PBFAssimilatedFrameMetadata PBFModel::assimilate(ProcessedFrame& frame, const RawFrame& rawFrame = frame.rawFrame; const size_t width = rawFrame.width; const size_t height = rawFrame.height; - + + const bool useHighConfPriorBypass = + (headPoseDelta != nullptr) && (headPoseConfidence >= kHighConfidenceThreshold); + if (_surfels.size() > 0) { - ICPResult icpResult = _runICP(frame, surfelFusionConfiguration, icpConfig, pbfConfig); - - Matrix4f extrinsicMatrixTmp = toMatrix4f(icpResult.sourceTransform) * _extrinsicMatrix; - // Store this whether or not we end up using it since we also store information about whether - // the frame was assimilated or not - frameMeta.viewMatrix = extrinsicMatrixTmp; - frameMeta.icpIterationCount = icpResult.iterationCount; - frameMeta.correspondenceError = icpResult.rmsCorrespondenceError; - - if (!icpResult.succeeded) { - DEBUG_LOG("ICP rejected due to bad convergence after %d/%d iterations", icpResult.iterationCount, icpConfig.maxIterations); - frameMeta.icpUnusedIterationFraction = 0; + if (useHighConfPriorBypass) { + // High-confidence head-pose prior: skip ICP. headPoseDelta is the + // frame-to-frame camera-in-head delta supplied by the caller; apply + // it to the cumulative extrinsic and proceed straight to fusion. + Matrix4f extrinsicMatrixTmp = (*headPoseDelta) * _extrinsicMatrix; + frameMeta.viewMatrix = extrinsicMatrixTmp; + frameMeta.icpIterationCount = 0; + frameMeta.correspondenceError = 0.0f; + frameMeta.icpUnusedIterationFraction = 1.0f; + _extrinsicMatrix = extrinsicMatrixTmp; } else { - frameMeta.icpUnusedIterationFraction = 1.0f - (float)icpResult.iterationCount / (float)icpConfig.maxIterations; - - CameraVelocity cv = _cameraVelocity(previousFrameMeta, &frameMeta); - - if (cv.angularVelocity.hasNaN() || cv.angularVelocity.norm() > pbfConfig.maxCameraAngularVelocity) { - DEBUG_LOG("Rejecting ICP due to bad fit with angular velocity %f", cv.angularVelocity.norm()); + ICPResult icpResult = _runICP(frame, surfelFusionConfiguration, icpConfig, pbfConfig); + + Matrix4f extrinsicMatrixTmp = toMatrix4f(icpResult.sourceTransform) * _extrinsicMatrix; + // Store this whether or not we end up using it since we also store information about whether + // the frame was assimilated or not + frameMeta.viewMatrix = extrinsicMatrixTmp; + frameMeta.icpIterationCount = icpResult.iterationCount; + frameMeta.correspondenceError = icpResult.rmsCorrespondenceError; + + if (!icpResult.succeeded) { + DEBUG_LOG("ICP rejected due to bad convergence after %d/%d iterations", icpResult.iterationCount, icpConfig.maxIterations); frameMeta.icpUnusedIterationFraction = 0; + } else { + frameMeta.icpUnusedIterationFraction = 1.0f - (float)icpResult.iterationCount / (float)icpConfig.maxIterations; + + CameraVelocity cv = _cameraVelocity(previousFrameMeta, &frameMeta); + + if (cv.angularVelocity.hasNaN() || cv.angularVelocity.norm() > pbfConfig.maxCameraAngularVelocity) { + DEBUG_LOG("Rejecting ICP due to bad fit with angular velocity %f", cv.angularVelocity.norm()); + frameMeta.icpUnusedIterationFraction = 0; + } + + else if (cv.velocity.hasNaN() || cv.velocity.norm() > pbfConfig.maxCameraVelocity) { + DEBUG_LOG("Rejecting ICP due to bad fit with linear velocity %f", cv.velocity.norm()); + frameMeta.icpUnusedIterationFraction = 0; + } } - - else if (cv.velocity.hasNaN() || cv.velocity.norm() > pbfConfig.maxCameraVelocity) { - DEBUG_LOG("Rejecting ICP due to bad fit with linear velocity %f", cv.velocity.norm()); - frameMeta.icpUnusedIterationFraction = 0; + + if (frameMeta.icpUnusedIterationFraction > 0) { + _extrinsicMatrix = extrinsicMatrixTmp; + } else { + // It didn't converge in time, so bail out + DEBUG_LOG("ICP didn't converge with enough quality (%f) after %d/%d iterations. Ignoring frame.", frameMeta.icpUnusedIterationFraction, icpResult.iterationCount, icpConfig.maxIterations); + _assimilatedFrameMetadatas.push_back(frameMeta); + return frameMeta; } } - - if (frameMeta.icpUnusedIterationFraction > 0) { - _extrinsicMatrix = extrinsicMatrixTmp; - } else { - // It didn't converge in time, so bail out - DEBUG_LOG("ICP didn't converge with enough quality (%f) after %d/%d iterations. Ignoring frame.", frameMeta.icpUnusedIterationFraction, icpResult.iterationCount, icpConfig.maxIterations); - _assimilatedFrameMetadatas.push_back(frameMeta); - return frameMeta; - } } if (_surfels.size() == 0) { diff --git a/StandardCyborgFusion/Sources/StandardCyborgFusion/Algorithm/PBFModel.hpp b/StandardCyborgFusion/Sources/StandardCyborgFusion/Algorithm/PBFModel.hpp index f10bbda2..41babc58 100644 --- a/StandardCyborgFusion/Sources/StandardCyborgFusion/Algorithm/PBFModel.hpp +++ b/StandardCyborgFusion/Sources/StandardCyborgFusion/Algorithm/PBFModel.hpp @@ -40,7 +40,9 @@ class PBFModel { ICPConfiguration icpConfig, SurfelFusionConfiguration surfelFusionConfiguration, double currentTime, - const std::vector* screenSpaceLandmarks = NULL); + const std::vector* screenSpaceLandmarks = NULL, + const Eigen::Matrix4f* headPoseDelta = nullptr, + float headPoseConfidence = 0.0f); PBFFinalStatistics finishAssimilating(SurfelFusionConfiguration surfelFusionConfiguration); diff --git a/StandardCyborgFusion/Sources/StandardCyborgFusion/Helpers/PerspectiveCamera+AVFoundation.mm b/StandardCyborgFusion/Sources/StandardCyborgFusion/Helpers/PerspectiveCamera+AVFoundation.mm index 2ed592c5..131f7d8a 100644 --- a/StandardCyborgFusion/Sources/StandardCyborgFusion/Helpers/PerspectiveCamera+AVFoundation.mm +++ b/StandardCyborgFusion/Sources/StandardCyborgFusion/Helpers/PerspectiveCamera+AVFoundation.mm @@ -10,6 +10,7 @@ #import #import #import +#import #import #import "EigenHelpers.hpp" @@ -18,6 +19,19 @@ using namespace standard_cyborg; +// iPhone 17 Pro and 17 Pro Max ship a 16:9 TrueDepth sensor that needs a +// different orientation matrix than the 4:3 sensor used on every prior +// iPhone. Apple identifies them as iPhone18,1 and iPhone18,2 respectively. +// Selecting by device model rather than sensor aspect ratio because the +// upstream's aspect-ratio heuristic mis-classified iPhone 15 Pro Max as +// widescreen and flipped its scan orientation. +static bool _isIPhone17ProFamily(void) +{ + struct utsname info; + if (uname(&info) != 0) return false; + return strncmp(info.machine, "iPhone18,", 9) == 0; +} + sc3d::PerspectiveCamera PerspectiveCameraFromAVCameraCalibrationData(AVCameraCalibrationData *calibrationData, size_t pixelsWide, size_t pixelsHigh) { NSData *lensDistortionLookupTableData = calibrationData.lensDistortionLookupTable; @@ -46,13 +60,9 @@ // coordinate system we otherwise prefer, so we post-multiply by an orientation matrix to // produce world space with x to the right, y up, and z toward the user. // - // On 4:3 TrueDepth sensors, the sensor's pixel-u axis points UP in portrait, so we swap - // X↔Y and negate Z. On 16:9 sensors (iPhone 17 Pro+), the pixel-u axis points RIGHT in - // portrait, so no swap is needed — just negate Y and Z. - CGSize refDims = calibrationData.intrinsicMatrixReferenceDimensions; - bool isWidescreenSensor = (refDims.width / refDims.height) > 1.5f; - - math::Mat3x4 desiredOrientation = isWidescreenSensor + // iPhone 17 Pro family (16:9 sensor): pixel-u axis points RIGHT in portrait — just negate + // Y and Z. Every other iPhone (4:3 sensor): swap X↔Y and negate Z. + math::Mat3x4 desiredOrientation = _isIPhone17ProFamily() ? math::Mat3x4({ 1, 0, 0, 0, 0, -1, 0, 0, diff --git a/StandardCyborgFusion/Sources/StandardCyborgFusion/Public/SCMeshTexturing.mm b/StandardCyborgFusion/Sources/StandardCyborgFusion/Public/SCMeshTexturing.mm index 5a6c92e6..4f4861f9 100644 --- a/StandardCyborgFusion/Sources/StandardCyborgFusion/Public/SCMeshTexturing.mm +++ b/StandardCyborgFusion/Sources/StandardCyborgFusion/Public/SCMeshTexturing.mm @@ -360,10 +360,13 @@ - (NSError *)_buildAPIError:(SCMeshTexturingAPIError)errorCode va_start(args, description); NSString *message = [[NSString alloc] initWithFormat:description arguments:args]; va_end(args); - + return [NSError errorWithDomain:SCMeshTexturingAPIErrorDomain code:errorCode - userInfo:@{NSDebugDescriptionErrorKey: message}]; + userInfo:@{ + NSLocalizedDescriptionKey: message, + NSDebugDescriptionErrorKey: message, + }]; } - (void)_removeDataFromPreviousRuns @@ -624,54 +627,102 @@ - (BOOL)_meshPointCloud:(SCPointCloud *)pointCloud error:(NSError **)errorOut progressHandler:(void (^)(float, BOOL *))progressHandler { + // Pre-meshing input validation. PoissonRecon will silently produce nothing + // for trivially sparse clouds; refuse early with a concrete reason so the + // app can show it instead of waiting through a doomed meshing pass. + static const NSInteger kMinPointCountForMeshing = 500; + NSInteger inputCount = pointCloud.pointCount; + if (inputCount < kMinPointCountForMeshing) { + if (errorOut != NULL) { + *errorOut = [self _buildAPIError:SCMeshTexturingAPIErrorArgument + description:@"Point cloud too sparse to mesh: %ld points (minimum %ld). Likely cause: registration diverged during scan, so few frames were fused.", + (long)inputCount, (long)kMinPointCountForMeshing]; + } + return NO; + } + // Write the point cloud to a .ply file, so we can use SCMeshingOperation NSString *plyFilename = @"temp-point-cloud.ply"; NSString *pointCloudPlyPath = [_containerPath stringByAppendingPathComponent:plyFilename]; NSString *outputPath = [pointCloudPlyPath stringByReplacingOccurrencesOfString:@".ply" withString:@"-mesh.ply"]; - + [self _ensureContainerDirectory]; - + + // Remove any stale output from a prior run so post-meshing existence checks + // are meaningful. + [[NSFileManager defaultManager] removeItemAtPath:outputPath error:NULL]; + BOOL success = [pointCloud writeToPLYAtPath:pointCloudPlyPath]; if (!success) { if (errorOut != NULL) { *errorOut = [self _buildAPIError:SCMeshTexturingAPIErrorInternal - description:@"Error writing to %@", pointCloudPlyPath]; + description:@"Error writing point cloud to %@", pointCloudPlyPath]; } return NO; } - + __block BOOL shouldStop = NO; - + SCMeshingOperation *operation = [[SCMeshingOperation alloc] initWithInputPLYPath:pointCloudPlyPath outputPLYPath:outputPath]; operation.parameters = parameters; - + __weak SCMeshingOperation *weakOperation = operation; operation.progressHandler = ^(float progress) { // Adapt the progress handler to allow cancellation progressHandler(progress, &shouldStop); - + if (shouldStop) { NSLog(@"Cancelling meshing operation"); [weakOperation cancel]; } }; - + [operation start]; - + if ([operation isCancelled]) { return NO; - } else { - io::ply::ReadGeometryFromPLYFile(geometryOut, std::string([outputPath UTF8String])); - - // color is no longer needed beyond this point. discard. - std::vector newColors(geometryOut.vertexCount(), math::Vec3{1, 1, 1}); - - if (!geometryOut.setColors(newColors)) { - return NO; + } + + // Surface silent failures from the C++ layer (PoissonRecon NULL solver, + // SurfaceTrimmer -1 returns, empty output PLY) rather than reading garbage. + if (operation.failureReason != nil) { + if (errorOut != NULL) { + *errorOut = [self _buildAPIError:SCMeshTexturingAPIErrorInternal + description:@"Meshing failed: %@", operation.failureReason]; } - - return YES; + return NO; + } + + if (![[NSFileManager defaultManager] fileExistsAtPath:outputPath]) { + if (errorOut != NULL) { + *errorOut = [self _buildAPIError:SCMeshTexturingAPIErrorInternal + description:@"Meshing produced no output file at %@", outputPath]; + } + return NO; + } + + io::ply::ReadGeometryFromPLYFile(geometryOut, std::string([outputPath UTF8String])); + + if (geometryOut.vertexCount() == 0) { + if (errorOut != NULL) { + *errorOut = [self _buildAPIError:SCMeshTexturingAPIErrorInternal + description:@"Meshing produced an empty mesh (0 vertices). Likely cause: input normals invalid or points coplanar."]; + } + return NO; + } + + // color is no longer needed beyond this point. discard. + std::vector newColors(geometryOut.vertexCount(), math::Vec3{1, 1, 1}); + + if (!geometryOut.setColors(newColors)) { + if (errorOut != NULL) { + *errorOut = [self _buildAPIError:SCMeshTexturingAPIErrorInternal + description:@"Failed to assign vertex colors to meshed geometry."]; + } + return NO; } + + return YES; } #ifdef SAVE_DIAGNOSTICS diff --git a/StandardCyborgFusion/Sources/StandardCyborgFusion/Public/SCMeshingOperation.mm b/StandardCyborgFusion/Sources/StandardCyborgFusion/Public/SCMeshingOperation.mm index 75273e5f..5af6b727 100644 --- a/StandardCyborgFusion/Sources/StandardCyborgFusion/Public/SCMeshingOperation.mm +++ b/StandardCyborgFusion/Sources/StandardCyborgFusion/Public/SCMeshingOperation.mm @@ -59,4 +59,9 @@ - (BOOL)isCancelled return [_operation isCancelled]; } +- (NSString *)failureReason +{ + return _operation.failureReason; +} + @end diff --git a/StandardCyborgFusion/Sources/StandardCyborgFusion/Public/SCReconstructionManager.mm b/StandardCyborgFusion/Sources/StandardCyborgFusion/Public/SCReconstructionManager.mm index e65f140e..a878bfb4 100644 --- a/StandardCyborgFusion/Sources/StandardCyborgFusion/Public/SCReconstructionManager.mm +++ b/StandardCyborgFusion/Sources/StandardCyborgFusion/Public/SCReconstructionManager.mm @@ -43,11 +43,15 @@ @interface _IncomingFrameData : NSObject @property (nonatomic, readonly) CVPixelBufferRef depthBuffer; @property (nonatomic, readonly) CVPixelBufferRef colorBuffer; @property (nonatomic, readonly) AVCameraCalibrationData *calibrationData; +@property (nonatomic, readonly) simd_float4x4 headPoseDelta; +@property (nonatomic, readonly) float headPoseConfidence; - (instancetype)initWithSequence:(int)sequence depthBuffer:(CVPixelBufferRef)depthBuffer colorBuffer:(CVPixelBufferRef)colorBuffer - calibrationData:(AVCameraCalibrationData *)calibrationData; + calibrationData:(AVCameraCalibrationData *)calibrationData + headPoseDelta:(simd_float4x4)headPoseDelta + headPoseConfidence:(float)headPoseConfidence; @end @@ -57,6 +61,8 @@ - (instancetype)initWithSequence:(int)sequence depthBuffer:(CVPixelBufferRef)depthBuffer colorBuffer:(CVPixelBufferRef)colorBuffer calibrationData:(AVCameraCalibrationData *)calibrationData + headPoseDelta:(simd_float4x4)headPoseDelta + headPoseConfidence:(float)headPoseConfidence { self = [super init]; if (self) { @@ -67,6 +73,8 @@ - (instancetype)initWithSequence:(int)sequence _depthBuffer = CVPixelBufferRetain(depthBuffer); _colorBuffer = CVPixelBufferRetain(colorBuffer); _calibrationData = calibrationData; + _headPoseDelta = headPoseDelta; + _headPoseConfidence = headPoseConfidence; } return self; } @@ -365,26 +373,41 @@ - (SCPointCloud *)reconstructSingleDepthBuffer:(CVPixelBufferRef)depthBuffer - (void)accumulateDepthBuffer:(CVPixelBufferRef)depthBuffer colorBuffer:(CVPixelBufferRef)colorBuffer calibrationData:(AVCameraCalibrationData *)calibrationData +{ + [self accumulateDepthBuffer:depthBuffer + colorBuffer:colorBuffer + calibrationData:calibrationData + headPoseDelta:matrix_identity_float4x4 + headPoseConfidence:0.0f]; +} + +- (void)accumulateDepthBuffer:(CVPixelBufferRef)depthBuffer + colorBuffer:(CVPixelBufferRef)colorBuffer + calibrationData:(AVCameraCalibrationData *)calibrationData + headPoseDelta:(simd_float4x4)headPoseDelta + headPoseConfidence:(float)headPoseConfidence { if (depthBuffer == NULL || colorBuffer == NULL || calibrationData == nil) { return; } CVPixelBufferRetain(depthBuffer); CVPixelBufferRetain(colorBuffer); - + dispatch_async(_inputQueue, ^{ _inputQueue_stopped = NO; - + int sequence = _inputQueue_incomingFrameSequence++; _IncomingFrameData *data = [[_IncomingFrameData alloc] initWithSequence:sequence depthBuffer:depthBuffer colorBuffer:colorBuffer - calibrationData:calibrationData]; + calibrationData:calibrationData + headPoseDelta:headPoseDelta + headPoseConfidence:headPoseConfidence]; CVPixelBufferRelease(depthBuffer); CVPixelBufferRelease(colorBuffer); - + // We only use the most recent raw frame, dropping any other ones that haven't had a chance to process BOOL dropped = _inputQueue_incomingFrameData != nil; _inputQueue_incomingFrameData = data; - + if (!dropped) { dispatch_semaphore_signal(_incomingFrameDataSemaphore); } @@ -574,8 +597,28 @@ - (PBFAssimilatedFrameMetadata)_modelQueue_assimilateIncomingFrameData:(_Incomin [self _modelQueue_unprojectRawFrameIntoFrame]; [self _modelQueue_configureModelForRawFrame]; - - auto metadata = _modelQueue_model->assimilate(*_modelQueue_frame, _pbfConfig, _icpConfig, _surfelFusionConfig, startTime); + + // Convert simd_float4x4 (column-major) to Eigen::Matrix4f for the C++ side. + // simd and Eigen agree on column-major layout, but Eigen wraps memory by + // reference via Map, so a plain memcpy through Matrix4f's data buffer is safe. + Eigen::Matrix4f headPoseDeltaEigen; + simd_float4x4 swiftPose = data.headPoseDelta; + for (int col = 0; col < 4; ++col) { + headPoseDeltaEigen(0, col) = swiftPose.columns[col].x; + headPoseDeltaEigen(1, col) = swiftPose.columns[col].y; + headPoseDeltaEigen(2, col) = swiftPose.columns[col].z; + headPoseDeltaEigen(3, col) = swiftPose.columns[col].w; + } + + const Eigen::Matrix4f* headPosePtr = (data.headPoseConfidence > 0.0f) ? &headPoseDeltaEigen : nullptr; + auto metadata = _modelQueue_model->assimilate(*_modelQueue_frame, + _pbfConfig, + _icpConfig, + _surfelFusionConfig, + startTime, + /* screenSpaceLandmarks */ nullptr, + headPosePtr, + data.headPoseConfidence); #ifndef XCODE_ACTION_install // Avoid logging in archive builds float quality = metadata.icpUnusedIterationFraction; diff --git a/StandardCyborgFusion/Sources/include/StandardCyborgFusion/SCMeshingOperation.h b/StandardCyborgFusion/Sources/include/StandardCyborgFusion/SCMeshingOperation.h index 6a18a918..2a28d25d 100644 --- a/StandardCyborgFusion/Sources/include/StandardCyborgFusion/SCMeshingOperation.h +++ b/StandardCyborgFusion/Sources/include/StandardCyborgFusion/SCMeshingOperation.h @@ -54,6 +54,13 @@ NS_ASSUME_NONNULL_BEGIN */ @property (nonatomic, copy) void (^progressHandler)(float progress); +/** + If meshing completed but produced no usable output (e.g. PoissonRecon silently + failed on a sparse/degenerate input cloud), this carries a human-readable reason. + nil if meshing succeeded or was cancelled. + */ +@property (nonatomic, readonly, nullable) NSString *failureReason; + @end NS_ASSUME_NONNULL_END diff --git a/StandardCyborgFusion/Sources/include/StandardCyborgFusion/SCReconstructionManager.h b/StandardCyborgFusion/Sources/include/StandardCyborgFusion/SCReconstructionManager.h index 4996e0fd..92238108 100644 --- a/StandardCyborgFusion/Sources/include/StandardCyborgFusion/SCReconstructionManager.h +++ b/StandardCyborgFusion/Sources/include/StandardCyborgFusion/SCReconstructionManager.h @@ -83,6 +83,21 @@ typedef struct { calibrationData:(AVCameraCalibrationData *)calibrationData NS_SWIFT_NAME(accumulate(depthBuffer:colorBuffer:calibrationData:)); +/** Variant carrying a head-pose-delta prior (camera-in-head frame-to-frame transform) + obtained from an external tracker such as ARKit's ARFaceAnchor. At high confidence + (>= 0.9) the prior bypasses ICP entirely, which lets the pipeline fuse frames where + plain ICP would otherwise diverge under head motion. At lower confidence the prior is + currently ignored and the standard ICP path runs. + + confidence is expected to be in [0, 1]. Pass simd_float4x4(1) and 0 to disable, which + is equivalent to the non-prior accumulate variant above. */ +- (void)accumulateDepthBuffer:(CVPixelBufferRef)depthBuffer + colorBuffer:(CVPixelBufferRef)colorBuffer + calibrationData:(AVCameraCalibrationData *)calibrationData + headPoseDelta:(simd_float4x4)headPoseDelta + headPoseConfidence:(float)headPoseConfidence +NS_SWIFT_NAME(accumulate(depthBuffer:colorBuffer:calibrationData:headPoseDelta:headPoseConfidence:)); + /** Pass in device motion updates as fast as they are made available by the system */ - (void)accumulateDeviceMotion:(CMDeviceMotion *)deviceMotion; diff --git a/StandardCyborgUI/StandardCyborgUI/Sources/ARFaceCameraManager.swift b/StandardCyborgUI/StandardCyborgUI/Sources/ARFaceCameraManager.swift new file mode 100644 index 00000000..8f5ab0c9 --- /dev/null +++ b/StandardCyborgUI/StandardCyborgUI/Sources/ARFaceCameraManager.swift @@ -0,0 +1,260 @@ +// +// ARFaceCameraManager.swift +// StandardCyborgUI +// +// Drop-in replacement for CameraManager that drives capture from an ARSession +// running ARFaceTrackingConfiguration. Per frame it supplies: +// - the AR-captured RGB image (converted YUV -> BGRA), +// - the depth pixel buffer from the front TrueDepth camera, +// - the per-frame AVCameraCalibrationData, +// - and a frame-to-frame "camera-in-face" pose delta derived from the +// ARFaceAnchor + ARCamera transforms, which the C++ fusion layer trusts +// as a high-confidence ICP bypass when face tracking is healthy. +// +// The face-frame delta is the key idea: by feeding deltas measured in the +// face anchor's local coordinates, the accumulated _extrinsicMatrix in +// PBFModel naturally lives in face-frame (Phase C), so surfels stay +// registered to the head even as the head rotates in world. +// + +import ARKit +import AVFoundation +import CoreImage +import Foundation + +@objc public protocol ARFaceCameraManagerDelegate: AnyObject { + /// Mirrors CameraManagerDelegate.cameraDidOutput plus a head-pose delta. + /// headPoseDelta is camera-in-face_N * inverse(camera-in-face_{N-1}); on the + /// very first valid frame the delta is identity. + /// confidence is in [0, 1]; >= 0.9 triggers the ICP-bypass path inside + /// SCReconstructionManager. + func arFaceCameraDidOutput(colorBuffer: CVPixelBuffer, + depthBuffer: CVPixelBuffer, + depthCalibrationData: AVCameraCalibrationData, + headPoseDelta: simd_float4x4, + headPoseConfidence: Float) + + /// Optional UI-feedback channel. Fires every face-tracking frame with the + /// face anchor's pose expressed in camera coordinates (inv(camera) * face). + /// Powers the Face-ID-style scan overlay (Phase E) that fills cardinal arcs + /// as the user rotates their head. + @objc optional func arFaceCameraDidObserveFacePose(_ facePoseInCamera: simd_float4x4, + isTracked: Bool) + + @objc optional func arFaceCameraManagerDidStartSession(_ manager: ARFaceCameraManager) + @objc optional func arFaceCameraManagerDidFailToStart(_ manager: ARFaceCameraManager, reason: String) +} + +/// UserDefaults key gating the Kabsch refiner pass (Phase D). When true and the +/// AR face pipeline is engaged, each frame runs a RANSAC + Kabsch validation +/// against ARKit's face-mesh vertices and downgrades head-pose confidence when +/// the constellation disagrees with ARKit's anchor transform. +public let kScanUseKabschRefinerDefaultsKey = "scan.use_kabsch_refiner" + +@objc public final class ARFaceCameraManager: NSObject { + + @objc public weak var delegate: ARFaceCameraManagerDelegate? + + @objc public class var isSupported: Bool { + ARFaceTrackingConfiguration.isSupported + } + + /// Public ARSession so callers (e.g. the scanning view controller) can + /// embed an ARSCNView or read tracking state if they want to. + public let session = ARSession() + + @objc public private(set) var isSessionRunning = false + + /// When true (driven by kScanUseKabschRefinerDefaultsKey), each frame + /// validates the ARKit-derived head-pose delta against a Kabsch alignment + /// over a face-mesh vertex constellation; low agreement -> confidence drops. + private lazy var _useKabschRefiner: Bool = { + UserDefaults.standard.bool(forKey: kScanUseKabschRefinerDefaultsKey) + }() + + // MARK: - Lifecycle + + @objc public override init() { + super.init() + session.delegate = self + session.delegateQueue = _delegateQueue + } + + @objc public func startSession() { + guard ARFaceCameraManager.isSupported else { + delegate?.arFaceCameraManagerDidFailToStart?(self, reason: "Face tracking not supported on this device") + return + } + let config = ARFaceTrackingConfiguration() + config.maximumNumberOfTrackedFaces = 1 + config.isLightEstimationEnabled = true + // Reset accumulated state so a new scan starts from identity. + _previousCameraInFace = nil + _previousFaceVerticesInWorld = nil + session.run(config, options: [.resetTracking, .removeExistingAnchors]) + isSessionRunning = true + delegate?.arFaceCameraManagerDidStartSession?(self) + } + + @objc public func stopSession() { + session.pause() + isSessionRunning = false + } + + // MARK: - Private state + + private let _delegateQueue = DispatchQueue(label: "ARFaceCameraManager.delegate", + qos: .userInitiated) + + // Reused per-frame to convert YpCbCr -> BGRA, the format the fusion layer expects. + private lazy var _ciContext: CIContext = { + if let device = MTLCreateSystemDefaultDevice() { + return CIContext(mtlDevice: device, options: nil) + } + return CIContext() + }() + + // The BGRA conversion target; allocated on demand and reused while its size matches. + private var _bgraConversionBuffer: CVPixelBuffer? + private var _bgraConversionSize: CGSize = .zero + + // Most-recently-emitted camera-in-face transform, used to compute the next delta. + private var _previousCameraInFace: simd_float4x4? + + // Subsampled face-mesh vertices of the previous frame, expressed in world + // coordinates. Used by the Kabsch refiner to validate the head-pose delta. + private var _previousFaceVerticesInWorld: [SIMD3]? +} + +// MARK: - ARSessionDelegate + +extension ARFaceCameraManager: ARSessionDelegate { + + public func session(_ session: ARSession, didUpdate frame: ARFrame) { + // We need a tracked face anchor. capturedDepthData is required for the + // scan path but the UI face-pose observer fires on every face-tracking + // frame (60Hz) regardless of depth availability, so we report it first. + let faceAnchor = frame.anchors.compactMap({ $0 as? ARFaceAnchor }).first + if let faceAnchor = faceAnchor { + let facePoseInCamera = simd_inverse(frame.camera.transform) * faceAnchor.transform + DispatchQueue.main.async { [weak self] in + guard let self = self else { return } + self.delegate?.arFaceCameraDidObserveFacePose?(facePoseInCamera, isTracked: faceAnchor.isTracked) + } + } + + // We need synchronized depth + a tracked face anchor. ARKit drops + // capturedDepthData on frames where the depth sensor didn't fire, + // which is roughly half of them at 60Hz RGB vs. 15Hz depth. + guard let depthData = frame.capturedDepthData else { return } + guard let faceAnchor = faceAnchor else { return } + + let depthBuffer = depthData.depthDataMap + guard let calibration = depthData.cameraCalibrationData else { return } + + // Convert YUV captured image to BGRA. The fusion layer's _fillDepthVector + // reads 4 bytes/pixel from this buffer. + guard let colorBuffer = _convertedBGRABuffer(from: frame.capturedImage) else { return } + + // Compute the camera-in-face transform for this frame, then the delta. + let faceWorld = faceAnchor.transform + let cameraWorld = frame.camera.transform + let cameraInFace = simd_inverse(faceWorld) * cameraWorld + + let delta: simd_float4x4 + if let prev = _previousCameraInFace { + delta = cameraInFace * simd_inverse(prev) + } else { + delta = matrix_identity_float4x4 + } + _previousCameraInFace = cameraInFace + + // Base confidence: ARFaceAnchor.isTracked is binary, ARCamera.trackingState + // is finer. Collapse to: 1.0 on healthy, 0.5 on limited, 0.0 on lost. + var confidence: Float + if !faceAnchor.isTracked { + confidence = 0.0 + } else { + switch frame.camera.trackingState { + case .normal: confidence = 1.0 + case .limited: confidence = 0.5 + case .notAvailable: confidence = 0.0 + } + } + + // Phase D: Kabsch refiner. Build a subsampled constellation of face-mesh + // vertices in world coordinates and validate ARKit's reported delta + // against a rigid alignment recovered from those vertices. Disagreement + // (low inlier fraction) drops the confidence so the C++ side falls back + // to plain ICP for this frame. + let currentVerticesInWorld = _subsampledFaceVerticesInWorld(faceAnchor: faceAnchor) + if _useKabschRefiner, confidence > 0, let prevVerts = _previousFaceVerticesInWorld, prevVerts.count == currentVerticesInWorld.count { + let inlierFraction = KabschRefiner.inlierFraction(previousVerticesInWorld: prevVerts, + currentVerticesInWorld: currentVerticesInWorld) + // Linearly scale confidence by inlier agreement. inlierFraction >= 0.9 + // leaves confidence ~unchanged; 0.5 halves it; below 0.3 effectively + // disables the prior for this frame. + confidence *= max(0, min(1, (inlierFraction - 0.3) / 0.6)) + } + _previousFaceVerticesInWorld = currentVerticesInWorld + + delegate?.arFaceCameraDidOutput(colorBuffer: colorBuffer, + depthBuffer: depthBuffer, + depthCalibrationData: calibration, + headPoseDelta: delta, + headPoseConfidence: confidence) + } + + /// Subsamples the ARKit face-mesh vertices into a small constellation + /// expressed in world coordinates. Used by the Kabsch refiner to validate + /// the head-pose delta. Stride matches KabschRefiner.defaultSubsampleStride. + private func _subsampledFaceVerticesInWorld(faceAnchor: ARFaceAnchor) -> [SIMD3] { + let vertices = faceAnchor.geometry.vertices + let stride = KabschRefiner.defaultSubsampleStride + let faceWorld = faceAnchor.transform + var out: [SIMD3] = [] + out.reserveCapacity(vertices.count / stride + 1) + var i = 0 + while i < vertices.count { + let v = vertices[i] + let worldH = faceWorld * SIMD4(v.x, v.y, v.z, 1) + out.append(SIMD3(worldH.x, worldH.y, worldH.z)) + i += stride + } + return out + } + + public func session(_ session: ARSession, didFailWithError error: Error) { + delegate?.arFaceCameraManagerDidFailToStart?(self, reason: error.localizedDescription) + } + + // MARK: - YUV -> BGRA conversion + + private func _convertedBGRABuffer(from yuvBuffer: CVPixelBuffer) -> CVPixelBuffer? { + let width = CVPixelBufferGetWidth(yuvBuffer) + let height = CVPixelBufferGetHeight(yuvBuffer) + let targetSize = CGSize(width: width, height: height) + + if _bgraConversionBuffer == nil || _bgraConversionSize != targetSize { + let attrs: [String: Any] = [ + kCVPixelBufferIOSurfacePropertiesKey as String: [:], + kCVPixelBufferMetalCompatibilityKey as String: true, + ] + var pb: CVPixelBuffer? + let status = CVPixelBufferCreate(kCFAllocatorDefault, + width, + height, + kCVPixelFormatType_32BGRA, + attrs as CFDictionary, + &pb) + guard status == kCVReturnSuccess, let buffer = pb else { return nil } + _bgraConversionBuffer = buffer + _bgraConversionSize = targetSize + } + + guard let dest = _bgraConversionBuffer else { return nil } + let image = CIImage(cvPixelBuffer: yuvBuffer) + _ciContext.render(image, to: dest) + return dest + } +} diff --git a/StandardCyborgUI/StandardCyborgUI/Sources/KabschRefiner.swift b/StandardCyborgUI/StandardCyborgUI/Sources/KabschRefiner.swift new file mode 100644 index 00000000..f023a1de --- /dev/null +++ b/StandardCyborgUI/StandardCyborgUI/Sources/KabschRefiner.swift @@ -0,0 +1,160 @@ +// +// KabschRefiner.swift +// StandardCyborgUI +// +// Validates the ARKit-derived camera-in-face head-pose delta by running a +// RANSAC + Kabsch rigid alignment over a constellation of stable face-mesh +// vertices. The rotation is recovered via polar decomposition (Higham +// iteration) rather than SVD, which avoids pulling in BLAS/LAPACK while +// remaining numerically robust for 3x3 covariance matrices. +// +// Used by ARFaceCameraManager as a confidence-adjustment layer: +// - if Kabsch and ARKit agree (small residual, high inlier ratio), +// we trust the prior at full confidence -> SCReconstructionManager +// bypasses ICP entirely; +// - if they disagree, we drop confidence so the C++ side falls back +// to plain ICP for that frame. +// +// This is the "patch constellation" Phase D approach adapted to ARKit's +// pre-tracked face mesh: ARKit's face-mesh vertices are already paired +// across frames by index, so we don't need optical flow / OpenCV. +// + +import ARKit +import Foundation +import simd + +@objc public final class KabschRefiner: NSObject { + + /// Subsample stride applied to the ~1220-vertex ARKit face mesh. Every + /// stride-th vertex is fed into RANSAC. 30 keeps the constellation small + /// enough to RANSAC quickly while still oversampling enough to survive + /// occlusion of large face regions under 30-45 degree rotations. + @objc public static let defaultSubsampleStride = 30 + + /// RANSAC iteration count. With ~40 vertices and 3-point hypotheses, this + /// gives ~99% probability of hitting an all-inlier sample when the true + /// inlier ratio is >= 0.5. + @objc public static let defaultRansacIterations = 20 + + /// Inlier residual threshold in metres. Face-mesh vertices live in a ~0.2 m + /// box; 5 mm is a tight-but-not-pathological threshold. + @objc public static let defaultInlierThresholdMeters: Float = 0.005 + + /// Validates a naive ARKit-derived camera-in-face delta against a Kabsch + /// alignment over the supplied face-mesh vertex constellation. + /// + /// - Parameters: + /// - previousVerticesInWorld: face-mesh vertices of frame N-1 in world coords. + /// - currentVerticesInWorld: face-mesh vertices of frame N in world coords. + /// - Returns: an inlier fraction in [0, 1] describing how many constellation + /// points agree with the recovered rigid transform. ARFaceCameraManager + /// maps this to a confidence multiplier on the head-pose prior. + public static func inlierFraction(previousVerticesInWorld: [simd_float3], + currentVerticesInWorld: [simd_float3], + inlierThresholdMeters: Float = defaultInlierThresholdMeters, + ransacIterations: Int = defaultRansacIterations) -> Float { + let n = min(previousVerticesInWorld.count, currentVerticesInWorld.count) + guard n >= 4 else { return 0 } + + var bestInlierCount = 0 + var rng = SystemRandomNumberGenerator() + + for _ in 0..(previousVerticesInWorld[i].x, + previousVerticesInWorld[i].y, + previousVerticesInWorld[i].z, + 1) + let transformed = candidate * src4 + let resid = simd_length(SIMD3(transformed.x, transformed.y, transformed.z) - currentVerticesInWorld[i]) + if resid < inlierThresholdMeters { + inlierCount += 1 + } + } + if inlierCount > bestInlierCount { + bestInlierCount = inlierCount + } + } + return Float(bestInlierCount) / Float(n) + } + + // MARK: - Sampling + + private static func sample3Indices(upperBound: Int, + using rng: inout SystemRandomNumberGenerator) -> (Int, Int, Int)? { + guard upperBound >= 3 else { return nil } + let i0 = Int.random(in: 0.. cross-covariance H -> polar decomp(H) -> rotation R, + /// then translation t = target_centroid - R * source_centroid. + static func kabschAlign(source: [SIMD3], target: [SIMD3]) -> simd_float4x4 { + let n = min(source.count, target.count) + guard n >= 3 else { return matrix_identity_float4x4 } + + var srcCentroid = SIMD3(repeating: 0) + var tgtCentroid = SIMD3(repeating: 0) + for i in 0...zero, SIMD3.zero, SIMD3.zero) + for i in 0..(R.columns.0, 0), + SIMD4(R.columns.1, 0), + SIMD4(R.columns.2, 0), + SIMD4(t, 1)) + } +} diff --git a/StandardCyborgUI/StandardCyborgUI/Sources/ScanningViewController.swift b/StandardCyborgUI/StandardCyborgUI/Sources/ScanningViewController.swift index fde8387e..ce783a66 100644 --- a/StandardCyborgUI/StandardCyborgUI/Sources/ScanningViewController.swift +++ b/StandardCyborgUI/StandardCyborgUI/Sources/ScanningViewController.swift @@ -7,22 +7,30 @@ import UIKit @objc optional func scanningViewController(_ controller: ScanningViewController, didScan pointCloud: SCPointCloud) } +/// UserDefaults key that opts the scan flow into the ARKit + head-pose-prior +/// pipeline. When false (default), the legacy AVCaptureSession + plain ICP +/// pipeline runs unchanged. When true and ARFaceTrackingConfiguration is +/// supported, the controller drives capture from an ARSession and feeds a +/// face-frame head-pose delta into SCReconstructionManager's ICP-bypass path. +public let kScanUseARFacePipelineDefaultsKey = "scan.use_ar_face_pipeline" + /** Shows a live color + depth camera preview and shutter button. - + When the shutter is tapped, performs a customizable 3-second countdown, then starts scanning. - + When scanning is manually finished, or if it fails, reconstructs a 3D point cloud and informs its delegate. - + This class does not itself show a preview of the scan. - + Rendering can be customized by setting the scanningViewRenderer to your own object conforming to that protocol. */ @objc open class ScanningViewController: UIViewController, CameraManagerDelegate, + ARFaceCameraManagerDelegate, SCReconstructionManagerDelegate { @@ -34,7 +42,13 @@ import UIKit } @objc public weak var delegate: ScanningViewControllerDelegate? - + + /// Optional Face-ID-style overlay hook. When the AR pipeline is engaged, + /// fires for every face-tracking frame with the face anchor's pose in + /// camera coordinates. Use this to drive a SwiftUI overlay (cardinal arc + /// fills, prompts, etc.). Always called on the main thread. + @objc public var facePoseObserver: ((simd_float4x4, Bool) -> Void)? + /** Override to drop in your own visualization */ @objc public lazy var scanningViewRenderer: ScanningViewRenderer = DefaultScanningViewRenderer(device: _metalDevice, commandQueue: _visualizationCommandQueue) @@ -58,11 +72,12 @@ import UIKit } @objc public func shutterTapped(_ sender: UIButton?) { + let sessionRunning = _cameraManager?.isSessionRunning ?? _arFaceCameraManager?.isSessionRunning ?? false guard presentedViewController == nil, - _cameraManager.isSessionRunning + sessionRunning else { return } - + switch _state { case .default: _startCountdown { self.startScanning() } @@ -100,8 +115,9 @@ import UIKit } if reason == .finished { - _cameraManager.stopSession() - + _cameraManager?.stopSession() + _arFaceCameraManager?.stopSession() + meshTexturing.cameraCalibrationData = _reconstructionManager.latestCameraCalibrationData meshTexturing.cameraCalibrationFrameWidth = _reconstructionManager.latestCameraCalibrationFrameWidth meshTexturing.cameraCalibrationFrameHeight = _reconstructionManager.latestCameraCalibrationFrameHeight @@ -124,20 +140,22 @@ import UIKit @objc public var maxDepthResolution: Int = 320 { didSet { if isViewLoaded && oldValue != maxDepthResolution { - _cameraManager.configureCaptureSession(maxResolution: maxDepthResolution) + _cameraManager?.configureCaptureSession(maxResolution: maxDepthResolution) } } } - + /** To manually pause the camera output, set this to true */ @objc public var isCameraPaused: Bool = false { didSet { guard oldValue != isCameraPaused else { return } - + if isCameraPaused { - _cameraManager.stopSession() + _cameraManager?.stopSession() + _arFaceCameraManager?.stopSession() } else { - _cameraManager.startSession(nil) + _cameraManager?.startSession(nil) + _arFaceCameraManager?.startSession() } } } @@ -166,31 +184,37 @@ import UIKit override open func viewDidLoad() { super.viewDidLoad() - + _setUpSubviews() - - _cameraManager.delegate = self - _cameraManager.configureCaptureSession(maxResolution: maxDepthResolution) - + + if let cm = _cameraManager { + cm.delegate = self + cm.configureCaptureSession(maxResolution: maxDepthResolution) + } + if let arcm = _arFaceCameraManager { + arcm.delegate = self + } + _reconstructionManager.delegate = self - + NotificationCenter.default.addObserver(self, selector: #selector(_thermalStateChanged), name: ProcessInfo.thermalStateDidChangeNotification, object: nil) } - + override open func viewDidAppear(_ animated: Bool) { super.viewDidAppear(animated) - + guard CameraManager.isDepthCameraAvailable else { return } - + _startCameraSession() } - + override open func viewWillDisappear(_ animated: Bool) { super.viewWillDisappear(animated) - + stopScanning(reason: ScanningViewController.ScanningTerminationReason.canceled) - - _cameraManager.stopSession() + + _cameraManager?.stopSession() + _arFaceCameraManager?.stopSession() } override open func viewDidLayoutSubviews() { @@ -236,10 +260,11 @@ import UIKit @objc private func _focusOnTap(_ gesture: UITapGestureRecognizer) { // Disallow this while scanning guard _state != _State.scanning else { return } - + let location = gesture.location(in: view) - - _cameraManager.focusOnTap(at: location) + + // Manual focus only applies to the AVCaptureSession path; ARKit controls focus itself. + _cameraManager?.focusOnTap(at: location) } @objc private func _thermalStateChanged(notification: Notification) { @@ -251,15 +276,45 @@ import UIKit } // MARK: - CameraManagerDelegate - + public func cameraDidOutput(colorBuffer: CVPixelBuffer, depthBuffer: CVPixelBuffer, depthCalibrationData: AVCameraCalibrationData) { + _handleFrame(colorBuffer: colorBuffer, + depthBuffer: depthBuffer, + depthCalibrationData: depthCalibrationData, + headPoseDelta: matrix_identity_float4x4, + headPoseConfidence: 0.0) + } + + // MARK: - ARFaceCameraManagerDelegate + + public func arFaceCameraDidOutput(colorBuffer: CVPixelBuffer, + depthBuffer: CVPixelBuffer, + depthCalibrationData: AVCameraCalibrationData, + headPoseDelta: simd_float4x4, + headPoseConfidence: Float) { + _handleFrame(colorBuffer: colorBuffer, + depthBuffer: depthBuffer, + depthCalibrationData: depthCalibrationData, + headPoseDelta: headPoseDelta, + headPoseConfidence: headPoseConfidence) + } + + public func arFaceCameraDidObserveFacePose(_ facePoseInCamera: simd_float4x4, isTracked: Bool) { + facePoseObserver?(facePoseInCamera, isTracked) + } + + private func _handleFrame(colorBuffer: CVPixelBuffer, + depthBuffer: CVPixelBuffer, + depthCalibrationData: AVCameraCalibrationData, + headPoseDelta: simd_float4x4, + headPoseConfidence: Float) { var isScanning = false DispatchQueue.main.sync { isScanning = self._state == _State.scanning } - + let pointCloud: SCPointCloud - + if isScanning { pointCloud = _reconstructionManager.buildPointCloud() } else { @@ -272,17 +327,19 @@ import UIKit with: depthCalibrationData, smoothingPoints: true) } - + scanningViewRenderer.draw(colorBuffer: colorBuffer, pointCloud: pointCloud, depthCameraCalibrationData: depthCalibrationData, viewMatrix: _latestViewMatrix, into: _metalLayer) - + if isScanning { _reconstructionManager.accumulate(depthBuffer: depthBuffer, colorBuffer: colorBuffer, - calibrationData: depthCalibrationData) + calibrationData: depthCalibrationData, + headPoseDelta: headPoseDelta, + headPoseConfidence: headPoseConfidence) } } @@ -330,7 +387,15 @@ import UIKit private lazy var _algorithmCommandQueue = _metalDevice.makeCommandQueue()! private lazy var _visualizationCommandQueue = _metalDevice.makeCommandQueue()! private lazy var _reconstructionManager = SCReconstructionManager(device: _metalDevice, commandQueue: _algorithmCommandQueue, maxThreadCount: _maxReconstructionThreadCount) - private let _cameraManager = CameraManager() + // Exactly one of these is non-nil based on the kScanUseARFacePipelineDefaultsKey flag at + // construction time. The legacy AVCaptureSession path is the default; the ARKit path + // engages when the flag is true AND the device supports face tracking. + private lazy var _useARFacePipeline: Bool = { + UserDefaults.standard.bool(forKey: kScanUseARFacePipelineDefaultsKey) + && ARFaceCameraManager.isSupported + }() + private lazy var _cameraManager: CameraManager? = _useARFacePipeline ? nil : CameraManager() + private lazy var _arFaceCameraManager: ARFaceCameraManager? = _useARFacePipeline ? ARFaceCameraManager() : nil private var _latestViewMatrix = matrix_identity_float4x4 private var _assimilatedFrameIndex = 0 @@ -426,16 +491,20 @@ import UIKit } - _cameraManager.isFocusLocked = _state == .scanning - + _cameraManager?.isFocusLocked = _state == .scanning + _mirrorModeBackground.isHidden = !showsMirrorModeButton _mirrorModeLabel.isHidden = !mirrorModeEnabled scanningViewRenderer.flipsInputHorizontally = mirrorModeEnabled _reconstructionManager.flipsInputHorizontally = mirrorModeEnabled } - + private func _startCameraSession() { - _cameraManager.startSession { result in + if let arcm = _arFaceCameraManager { + arcm.startSession() + return + } + _cameraManager?.startSession { result in switch result { case .success: break @@ -452,7 +521,7 @@ import UIKit { _ in UIApplication.shared.open(URL.init(string: UIApplication.openSettingsURLString)!, options: [:], completionHandler: nil) }) - + self.present(alertController, animated: true) } } diff --git a/TrueDepthFusion/ScanningViewRendering/DepthColoringFilter.swift b/TrueDepthFusion/ScanningViewRendering/DepthColoringFilter.swift index 959f0dec..a640f986 100644 --- a/TrueDepthFusion/ScanningViewRendering/DepthColoringFilter.swift +++ b/TrueDepthFusion/ScanningViewRendering/DepthColoringFilter.swift @@ -214,7 +214,7 @@ class DepthColoringFilter { private func _metalTexture(fromDepthBuffer depthBuffer: CVPixelBuffer, device: MTLDevice) -> MTLTexture? { let textureAttributes: [CFString: Any] = [ kCVPixelBufferMetalCompatibilityKey: true, - kCVMetalTextureUsage: MTLTextureUsage.shaderWrite.rawValue + kCVMetalTextureUsage: MTLTextureUsage.shaderRead.rawValue ] var texture: CVMetalTexture?