Question

I’m practising POSIT algorithm in OpenCV, following the tutorial http://opencv.willowgarage.com/wiki/Posit

My goal is to estimate the pose of a 3D object and especially solve the unknown depth problem. In this tutorial, it is a cube side of which is 10mm.

part of the code:

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 );
}

According to the equation, Z is already known. How can POSIT solve unknown depth? or solved in this tutorial?

Any idea, please!

Answer 1

cvPOSIT estimates the translation (X,Y,Z) and rotation vectors between the camera’s and model’s coordinates systems. The code you posted calculates the projections of the model points using a pose matrix. In the tutorial, that function is used with the real pose of the model (unknown in a real scenario) and the pose estimated by POSIT in order to compare both projections.
cvGEMS performs the points transformation( R*p + T ) in just one matrix multiplication. Note that the model points p have 4 coordinates(x,y,z,1).