LiDAR Depth + Vision Hand Tracking for 3D Hand Tracking

Viewed 544

I want to use Vision 2D Hand Tracking input coupled with ARKit > People Occlusion > Body Segmentation With Depth, which leverage LiDAR, to get 3D World Coordinates of the tip of the index.

Steps I am doing:

1 - The 2D screen location of the finger tip provided by Vision works

2 - The Depth data from the CVPixelBuffer seems correct too

3 - The unprojection from 2D Screen Coordinates + Depth data to 3D World Coordinates is wrong

Ideally I could have a result similar to the LiDAR Lab app by Josh Caspersz:

enter image description here

Here is my code which treats the 2D point coordinates + depth into 3D world coordinates:

// Result from Vision framework
// Coordinates top right of the screen with Y to the left, X down
indexTip = CGPoint(x:(indexTipPoint.location.x) * CGFloat(arView.bounds.width),
                           y:(1 - indexTipPoint.location.y) * CGFloat(arView.bounds.height))
        
if let segmentationBuffer:CVPixelBuffer = frame.estimatedDepthData {

      let segmentationWidth = CVPixelBufferGetWidth(segmentationBuffer)
      let segmentationHeight = CVPixelBufferGetHeight(segmentationBuffer)
            
      let xConverted:CGFloat = indexTip.x * CGFloat(segmentationWidth) / CGFloat(arView.bounds.width)
      let yConverted:CGFloat = indexTip.y * CGFloat(segmentationHeight) / CGFloat(arView.bounds.height)

      if let indexDepth:Float = segmentationBuffer.value(column: Int(xConverted), row: Int(yConverted)) {

           if indexDepth != 0 {
                 let cameraIntrinsics = frame.camera.intrinsics

                 var xrw: Float = (Float(indexTip.x) - cameraIntrinsics[2][0]) * indexDepth
                 xrw = xrw / cameraIntrinsics[0][0]
                 var yrw: Float = (Float(indexTip.y) - cameraIntrinsics[2][1]) * indexDepth
                 yrw = yrw / cameraIntrinsics[1][1]
                 let xyzw: SIMD4<Float> = SIMD4<Float>(xrw, yrw, indexDepth, 1.0)
                 let vecResult = frame.camera.viewMatrix(for: .portrait) * xyzw
                    
                 resultAnchor.setPosition(SIMD3<Float>(vecResult.x, vecResult.y, vecResult.z), relativeTo: nil)
           }
      }
 }

Here is a video of what it looks like when running, seems to be always located at a specific area in space: Video

The calculations are basically the ones from the Sample Code Displaying a Point Cloud Using Scene Depth

enter image description here

Finally here is the full zip file if you want to try it yourself : ZIP.

Any idea what is wrong in my calculations?

1 Answers
Related