Get ScreenCoordinates from 3D object OpenGL and draw sample points - c++

Question

I am working on a Particle Filter that needs to compare coordinates of pixels detected from a VideoCapture with coordinates of sample points on a 3D object. Currently, there are no sample points, but just one (top left corner of the cube you see on the (left) image below). However, contours are detected and drawn (there are not many as it's dark, but please ignore that for now) on the video stream, which means I can easily get their [x,y] coordinates.
The problem here was that I could not (properly) translate 3D coordinates of the object into 2D screen coordinates. I am using perspective projection as I obviously draw 3D objects.
Left - Video Capture / Right - coordinates(first comes the "translated" screen followed by object coordinates relative to origin point (0,0) in the centre, and then normalized errors, which represent the error in distance from the origin point (ignore that for now)):
The problem was that for each particle I was getting projection and model view matrices over and over. I simply moved the code for obtaining matrices before the for loop in order to get them only once. It worked!!!
Here is the initialisation of the screen and vectors that I am using:
int width = 640;
int height = 480;
Mat image;
int iteration = 0;
struct Coordinates{ //stores coordinates of drawn particles
GLdouble x;
GLdouble y;
GLdouble z;
GLdouble angle;
};
struct ScreenCoordinates{ //stores coordinates on screen
GLdouble x;
GLdouble y;
GLdouble z;
};
float X_object, Y_object, Z_object, cube_angle;
GLdouble sum_of_errors;
GLint viewport[4];
GLdouble mvmatrix[16], projmatrix[16];
vector<Coordinates> points(number_of_particles);
vector<GLdouble> errors(number_of_particles);
vector<GLdouble> normalizedErrors(number_of_particles);
vector<GLint> indexes(number_of_particles);
vector<GLdouble> weights(number_of_particles);
vector<vector<Point>> Contours;// = new vector<vector<Point>>(number_of_particles); //2D vector for storing the edges coordinates
vector<Point> screenCoordinates;
vector<ScreenCoordinates> point_on_screen(number_of_particles);
Particles are drawn in the following way:
void drawParticle()
{
//glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Clear color and depth buffers
glPushAttrib(GL_POLYGON_BIT | GL_ENABLE_BIT | GL_COLOR_BUFFER_BIT) ;
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE) ;
//glDisable(GL_LIGHTING) ;
glBegin(GL_QUADS); // Begin drawing the color cube with size 3cm x 3.5cm x 4cm
// Top face (y = 1.0f)
// Define vertices in counter-clockwise (CCW) order with normal pointing out
glColor3f(0.0f, 1.0f, 0.0f); // Green
glVertex3f( 1.75f, 1.75f, -4.0f);
glVertex3f(-1.75f, 1.75f, -4.0f);
glVertex3f(-1.75f, 1.75f, 1.0f);
glVertex3f( 1.75f, 1.75f, 1.0f);
// Bottom face (y = -1.0f)
glColor3f(1.0f, 0.5f, 0.0f); // Orange
glVertex3f( 1.75f, -1.75f, 1.0f);
glVertex3f(-1.75f, -1.75f, 1.0f);
glVertex3f(-1.75f, -1.75f, -4.0f);
glVertex3f( 1.75f, -1.75f, -4.0f);
// Front face (z = 1.0f)
glColor3f(1.0f, 0.0f, 0.0f); // Red
glVertex3f( 1.75f, 1.75f, 1.0f);
glVertex3f(-1.75f, 1.75f, 1.0f);
glVertex3f(-1.75f, -1.75f, 1.0f);
glVertex3f( 1.75f, -1.75f, 1.0f);
// Back face (z = -1.0f)
glColor3f(1.0f, 1.0f, 0.0f); // Yellow
glVertex3f( 1.75f, -1.75f, -4.0f);
glVertex3f(-1.75f, -1.75f, -4.0f);
glVertex3f(-1.75f, 1.75f, -4.0f);
glVertex3f( 1.75f, 1.75f, -4.0f);
// Left face (x = -1.0f)
glColor3f(0.0f, 0.0f, 1.0f); // Blue
glVertex3f(-1.75f, 1.75f, 1.0f);
glVertex3f(-1.75f, 1.75f, -4.0f);
glVertex3f(-1.75f, -1.75f, -4.0f);
glVertex3f(-1.75f, -1.75f, 1.0f);
// Right face (x = 1.0f)
glColor3f(1.0f, 0.0f, 1.0f); // Magenta
glVertex3f(1.75f, 1.75f, -4.0f);
glVertex3f(1.75f, 1.75f, 1.0f);
glVertex3f(1.75f, -1.75f, 1.0f);
glVertex3f(1.75f, -1.75f, -4.0f);
glEnd(); // End of drawing color-cube
glPopAttrib() ;
}
Display function:
void display()
{
// 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();
float 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);
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 = 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(X_object, Y_object, Z_object, mvmatrix, projmatrix, viewport, &point_on_screen[i].x, &point_on_screen[i].y, &point_on_screen[i].z);
printf("Screen: %f, %f\n", point_on_screen[i].x, point_on_screen[i].y);
//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
glRotatef(cube_angle, 1.0f, 1.0f, 1.0f); // Rotate about (1,1,1)-axis [NEW]
drawParticle();
//glPopMatrix();
}
scoreParticles();
// show the rendering on the screen
glutSwapBuffers();
// post the next redisplay
glutPostRedisplay();
}
This is the exact way of translation:
glGetIntegerv (GL_VIEWPORT, viewport);
glGetDoublev (GL_MODELVIEW_MATRIX, mvmatrix);
glGetDoublev (GL_PROJECTION_MATRIX, projmatrix);
gluProject(X_object, Y_object, Z_object, mvmatrix, projmatrix, viewport, &point_on_screen[i].x, &point_on_screen[i].y, &point_on_screen[i].z);
printf("Screen: %f, %f\n", point_on_screen[i].x, point_on_screen[i].y);
The question now is how can I include sample points between vertices? Taking into account the way I draw particles, how should I divide the space between all vertices into equal parts and place a point there so that this point can be translated into screen coordinates and then compared to pixel coordinates?
Just for clarification: the error in distance for each particle is found and a certain weight is assigned, after that I use Stochastic Sampling for redistribution of particles over following population.