Verwendung des POSIT-Algorithmus in OpenCV

Question

Ich übe den POSIT-Algorithmus in OpenCV, indem ich dem Tutorial folge http://opencv.willowgarage.com/wiki/Posit

Mein Ziel ist es, die Pose eines 3D-Objekts zu schätzen und insbesondere das Problem der unbekannten Tiefe zu lösen. In diesem Tutorial handelt es sich um einen Würfel mit einer Seitenlänge von 10 mm.

Teil des Codes:

std::vector<CvPoint2D32f> projectedPoints;
...
CvMat poseMatrix = cvMat( 4, 4, CV_32F, pose );  // pose moves object coord. to camera coord.
for ( size_t  p=0; p<modelPoints.size(); p++ )
{
    float modelPoint[] =  { modelPoints[p].x, modelPoints[p].y, modelPoints[p].z, 1.0f };
    CvMat modelPointMatrix = cvMat( 4, 1, CV_32F, modelPoint );
    float point3D[4];
    CvMat point3DMatrix = cvMat( 4, 1, CV_32F, point3D )
    //Transform the points from model space coordinates to camera space
    //The pose must be transposed because is in OpenGL format
    cvGEMM( &poseMatrix, &modelPointMatrix, 1.0, NULL, 0.0, &point3DMatrix, CV_GEMM_A_T );
    //Project the transformed 3D points
    CvPoint2D32f point2D = cvPoint2D32f( 0.0, 0.0 );
    if ( point3D[2] != 0 )
    {
            point2D.x = cvmGet( intrinsics, 0, 0 ) * point3D[0] / point3D[2]; // this is fx * X/Z
            point2D.y = cvmGet( intrinsics, 1, 1 ) * point3D[1] / point3D[2]; // this is fy * Y/Z
    }
    projectedPoints.push_back( point2D );
}

Gemäß der Gleichung ist Z bereits bekannt. Wie kann POSIT unbekannte Tiefe lösen? oder in diesem Tutorial gelöst?

Irgendeine Idee, bitte!