Hello,
This is my first post here as I am a beginner with OpenGL.
So basically I am implementing a Particle Filter for 3D Tracking. As a base I use OpenCV and OpenGL on Windows 7 (64-bit).
Currently I am able to capture video from the web cam of my laptop, stream it as a texture and render a cube on top of that. From OpenCV I detect contours of objects and get their coordinates (on screen). I am also able to get the screen coordinates of the front top left corner of the cube. However, in order to make the filter more accurate I used subdivision of surfaces - so for each side/plane of the cube I get several points (I call them sample points) to compare with. The code is provided below.
My question is how to transform those sample points from 3D object coordinates to 2D screen coordinates? For a starter I found the min (X, Y, Z) and max (X, Y, Z), which define all 8 corners of the cube and I am trying to translate only those at the moment.
So, here is how I draw the cube, subdivide spaces, and collect the min/max X, Y, Z.
void DrawBox(GLfloat fWidth,GLfloat fHeight,GLfloat fDepth,GLint wslices,GLint dslices,GLint stacks,GLfloat scanSize, int partNum)
{
glPushAttrib(GL_POLYGON_BIT | GL_ENABLE_BIT | GL_COLOR_BUFFER_BIT) ;
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE) ;
glEnable(GL_CULL_FACE); // allow only front surfaces to be visible
int iTopButtonQuads = wslices * dslices * 2;
int iLeftRightQuads = dslices * stacks * 2;
int iFrontBackQuads = wslices * stacks * 2;
float* pfVertices = new float[(iTopButtonQuads + iLeftRightQuads + iFrontBackQuads) * 3 * 4];
float* pfColors = new float[(iTopButtonQuads + iLeftRightQuads + iFrontBackQuads) * 3 * 4];
float* pfNormals = new float[(iTopButtonQuads + iLeftRightQuads + iFrontBackQuads) * 3 * 4];
int iVertexIndex = 0;
GLfloat Xstep = fWidth / wslices;
GLfloat Ystep = fHeight / stacks;
GLfloat Zstep = fDepth / dslices;
GLfloat firstX = fWidth / 2.0f;
GLfloat firstY = fHeight / 2.0f;
GLfloat firstZ = fDepth / 2.0f;
GLfloat currX = 0.0f;
GLfloat currY = 0.0f;
GLfloat currZ = 0.0f;
GLfloat x_status = 0.0f;
GLfloat y_status = 0.0f;
GLfloat z_status = 0.0f;
int rowNum, innerCube, currPoint = 0;
// the bottom and the top of the box
for (currZ = -firstZ, z_status = 0.0f, rowNum = 0; currZ < firstZ - Zstep / 2.0f; currZ += Zstep, z_status += Zstep, rowNum++)
{
for (currX = -firstX, x_status = 0.0f, innerCube = 0; currX < firstX - Xstep / 2.0f; currX += Xstep, x_status += Xstep, innerCube++)
{
int iCurrentIndex = iVertexIndex * 3 * 4;
float pfNormal[3] = { 0.0f, -1.0f, 0.0f };
memcpy(pfNormals + iCurrentIndex, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 3, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 6, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 9, pfNormal, 3 * 4);
float pfColor[3] = { 1.0f, 0.0f, 0.0f };
memcpy(pfColors + iCurrentIndex, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 3, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 6, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 9, pfColor, 3 * 4);
float pfVertex0[3] = {currX,-firstY,currZ};
float pfVertex1[3] = {currX + Xstep,-firstY,currZ};
float pfVertex2[3] = {currX + Xstep,-firstY,currZ + Zstep};
float pfVertex3[3] = {currX,-firstY,currZ + Zstep};
memcpy(pfVertices + iCurrentIndex, pfVertex0, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 3, pfVertex1, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 6, pfVertex2, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 9, pfVertex3, 3 * 4);
iVertexIndex++;
}
for (currX = -firstX, x_status = 0.0f, innerCube = 0; currX < firstX - Xstep / 2.0f; currX += Xstep, x_status += Xstep, innerCube++)
{
int iCurrentIndex = iVertexIndex * 3 * 4;
float pfNormal[3] = { 0.0f, 1.0f, 0.0f };
memcpy(pfNormals + iCurrentIndex, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 3, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 6, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 9, pfNormal, 3 * 4);
float pfColor[3] = { 0.0f, 1.0f, 0.0f };
memcpy(pfColors + iCurrentIndex, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 3, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 6, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 9, pfColor, 3 * 4);
float pfVertex0[3] = {currX + Xstep,firstY,currZ + Zstep};
float pfVertex1[3] = {currX + Xstep,firstY,currZ};
float pfVertex2[3] = {currX,firstY,currZ};
float pfVertex3[3] = {currX,firstY,currZ + Zstep};
memcpy(pfVertices + iCurrentIndex, pfVertex0, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 3, pfVertex1, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 6, pfVertex2, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 9, pfVertex3, 3 * 4);
iVertexIndex++;
}
}
// the front and the back of the box
for (currY = -firstY, y_status = 0.0f, rowNum = 0; currY < firstY - Ystep / 2.0f ; currY += Ystep, y_status += Ystep, rowNum++)
{
for (currX = -firstX, x_status = 0.0f, innerCube = 0; currX < firstX - Xstep / 2.0f; currX += Xstep, x_status += Xstep, innerCube++)
{
int iCurrentIndex = iVertexIndex * 3 * 4;
float pfNormal[3] = { 0.0f, 0.0f, 1.0f };
memcpy(pfNormals + iCurrentIndex, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 3, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 6, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 9, pfNormal, 3 * 4);
float pfColor[3] = { 0.0f, 0.0f, 1.0f };
memcpy(pfColors + iCurrentIndex, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 3, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 6, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 9, pfColor, 3 * 4);
float pfVertex0[3] = {currX,currY,firstZ};
float pfVertex1[3] = {currX + Xstep,currY,firstZ};
float pfVertex2[3] = {currX + Xstep,currY + Ystep,firstZ};
float pfVertex3[3] = {currX,currY + Ystep,firstZ};
memcpy(pfVertices + iCurrentIndex, pfVertex0, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 3, pfVertex1, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 6, pfVertex2, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 9, pfVertex3, 3 * 4);
iVertexIndex++;
}
for (currX = -firstX, x_status = 0.0f, innerCube = 0; currX < firstX - Xstep / 2.0f; currX += Xstep, x_status += Xstep, innerCube++)
{
int iCurrentIndex = iVertexIndex * 3 * 4;
float pfNormal[3] = { 0.0f, 0.0f, -1.0f };
memcpy(pfNormals + iCurrentIndex, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 3, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 6, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 9, pfNormal, 3 * 4);
float pfColor[3] = { 0.0f, 1.0f, 1.0f };
memcpy(pfColors + iCurrentIndex, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 3, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 6, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 9, pfColor, 3 * 4);
float pfVertex0[3] = {currX + Xstep,currY + Ystep,-firstZ};
float pfVertex1[3] = {currX + Xstep,currY,-firstZ};
float pfVertex2[3] = {currX,currY,-firstZ};
float pfVertex3[3] = {currX,currY + Ystep,-firstZ};
memcpy(pfVertices + iCurrentIndex, pfVertex0, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 3, pfVertex1, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 6, pfVertex2, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 9, pfVertex3, 3 * 4);
iVertexIndex++;
}
}
// Right side and the left side of the box
for (currY = -firstY, y_status = 0.0f, rowNum = 0; currY < firstY - Ystep / 2.0f; currY += Ystep, y_status += Ystep, rowNum++)
{
for (currZ = -firstZ, z_status = 0.0f, innerCube = 0; currZ < firstZ - Zstep / 2.0f; currZ += Zstep, z_status += Zstep, innerCube++)
{
int iCurrentIndex = iVertexIndex * 3 * 4;
float pfNormal[3] = { 1.0f, 0.0f, 0.0f };
memcpy(pfNormals + iCurrentIndex, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 3, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 6, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 9, pfNormal, 3 * 4);
float pfColor[3] = { 1.0f, 0.0f, 1.0f };
memcpy(pfColors + iCurrentIndex, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 3, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 6, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 9, pfColor, 3 * 4);
float pfVertex0[3] = {firstX,currY,currZ};
float pfVertex1[3] = {firstX,currY + Ystep,currZ};
float pfVertex2[3] = {firstX,currY + Ystep,currZ + Zstep};
float pfVertex3[3] = {firstX,currY,currZ + Zstep};
memcpy(pfVertices + iCurrentIndex, pfVertex0, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 3, pfVertex1, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 6, pfVertex2, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 9, pfVertex3, 3 * 4);
iVertexIndex++;
}
for (currZ = -firstZ, z_status = 0.0f, innerCube = 0; currZ < firstZ - Zstep / 2.0f; currZ += Zstep, z_status += Zstep, innerCube++)
{
int iCurrentIndex = iVertexIndex * 3 * 4;
float pfNormal[3] = { -1.0f, 0.0f, 0.0f };
memcpy(pfNormals + iCurrentIndex, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 3, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 6, pfNormal, 3 * 4);
memcpy(pfNormals + iCurrentIndex + 9, pfNormal, 3 * 4);
float pfColor[3] = { 1.0f, 1.0f, 0.0f };
memcpy(pfColors + iCurrentIndex, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 3, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 6, pfColor, 3 * 4);
memcpy(pfColors + iCurrentIndex + 9, pfColor, 3 * 4);
float pfVertex0[3] = {-firstX,currY,currZ};
float pfVertex1[3] = {-firstX,currY,currZ + Zstep};
float pfVertex2[3] = {-firstX,currY + Ystep,currZ + Zstep};
float pfVertex3[3] = {-firstX,currY + Ystep,currZ};
memcpy(pfVertices + iCurrentIndex, pfVertex0, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 3, pfVertex1, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 6, pfVertex2, 3 * 4);
memcpy(pfVertices + iCurrentIndex + 9, pfVertex3, 3 * 4);
iVertexIndex++;
}
}
glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_COLOR_ARRAY);
glEnableClientState(GL_NORMAL_ARRAY);
glColorPointer(3, GL_FLOAT, 0, (void*)pfColors);
glNormalPointer(GL_FLOAT, 0, (void*)pfNormals);
glVertexPointer(3, GL_FLOAT, 0, (void*)pfVertices);
glDrawArrays(GL_QUADS, 0, (iTopButtonQuads + iLeftRightQuads + iFrontBackQuads) * 4);
glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
glDisableClientState(GL_NORMAL_ARRAY);
// Get coord of first vertex
float fMinX = pfVertices[0];
float fMinY = pfVertices[1];
float fMinZ = pfVertices[2];
float fMaxX = pfVertices[0];
float fMaxY = pfVertices[1];
float fMaxZ = pfVertices[2];
for (int iVertexIndex = 0; iVertexIndex < (iTopButtonQuads + iLeftRightQuads + iFrontBackQuads) * 4; iVertexIndex++)
{
int iCurrentIndex = iVertexIndex * 3; // (x y z) per vertex
if (pfVertices[iCurrentIndex] < fMinX)
fMinX = pfVertices[iCurrentIndex];
if (pfVertices[iCurrentIndex + 1] < fMinY)
fMinY = pfVertices[iCurrentIndex + 1];
if (pfVertices[iCurrentIndex + 2] < fMinZ)
fMinZ = pfVertices[iCurrentIndex + 2];
if (pfVertices[iCurrentIndex] > fMaxX)
fMaxX = pfVertices[iCurrentIndex];
if (pfVertices[iCurrentIndex + 1] > fMaxY)
fMaxY = pfVertices[iCurrentIndex + 1];
if (pfVertices[iCurrentIndex + 2] > fMaxZ)
fMaxZ = pfVertices[iCurrentIndex + 2];
}
// Create an axes aligned bounding box
// by simply drawing inflated min-maxes, that we collect
// example of using indexed primitives
glDisable(GL_CULL_FACE);
GLfloat vertices[] = {
fMinX - scanSize, fMaxY + scanSize, fMaxZ + scanSize,
fMaxX + scanSize, fMaxY + scanSize, fMaxZ + scanSize,
fMaxX + scanSize, fMinY - scanSize, fMaxZ + scanSize,
fMinX - scanSize, fMinY - scanSize, fMaxZ + scanSize,
fMinX - scanSize, fMaxY + scanSize, fMinZ - scanSize,
fMaxX + scanSize, fMaxY + scanSize, fMinZ - scanSize,
fMaxX + scanSize, fMinY - scanSize, fMinZ - scanSize,
fMinX - scanSize, fMinY - scanSize, fMinZ - scanSize
};
vector<GLdouble> convertionV(vertices, vertices + sizeof vertices / sizeof vertices[0]);
vec = convertionV;
GLint indices[] = {
0, 1, 2, 3,
4, 5, 1, 0,
3, 2, 6, 7,
5, 4, 7, 6,
1, 5, 6, 2,
4, 0, 3, 7
};
glColor3f(1.0f, 1.0f, 1.0f);
glEnableClientState(GL_VERTEX_ARRAY);
glVertexPointer(3, GL_FLOAT, 0, (void*)vertices);
glDrawElements(GL_POINTS, 24, GL_UNSIGNED_INT, (void*)indices);
glDisableClientState(GL_VERTEX_ARRAY);
glEnable(GL_CULL_FACE);
delete [] pfVertices;
delete [] pfNormals;
delete [] pfColors;
glPopAttrib() ;
}
Then I copy the array with points to a vector, so that this vector is used in display:
void display()
{
if(iteration != 0 || conversion == false){
// clear the window
glClear( GL_COLOR_BUFFER_BIT );
// show the current camera frame
//based on the way cv::Mat stores data, you need to flip it before displaying it
cv::Mat tempimage;
cv::flip(image, tempimage, 0);
glDrawPixels( tempimage.size().width, tempimage.size().height, GL_RGB, GL_UNSIGNED_BYTE, tempimage.ptr() );
//////////////////////////////////////////////////////////////////////////////////
// Here, set up new parameters to render a scene viewed from the camera.
//set viewport
glViewport(0, 0, tempimage.size().width, tempimage.size().height);
//set projection matrix using intrinsic camera params
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
GLfloat aspect = tempimage.size().width*1.0/tempimage.size().height;
//gluPerspective is arbitrarily set, you will have to determine these values based
//on the intrinsic camera parameters
gluPerspective(60.0f, aspect, 0.1f, 100.0f);
//you will have to set modelview matrix using extrinsic camera params
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(0, 0, 5, 0, 0, 0, 0, 1, 0);
// get the viewport, modelView matrix and projection matrix
glGetIntegerv (GL_VIEWPORT, viewport);
glGetDoublev (GL_MODELVIEW_MATRIX, mvmatrix);
glGetDoublev (GL_PROJECTION_MATRIX, projmatrix);
/////////////////////////////////////////////////////////////////////////////////
// Drawing routine
//initialise certain number of particles
for(int i = 0; i<number_of_particles; i++){
// uniformly distributed for the very first generation
if((points[i].x == NULL) || (points[i].y = NULL)){
//randomly generate X,Y,Z and angle values
X_object = RandomNumber(-5.0, 5.0);//rand() % 10 - 2.5;
Y_object = RandomNumber(-5.0, 5.0);//rand() % 10 - 2.5;
Z_object = 23;
cube_angle = 0;//rand() % 360 + 1;
}else{
//printf("second generation should improve the distribution");
//set sigma accordingly
//use best particle coordinates for the next X and Y
X_object = points[i].x;
Y_object = points[i].y;
Z_object = points[i].z;
cube_angle = points[i].angle;
}
points[i].x = X_object;
points[i].y = Y_object;
points[i].z = Z_object;
points[i].angle = cube_angle;
gluProject(points[i].x, points[i].y, points[i].z, mvmatrix, projmatrix, viewport,
&point_on_screen[i].x, &point_on_screen[i].y, &point_on_screen[i].z);
for(int opi = 0; opi < vec.size()/3; opi++){
int currEl = opi * 3;
gluProject(vec[currEl], vec[currEl + 1], vec[currEl + 2], mvmatrix, projmatrix, viewport,
&testpoint_on_screen[currEl], &testpoint_on_screen[currEl + 1], &testpoint_on_screen[currEl + 2]);
}
//now that the camera params have been set, draw your 3D shapes
//first, save the current matrix
//glPushMatrix();
//move to the position where you want the 3D object to go
glLoadIdentity();
glTranslatef(X_object, Y_object, -Z_object); //this is an arbitrary position for demonstration
//you will need to adjust your transformations to match the positions where
//you want to draw your objects(i.e. chessboard center, chessboard corners)
glRotatef(cube_angle, 1.0f, 1.0f, 1.0f); // Rotate about (1,1,1)-axis
DrawBox(4.0f, 3.5f, 5.0f, 5, 5, 5, 1.0, i);
//glPopMatrix();
}
// show the rendering on the screen
glutSwapBuffers();
// post the next redisplay
glutPostRedisplay();
}
iteration++;
}
Here is an image of the output I’m getting:
[ATTACH=CONFIG]1310[/ATTACH]
Basically, the “Screen:” values are all the same in each iteration, which suggests that these points are never changing, which obviously is not true. :dejection:
Either I am missing some fundamental understanding of how objects are rendered on screen or it’s a silly mistake. If someone can explain it to me or provide a solution that would be highly appreciated.