Vuforia inverse model matrix produces undesired effects. How to fix? - augmented-reality

Question

I'm using Vuforia on Android for AR development. We can obtain the modelViewMatrix using
Matrix44F modelViewMatrix_Vuforia = Tool.convertPose2GLMatrix(trackableResult.getPose());
This works great. Any geometry multiplied by this matrix and then by the projection matrix shows up on the screen as expected, with (0,0,0) at the centre of the tracked target.
But what I also want to do is to simultaneously draw geometry relative to the user's device, so to achieve this we can work out the inverse modelViewMatrix using:
Matrix44F inverseMV = SampleMath.Matrix44FInverse(invTranspMV);
Matrix44F invTranspMV = SampleMath.Matrix44FTranspose(modelViewMatrix_Vuforia);
modelViewMatrixInverse = invTranspMV.getData();
This works pretty well, e.g. if I draw a cube using this matrix, then when I tilt my phone up and down, the cube is also tilted up and down correctly, but when I turn left and right there's a problem. Left turning causes the cube to turn the wrong way as if I'm looking to the right hand side of it. Similarly with right turning. What should be happening is that the cube should appear "stuck" to the screen, i.e. which ever way I turn I should be able to see the same face "stuck" to the screen always.
I think the problem might be do with the Vuforia projection matrix, and I am going to create my own projection matrix (using guidance here) to experiment with different settings. As this post says, it could be to do with the intrinsic camera calibration of a specific device.
Am I on the right track? Any ideas what might be wrong and how I might solve this?
UPDATE
I don't think it's the projection matrix anymore (due to experimentation and peedee's answer comment below)
Having looked at this post I think I've made some progress. I am now using the following code:
Matrix44F modelViewMatrix_Vuforia = Tool.convertPose2GLMatrix(trackableResult.getPose());
Matrix44F inverseMV = SampleMath.Matrix44FInverse(modelViewMatrix_Vuforia);
Matrix44F invTranspMV = SampleMath.Matrix44FTranspose(inverseMV);
modelViewMatrixInverse = invTranspMV.getData();
float [] position = {0, 0, 0, 1};
float [] lookAt = {0, 0, 1, 0};
float [] cam_position = new float[16];
float [] cam_lookat = new float[16];
Matrix.multiplyMV(cam_position, 0, modelViewMatrixInverse, 0, position, 0);
Matrix.multiplyMV(cam_lookat, 0, modelViewMatrixInverse, 0, lookAt, 0);
Log.v("QCV", "posx = " + cam_position[0] + ", posy = " + cam_position[1] + ", posz = " + cam_position[2]);
Log.v("QCV", "latx = " + cam_lookat[0] + ", laty = " + cam_lookat[1] + ", latz = " + cam_lookat[2]);
This successfully returns the camera position, and the normal to the camera as you move the camera about the target. I think I should be able to use this to project geometry in the way I want. Will update later if it works.
UPDATE2
Ok, some progress made. I'm now using the following code. It does the same thing as the previous code block but uses Matrix class instead of the SampleMath class.
float [] temp = new float[16];
temp = modelViewMatrix_Vuforia.getData();
Matrix.invertM(modelViewMatrixInverse, 0, temp, 0);
float [] position = {0, 0, 0, 1};
float [] lookAt = {0, 0, 1, 0};
float [] cam_position = new float[16];
float [] cam_lookat = new float[16];
Matrix.multiplyMV(cam_position, 0, modelViewMatrixInverse, 0, position, 0);
Matrix.multiplyMV(cam_lookat, 0, modelViewMatrixInverse, 0, lookAt, 0);
Log.v("QCV", "posx = " + cam_position[0] / kObjectScale + ", posy = " + cam_position[1] / kObjectScale + ", posz = " + cam_position[2] / kObjectScale);
Log.v("QCV", "latx = " + cam_lookat[0] + ", laty = " + cam_lookat[1] + ", latz = " + cam_lookat[2]);
The next bit of code gives (almost) the desired result:
modelViewMatrix = modelViewMatrix_Vuforia.getData();
Matrix.translateM(modelViewMatrix, 0, 0, 0, kObjectScale);
Matrix.scaleM(modelViewMatrix, 0, kObjectScale, kObjectScale, kObjectScale);
line.setVerts(cam_position[0] / kObjectScale,
cam_position[1] / kObjectScale,
cam_position[2] / kObjectScale,
cam_position[0] / kObjectScale + 0.5f,
cam_position[1] / kObjectScale + 0.5f,
cam_position[2] / kObjectScale - 30);
This defines a line along the negative z-axis from position vector equal to the camera position (which is calculated from the position of the actual physical device). Since the vector is normal, I have offsetted the X/Y so the normal can actually be visualised.
As you reposition your physical device, the normal moves with you. Great!
However, keeping the phone in the same position, but tilting the phone forwards/backwards or turning left/right, the line does not maintain it's central position within the camera's display. The effect I want is for the line to be rotated in world space as I tilt/turn so that in camera/screen space the line appears normal and is central to the physical display.
Note - you may wonder why I don't use something like:
line.setVerts(cam_position[0] / kObjectScale,
cam_position[1] / kObjectScale,
cam_position[2] / kObjectScale,
cam_position[0] / kObjectScale + cam_lookat[0] * 30,
cam_position[1] / kObjectScale + cam_lookat[1] * 30,
cam_position[2] / kObjectScale + cam_lookat[2] * 30);
The simple answer is I did try and it doesn't work ! All this achieves is that one end of the line stays where it is, whilst the other end points in the direction of the screen device normal. What we need is to rotate the line in world space based on angles obtained from cam_lookat so that the line actually appears in front of the camera in the centre and normal to the camera.
The next stage is to adjust the position of the line in world space based on angles calculated from the cam_lookat unit vector. These can be used to update the vertices of the line so that the normal always appears in the centre of the camera whichever way you orient the phone.
I think this is the right way to go. I will update again if this works!

Answer 1

Ok, this was a tough nut to crack but success is sooo sweet!
One crucial part is that it uses a function from SampleMath to compute the start of an intersection line from the centre of the physical device to the target. We combine this with the camera normal vector to get the line we want !
If you want to dig deeper I'm sure you can unearth/workout the matrix math behind the getPointToPlaneLineStart function.
This is the code that works. It's not optimal so you can probably tidy it up a bit/lot!
modelViewMatrix44F = Tool.convertPose2GLMatrix(trackableResult.getPose());
modelViewMatrixInverse44F = SampleMath.Matrix44FInverse(modelViewMatrix44F);
modelViewMatrixInverseTranspose44F = SampleMath.Matrix44FTranspose(modelViewMatrix44F);
modelViewMatrix = modelViewMatrix44F.getData();
Matrix.translateM(modelViewMatrix, 0, 0, 0, kObjectScale);
Matrix.scaleM(modelViewMatrix, 0, kObjectScale, kObjectScale, kObjectScale);
modelViewMatrix44F.setData(modelViewMatrix);
projectionMatrix44F = vuforiaAppSession.getProjectionMatrix();
projectionMatrixInverse44F = SampleMath.Matrix44FInverse(projectionMatrix44F);
projectionMatrixInverseTranspose44F = SampleMath.Matrix44FTranspose(projectionMatrixInverse44F);
// work out camera position and direction
modelViewMatrixInverse = modelViewMatrixInverseTranspose44F.getData();
position = new float [] {0, 0, 0, 1}; // camera position
lookAt = new float [] {0, 0, 1, 0}; // camera direction
float [] rotate = new float [] {(float) Math.cos(angle_degrees * 0.017453292f), (float) Math.sin(angle_degrees * 0.017453292f), 0, 0};
angle_degrees += 10;
if(angle_degrees > 359)
angle_degrees = 0;
float [] cam_position = new float[16];
float [] cam_lookat = new float[16];
float [] cam_rotate = new float[16];
Matrix.multiplyMV(cam_position, 0, modelViewMatrixInverse, 0, position, 0);
Matrix.multiplyMV(cam_lookat, 0, modelViewMatrixInverse, 0, lookAt, 0);
Matrix.multiplyMV(cam_rotate, 0, modelViewMatrixInverse, 0, rotate, 0);
Vec3F line_start = SampleMath.getPointToPlaneLineStart(projectionMatrixInverse44F, modelViewMatrix44F, 2*kObjectScale, 2*kObjectScale, new Vec2F(0, 0), new Vec3F(0, 0, 0), new Vec3F(0, 0, 1));
float x1 = line_start.getData()[0];
float y1 = line_start.getData()[1];
float z1 = line_start.getData()[2];
float x2 = x1 + cam_lookat[0] * 3 + cam_rotate[0] * 0.1f;
float y2 = y1 + cam_lookat[1] * 3 + cam_rotate[1] * 0.1f;
float z2 = z1 + cam_lookat[2] * 3 + cam_rotate[2] * 0.1f;
line.setVerts(x1, y1, z1, x2, y2, z2);
Note - I added the cam_rotate vector so that you could see the line, otherwise you can't see it - or at least you only see a speck on the screen - because it is defined to be perpendicular to the screen !
And it's Friday so I might go to the pub later to celebrate :-)
UPDATE
In fact the getPointToPlaneLineStart Java SampleMath method calls the following code (C++), so you can probably decipher the matrix math from it if you don't want to use the SampleMath class (c.f. this post)
SampleMath::projectScreenPointToPlane(QCAR::Matrix44F inverseProjMatrix, QCAR::Matrix44F modelViewMatrix,
float contentScalingFactor, float screenWidth, float screenHeight,
QCAR::Vec2F point, QCAR::Vec3F planeCenter, QCAR::Vec3F planeNormal,
QCAR::Vec3F &intersection, QCAR::Vec3F &lineStart, QCAR::Vec3F &lineEnd)
{
// Window Coordinates to Normalized Device Coordinates
QCAR::VideoBackgroundConfig config = QCAR::Renderer::getInstance().getVideoBackgroundConfig();
float halfScreenWidth = screenHeight / 2.0f;
float halfScreenHeight = screenWidth / 2.0f;
float halfViewportWidth = config.mSize.data[0] / 2.0f;
float halfViewportHeight = config.mSize.data[1] / 2.0f;
float x = (contentScalingFactor * point.data[0] - halfScreenWidth) / halfViewportWidth;
float y = (contentScalingFactor * point.data[1] - halfScreenHeight) / halfViewportHeight * -1;
QCAR::Vec4F ndcNear(x, y, -1, 1);
QCAR::Vec4F ndcFar(x, y, 1, 1);
// Normalized Device Coordinates to Eye Coordinates
QCAR::Vec4F pointOnNearPlane = Vec4FTransform(ndcNear, inverseProjMatrix);
QCAR::Vec4F pointOnFarPlane = Vec4FTransform(ndcFar, inverseProjMatrix);
pointOnNearPlane = Vec4FDiv(pointOnNearPlane, pointOnNearPlane.data[3]);
pointOnFarPlane = Vec4FDiv(pointOnFarPlane, pointOnFarPlane.data[3]);
// Eye Coordinates to Object Coordinates
QCAR::Matrix44F inverseModelViewMatrix = Matrix44FInverse(modelViewMatrix);
QCAR::Vec4F nearWorld = Vec4FTransform(pointOnNearPlane, inverseModelViewMatrix);
QCAR::Vec4F farWorld = Vec4FTransform(pointOnFarPlane, inverseModelViewMatrix);
lineStart = QCAR::Vec3F(nearWorld.data[0], nearWorld.data[1], nearWorld.data[2]);
lineEnd = QCAR::Vec3F(farWorld.data[0], farWorld.data[1], farWorld.data[2]);
linePlaneIntersection(lineStart, lineEnd, planeCenter, planeNormal, intersection);
}

Answer 2

I'm by no means an expert, but it sounds to me like this left/right inversion should be expected. In my mind, the object in world space is looking in the direction of the positive z-axis towards the camera, while the camera space is looking in the direction of the negative z-axis facing the camera. Such a transformation of the coordinate system is bound to invert one of the x/y-axes to keep the coordinate system consistent.
ELI5: When you're standing in front of someone and tell them "on the count of 3 we both step to the left", you won't be standing in front of each other anymore afterwards.
I think it's unlikely to be a problem with the projection matrix as you said. The projection matrix merely transforms the 3d objects onto your 2d screen. Also the camera intrinsics doesn't sound like the right place to me. That matrix will correct for small distortions caused by the camera lens shape and placement, nothing as drastic as a left/right inversion.
Unfortunately I also don't know how to solve it right now, but what I had to say was too long for a comment. Sorry :-(