OpenCV passed co-odrnates to OpenGL but OpenGL can`t draw it - c

Question

Below is my program which calculates the rotation vector, rotation matrix from chessboard through OpenCV. And then by creating array of 16 values from this vector and matrix I am passing this to OpenGL. But OpenGL window cant show anything.
Is this way is correct to connect OpenCV and OpenGL?
Why OpenGL not drawing the shape?
// this function for calculating rotation matrix and vector
void calculateOpenCVPoints(IplImage* image)
{
board_w = 4; // Board width in squares
board_h = 5; // Board height
n_boards =3; // Number of boards
board_n = board_w * board_h;
CvSize board_sz = cvSize( board_w, board_h );
CvMat* warp_matrix = cvCreateMat(3,3,CV_32FC1);
CvPoint2D32f* corners = new CvPoint2D32f[ board_n ];
int corner_count;
char pressedChar;
//cvNamedWindow("Livevideo",CV_WINDOW_AUTOSIZE);
imgPoints = cvCreateMat(n_boards*board_n,2,CV_32FC1) ;
objectPoints=cvCreateMat(n_boards*board_n,3,CV_32FC1);
pointCounts = cvCreateMat(n_boards,1,CV_32SC1);
intrinsicMatrix = cvCreateMat(3,3,CV_32FC1);
distortionCoeffs = cvCreateMat(5,1,CV_32FC1);
corners = new CvPoint2D32f[ board_n ];
IplImage *grayImage = cvCreateImage(cvGetSize(image),8,1);//subpixel
successes = 0;
while(successes < n_boards)
{
printf("\nIn calculateOpenCVPoints function for calculating image points ==%d",successes);
//Skip every board_dt frames to allow user to move chessboard
if(frame++ % board_dt == 0)
{
//Find chessboard corners:
int found = cvFindChessboardCorners(image, board_sz, corners, &cornerCount,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
cvCvtColor( image, grayImage, CV_BGR2GRAY );
cvFindCornerSubPix( grayImage, corners, cornerCount, cvSize( 11, 11 ),
cvSize( -1, -1 ), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
cvDrawChessboardCorners(image, board_sz, corners,cornerCount, found);
cvShowImage("Calibration", image );
if( cornerCount == board_n )
{
step = successes*board_n;
for( int i=step, j=0; j < board_n; ++i, ++j )
{
CV_MAT_ELEM( *imgPoints, float, i, 0 ) = corners[j].x;
CV_MAT_ELEM( *imgPoints, float, i, 1 ) = corners[j].y;
CV_MAT_ELEM( *objectPoints, float, i, 0 ) = j/board_w;
CV_MAT_ELEM( *objectPoints, float, i, 1 ) = j%board_w;
CV_MAT_ELEM( *objectPoints, float, i, 2 ) = 0.0f;
}
CV_MAT_ELEM( *pointCounts, int, successes, 0 ) = board_n;
successes++;
}
}//if(frame++ % board_dt == 0)
image = cvQueryFrame( cvCapture );
//cvShowImage("liveVideo",image);
presssedChar=cvWaitKey(5);
} //while(successes < n_boards)
cvFindExtrinsicCameraParams2(objectPoints,imgPoints,intrinsic,distortion,rotationVector,translationVector,0);
cvRodrigues2(rotationVector,rotationMatrix,NULL);
printf("\nRotation Vector and Rotation Matrx are calculated\n");
float rv[]={rotationVector->data.fl[0],rotationVector->data.fl[1],rotationVector->data.fl[2]};
float tv[]={translationVector->data.fl[0],translationVector->data.fl[1],translationVector->data.fl[2]};
float rm[9];
for(int i=0;i<9;i++)
rm[i]=rotationMatrix->data.fl[i];
for (int f=0; f<3; f++)
{
for (int c=0; c<3; c++)
{
toGLMatrix[c*4+f] = rm[f*3+c]; //transposed
}
}
toGLMatrix[3] = 0.0;
toGLMatrix[7] = 0.0;
toGLMatrix[11] = 0.0;
toGLMatrix[12] = -tv[0];
toGLMatrix[13] = -tv[1];
toGLMatrix[14] = tv[2];
toGLMatrix[15] = 1.0;
}
// openGL dispaly function
void display
{
calculateOpenCVPoints("Image from camera");
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
//argDraw3dCamera( 0, 0 );
glViewport(0, 0, 640,480);
glMatrixMode(GL_PROJECTION);
glLoadMatrixd(toGLMatrix);
glClearDepth( 1.0 );
glClear(GL_DEPTH_BUFFER_BIT);
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glMatrixMode(GL_MODELVIEW);
// Loading the calculated matrix from OpenCV rotation matrix and vector
glLoadMatrixd(toGLMatrix);
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
glLightfv(GL_LIGHT0, GL_POSITION, light_position);
glLightfv(GL_LIGHT0, GL_AMBIENT, ambi);
glLightfv(GL_LIGHT0, GL_DIFFUSE, lightZeroColor);
glMaterialfv(GL_FRONT, GL_SPECULAR, mat_flash);
glMaterialfv(GL_FRONT, GL_SHININESS, mat_flash_shiny);
glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);
glMatrixMode(GL_MODELVIEW);
glColor3d(1.0f,1.0f,1.0f);
glTranslatef( 0.0, 0.0,0.0 );
glutSolidCube(140);
glDisable( GL_LIGHTING );
glDisable( GL_DEPTH_TEST );
glutSwapBuffers();
}
// and remaining as initialize glut etc

Answer 1

glViewport(0, 0, 640,480);
glMatrixMode(GL_PROJECTION);
glLoadMatrixd(toGLMatrix);
glMatrixMode(GL_MODELVIEW);
// Loading the calculated matrix from OpenCV rotation matrix and vector
glLoadMatrixd(toGLMatrix);
You're loading the matrix two times here. That doesn't make sense. It would transform the vertices as
v' = M M v = M² v
Definitely not what you want.
The matrix delivered by OpenCV combines projection and modelview. Ideally you wouldn't use fixed function OpenGL, but use a vertex shader for this. This would make things clearer.
For a simple drop in application, load the matrix from OpenCV just into modelview and make projection a identity transform.
If you want it to be correct, separate the matrix from OpenCV into two parts: A orthonormal modelview part and a matching projection one, with the x and y components of the Z column being zero. Let C be the matrix from OpenCV. For this solve the system of equations:
C = P M
P.zx = 0
P.zy = 0
P.zz =/= 0
P.zw =/= 0
M.x · M.y = 0
M.y · M.z = 0
M.z · M.x = 0
M.xw = 0
M.yw = 0
M.zw = 0
M.ww = 1
| M.x | = 1
| M.y | = 1
| M.z | = 1