Camera calibration and pose estimation (OpenGL + OpenCV)

edit

Hello i'm fairly new to OpenCV. I'm trying to estimate the 3D pose of the camera in order to draw a 3D teapot using OpenGL. I have been testing for about a week and I partially understand the theory, I tried to replicate an example but I do not get it to appear correctly. I get the keypoints using SIFT and looks right.

I use this function in order to obtain the intrinsics and extrinsics parameters:

ret, K, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, im_size,None,None)

When I have these parameters I create the loop to draw the teapot:

def setup():
    pygame.init()
    pygame.display.set_mode((im_size[0], im_size[1]), pygame.OPENGL | pygame.DOUBLEBUF)
    pygame.display.set_caption('OpenGL AR demo')

setup()
S = 1 # Selected Image
while True:
    event = pygame.event.poll()
    if event.type in (pygame.QUIT, pygame.KEYDOWN):
        break

    draw_background(I[S - 1])
    set_projection_from_camera(K)
    set_modelview_from_camera(rvecs[S-1], tvecs[S-1])
    draw_teapot(100)

    pygame.display.flip()
    pygame.time.wait(50)

I estimate the projection using this function:

def set_projection_from_camera(K):
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    fx = K[0,0]
    fy = K[1,1]
    fovy = 2*arctan(0.5*im_size[1]/fy)*180/pi
    aspect = (im_size[0]*fy)/(im_size[1]*fx)
    # define the near and far clipping planes
    near = 0.1
    far = 500000.0
    # set perspective
    gluPerspective(fovy, aspect, near, far)
    glViewport(0, 0, im_size[0], im_size[1])

I estimate the modelview using this function:

def set_modelview_from_camera(rvec, tvec):
    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()

    Rx = array([[1, 0, 0], [0, 0, -1], [0, 1, 0]]) # rotate the teapot
    M = eye(4)
    M[:3, :3] = dot(Rx, cv2.Rodrigues(rvec)[0])
    M[:3, 3] = tvec.T
    cv2GlMat = array([[1,0,0,0],[0,-1,0,0],[0,0,-1,0],[0,0,0,1]]) #OpenCV -> OpenGL matrix
    M = dot(cv2GlMat, M)
    m = M.T.flatten()
    glLoadMatrixf(m)

The function to draw the background (the real image)

def draw_background(I):
    bg_image = Image.fromarray(I)
    bg_data = bg_image.tobytes('raw', 'RGBX', 0, -1)

    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
    # bind the texture
    glEnable(GL_TEXTURE_2D)
    glBindTexture(GL_TEXTURE_2D, glGenTextures(1))
    glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,bg_image.size[0],bg_image.size[1],0,GL_RGBA,GL_UNSIGNED_BYTE,bg_data)
    glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_NEAREST)
    glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_NEAREST)
    # create quad to fill the whole window
    glBegin(GL_QUADS)
    glTexCoord2f(0.0,0.0); glVertex3f(-1,-1,-1.0)
    glTexCoord2f(1.0,0.0); glVertex3f( 1,-1,-1.0)
    glTexCoord2f(1.0,1.0); glVertex3f( 1, 1,-1.0)
    glTexCoord2f(0.0,1.0); glVertex3f(-1, 1,-1.0)
    glEnd()
    # clear the texture
    glDeleteTextures(1)

I understand the concepts of Opencv Calibration, but I cannot put together into OpenGL and show the object correctly on the screen.