How to Calculate The Camera Position from Vuforia Gl Matrix

Using Vuforia provided Projection Matrix and Marker Pose in SceneKit

It just works!

The hard part is determining what pieces of SceneKit are necessary to make this work. Originally I read the article Making Augmented Reality app easily with Scenekit + Vuforia which outlined how to rejigger the sample app for user-defined targets. The downsides to that article include that it isn't always clear what the author changed, no sample project is provided, and it is based upon an older version of Vuforia. Ultimately, I found it unnecessary to invert the pose matrix.

Draw camera image and set projection matrix and update marker pose

override func viewDidLoad() 
{
    super.viewDidLoad()

    let scene = SmartScanScene()

    let camera = SCNCamera()
    let cameraNode = SCNNode()
    cameraNode.camera = camera
    scene.rootNode.addChildNode(cameraNode)
    _cameraNode = cameraNode

    let view = self.view as! SCNView
    view.backgroundColor = UIColor.blackColor()
    view.showsStatistics = true
    // view.debugOptions = SCNDebugOptions.ShowBoundingBoxes.union(.ShowWireframe)
    view.autoenablesDefaultLighting = true
    view.allowsCameraControl = false
}

func didUpdateProjectionMatrix(projectionMatrix: matrix_float4x4)
{
    let extrinsic = SCNMatrix4FromMat4(projectionMatrix)
    _cameraNode?.camera?.setProjectionTransform(extrinsic)
}

func didUpdateFramemarkers(framemarkers: [Framemarker]?)
{
    guard let framemarkers = framemarkers else {
        return
    }

    for framemarker in framemarkers {
        let pose = SCNMatrix4FromMat4(framemarker.pose)
        self.objectNode?.transform = pose
    }
}

func didUpdateCameraImage(image: UIImage?)
{
    if let image = image {
        _scene?.background.contents = image
    }
}

How to convert camera extrinsic matrix to SCNCamera position and rotation

SceneKit has the same view matrixes that you've come across in OpenGL, they're just a little hidden until you start toying with shaders. A little too hidden IMO.

You seem to have most of this figured out. The projection matrix comes from your camera projectionTransform, and the view matrix comes from the inverse of your camera matrix SCNMatrix4Invert(cameraNode.transform). In my case everything was in world coordinates making my model matrix a simple identity matrix.

The code I ended up using to get the classic model-view-projection matrix was something like...

let projection = camera.projectionTransform()
let view = SCNMatrix4Invert(cameraNode.transform)
let model = SCNMatrix4Identity

let viewProjection = SCNMatrix4Mult(view, projection)
let modelViewProjection = SCNMatrix4Mult(model, viewProjection)

For some reason I found SCNMatrix4Mult(...) took arguments in a different order than I was expecting (eg; opposite to GLKMatrix4Multiply(...)).

I'm still not 100% on this, so would welcome edits/tips. Using this method I was unable to get the SceneKit MVP matrix (as passed to shader) to match up with that calculated by the code above... but it was close enough for what I needed.

Getting a translate relative to a different matrix

Assuming that the object position is initially described by worldToCamera * cameraToTrackerB * objectPositionRelativeToTrackerB, you can get the object position relative to trackerA as currentObjectPositionRelativeToTrackerA = inv(cameraToTrackerA) * cameraToTrackerB * objectPositionRelativeToTrackerB.

Find my camera's 3D position and orientation according to a 2D marker

You can find some example code for the EPnP algorithm on this webpage. This code consists in one header file and one source file, plus one file for the usage example, so this shouldn't be too hard to include in your code.

Note that this code is released for research/evaluation purposes only, as mentioned on this page.

EDIT:

I just realized that this code needs OpenCV to work. By the way, although this would add a pretty big dependency to your project, the current version of OpenCV has a builtin function called solvePnP, which does what you want.

How can I get the WorldToScreenPoint from a camera position in the past?

I have found the solution on my own:

Save the projectionMatrix and worldToCameraMatrix of you camera
and then run this:

Matrix4x4 matrix = projectionMatrix * worldToCameraMatrix;
Vector3 screenPos = matrix.MultiplyPoint(destination.transform.position);

// (-1, 1)'s clip => (0 ,1)'s viewport 
screenPos = new Vector3(screenPos.x + 1f, screenPos.y + 1f, screenPos.z + 1f) / 2f;

// viewport => screen
screenPos = new Vector3(screenPos.x * Screen.width, screenPos.y * Screen.height, screenPos.z);
var unityScreenPos = new Vector2(screenPos.x, Screen.height - screenPos.y);