December 26, 2014

Render the captured image from real camera matrix using OpenGL

// intrinsic parameter: fx, fy, cx, cy, skew
// image size: width, hegiht
// view frustum: nearp, farp
// extrinsic parameter: r00, r01, r02, r10, r11, r12, r20, r21, r22, tx, ty, tz

// projection matrix
float pm[16] = { 0.0f };
pm[0]  = 2.0f*fx/width;
pm[4]  = -2.0f*skew/width;
pm[8]  = 1.0f - 2.0f*cx/width;
pm[12] = 0.0f;
pm[1]  = 0.0f;
pm[5]  = -2.0f*fy/height;
pm[9]  = 1.0f - 2.0f*cy/height;
pm[13] = 0.0f;
pm[2]  = 0.0f;
pm[6]  = 0.0f;
pm[10] = -(farp+nearp)/(farp-nearp);
pm[14] = -2.0f*farp*nearp/(farp-nearp);
pm[3]  = 0.0f;
pm[7]  = 0.0f;
pm[11] = -1.0f;
pm[15] = 0.0f;

glViewport ( 0, 0, width, height);
glMatrixMode ( GL_PROJECTION );

glLoadMatrixf ( pm );

glMatrixMode ( GL_MODELVIEW );
glLoadIdentity();

// -z direction
float rm[16] = { 0.0f };
rm[0]  = 1.0f;
rm[5]  = 1.0f;
rm[10] = -1.0f;
rm[15] = 1.0f;

glMultMatrixf ( rm );

rm[0]  = r00;
rm[4]  = r01;
rm[8]  = r02;
rm[1]  = r10;
rm[5]  = r11;
rm[9]  = r12;
rm[2]  = r20;
rm[6]  = r21;
rm[10] = r22;
rm[15] = 1.0f;

// inverse rotation and translation

glMultTransposeMatrixf ( rm );
glTranslatef ( -tx, -ty, -tz );