Log in or register to post comments

Problems calculating camera orientation and distance from target from pose matrix

October 21, 2014 - 3:21pm #1

I'm trying to calculate camera orientation with respect to my text target.
I would like to obtain camera orientation from trackable's pose matrix and then calculate the angles of inclination of the device.

I've read these articles:
and also some questions on the forum, like
but I'm still having problems.

More specifically after performing matrix calculation to obtain the camera position and orientation from model view matrix, I use a handler to write the calculated angles
(converted in degrees) on screen and the results are always the same or they changed a little but they are in any case wrong if compared to the position of the device with respect to the target.
I've also tried to calculate angles between axis in trackable's reference and axis in camera's reference
but I obtained always values that in my opinion can't represent the device orientation.

Furthermore I'm trying to obtain the distance from the target starting with the same information.

The article that I used as reference is https://developer.vuforia.com/resources/dev-guide/distance-target but sometimes I obtain distances from the target that are up to 4 centimeters longer than the real measure.
Could this be related to a small angle (due to the handle) that makes the device not fully perpendicular to the target?

The code I'm using:

WordResult wordResult = (WordResult) result;
Word word = (Word) wordResult.getTrackable();
if (trackable.getId() != lastTrackableId)
    lastTrackableId = trackable.getId();

    Matrix34F pose            = result.getPose();
    Matrix44F modelViewMatrix = Tool.convertPose2GLMatrix(pose);
    Matrix44F inverseMVMatrix = SampleMath.Matrix44FInverse(modelViewMatrix);
    Matrix44F invTranspMV     = SampleMath.Matrix44FTranspose(inverseMVMatrix);
    float cameraX         = invTranspMV.getData()[12];
    float cameraY         = invTranspMV.getData()[13];
    float cameraZ         = invTranspMV.getData()[14];
    float cameraRightX    = invTranspMV.getData()[0];
    float cameraRightY    = invTranspMV.getData()[1];
    float cameraRightZ    = invTranspMV.getData()[2];
    float cameraUpX       = -invTranspMV.getData()[4];
    float cameraUpY       = -invTranspMV.getData()[5];
    float cameraUpZ       = -invTranspMV.getData()[6];
    float cameraDirX      = invTranspMV.getData()[8];
    float cameraDirY      = invTranspMV.getData()[9];
    float cameraDirZ      = invTranspMV.getData()[10];
    Vec3F cameraPosition  = new Vec3F(cameraX, cameraY, cameraZ);
    Vec3F cameraRight     = new Vec3F(cameraRightX, cameraRightY, cameraRightZ);
    Vec3F cameraUp        = new Vec3F(cameraUpX, cameraUpY, cameraUpZ);
    Vec3F cameraDir       = new Vec3F(cameraDirX, cameraDirY, cameraDirZ);
    float trackableRightX = modelViewMatrix.getData()[0];
    float trackableRightY = modelViewMatrix.getData()[1];
    float trackableRightZ = modelViewMatrix.getData()[2];
    float trackableUpX    = modelViewMatrix.getData()[4];
    float trackableUpY    = modelViewMatrix.getData()[5];
    float trackableUpZ    = modelViewMatrix.getData()[6];
    float trackableDirX   = modelViewMatrix.getData()[8];
    float trackableDirY   = modelViewMatrix.getData()[9];
    float trackableDirZ   = modelViewMatrix.getData()[10];
    Vec3F trackableRight  = new Vec3F(trackableRightX, trackableRightY, trackableRightZ);
    Vec3F trackableUp     = new Vec3F(trackableUpX, trackableUpY, trackableUpZ);
    Vec3F trackableDir    = new Vec3F(trackableDirX, trackableDirY, trackableDirZ);
    float xAngle = (float) Math.acos(SampleMath.Vec3FDot(cameraRight, new Vec3F(1, 0, 0)) / Math.sqrt(SampleMath.Vec3FDot(cameraRight, cameraRight)));
    float yAngle = (float) Math.acos(SampleMath.Vec3FDot(cameraUp, new Vec3F(0, 1, 0)) / Math.sqrt(SampleMath.Vec3FDot(cameraUp, cameraUp)));
    float zAngle = (float) Math.acos(SampleMath.Vec3FDot(cameraDir, new Vec3F(0, 0, 1)) / Math.sqrt(SampleMath.Vec3FDot(cameraDir, cameraDir)));

  // These are related to the second attempt I described before                
                //    float xAngle         = (float) Math.acos(SampleMath.Vec3FDot(cameraRight, trackableRight) / (Math.sqrt(SampleMath.Vec3FDot(cameraRight, cameraRight))*Math.sqrt(SampleMath.Vec3FDot(trackableRight, trackableRight))));
               //     float yAngle         = (float) Math.acos((SampleMath.Vec3FDot(cameraUp, trackableUp) / (Math.sqrt(SampleMath.Vec3FDot(cameraUp, cameraUp))*Math.sqrt(SampleMath.Vec3FDot(trackableUp, trackableUp)))));
               //     float zAngle         = (float) Math.acos(SampleMath.Vec3FDot(cameraDir, trackableDir) / (Math.sqrt(SampleMath.Vec3FDot(cameraDir, cameraDir))*Math.sqrt(SampleMath.Vec3FDot(trackableDir, trackableDir))));

    displayMessage("xAngle: " + String.valueOf(xAngle * 360 / (2 * Math.PI)));
    displayMessage("yAngle: " + String.valueOf(yAngle * 360 / (2 * Math.PI)));
    displayMessage("zAngle: " + String.valueOf(zAngle * 360 / (2 * Math.PI)));
    Vec3F position = new Vec3F(pose.getData()[3], pose.getData()[7], pose.getData()[11]);

    double distance = Math.sqrt(position.getData()[0] * position.getData()[0] +
                                position.getData()[1] * position.getData()[1] +
                                position.getData()[2] * position.getData()[2]);

    Vec2F  vSize                 = word.getSize();
    double wordWidth             = vSize.getData()[0];
    double wordWidthInCentimeter = 8.8;
    double screenUPerCentimeter  = wordWidth / wordWidthInCentimeter;
    double distanceInCentimeter  = distance * wordWidthInCentimeter / wordWidth;

    displayMessage("Distance = " + String.valueOf(distanceInCentimeter) + " cm");

Thanks in advance.

Problems calculating camera orientation and distance from target from pose matrix

September 4, 2016 - 8:27am #2



Were you able to get the angle between camera and the trackable? 

Log in or register to post comments