[HELP] Correct Pose Estimation Opencv + Opengl

Hi everybody,

i’m developing an AR projection, but i run out into a problem. I can’t make my object stay exactly on the marker, it seems to travel all around the image…

I’ve used ARuco and OpenCV for detection of rvec and tvec and OpenGL for rendering. I know that a change of prospective matrix is needeed due to opengl different coordinate system but i can’t find the right one and how to apply it.

Can you help me?

Here’s my code

############################### # GLOBAL VARIABLES width = 1280 height = 720 texture_background = None chess_piece = None webcam = cv2.VideoCapture(0) camera_matrix, dist_coefs, rvecs, tvecs = calibrate() znear = 10 zfar = 10000.0 ############################### ############################### INIT OPENGL def init_gl(): global texture_background, chess_piece glClearColor(0.0, 0.0, 0.0, 0.0) glClearDepth(1.0) # glDepthFunc(GL_LESS) glDepthFunc(GL_LEQUAL) glEnable(GL_DEPTH_TEST) glShadeModel(GL_SMOOTH) glMatrixMode(GL_PROJECTION) glLoadIdentity() gluPerspective(40.34, 1.78, znear, zfar) #Cambiare all'occorrenza glViewport(0,0,width,height) ppm = getOpenglProjectionMatrix(camera_matrix, znear, zfar, width, height) # # glMatrixMode(GL_PROJECTION) # # glPushMatrix() glLoadMatrixf(ppm) glMatrixMode(GL_MODELVIEW) # assign shapes chess_piece = OBJ(obj_path, True) # assign texture glEnable(GL_TEXTURE_2D) texture_background = glGenTextures(1) ############################### WEBCAM HANDLER def getNextFrame(): global webcam ret, frame = webcam.read() if not ret or frame is None: webcam.release() print("Released webcam") return None return frame ############################### WINDOWS FUNCTION def draw_background(image): global texture_background bg_image = cv2.flip(image, 0) bg_image = Image.fromarray(bg_image) ix = bg_image.size[0] iy = bg_image.size[1] bg_image = bg_image.tobytes("raw", "BGRX", 0, -1) # create background texture glBindTexture(GL_TEXTURE_2D, texture_background) glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST) glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST) glTexImage2D(GL_TEXTURE_2D, 0, 3, ix, iy, 0, GL_RGBA, GL_UNSIGNED_BYTE, bg_image) # draw background glBindTexture(GL_TEXTURE_2D, texture_background) glPushMatrix() glTranslatef(0.0,0.0,-10.0) glBegin(GL_QUADS) glTexCoord2f(0.0, 1.0); glVertex3f(-4.0, -3.0, 3.0) glTexCoord2f(1.0, 1.0); glVertex3f( 4.0, -3.0, 3.0) glTexCoord2f(1.0, 0.0); glVertex3f( 4.0, 3.0, 3.0) glTexCoord2f(0.0, 0.0); glVertex3f(-4.0, 3.0, 3.0) glEnd( ) glPopMatrix() def draw_obj(rvec, tvec, center, p_k): ### OBTAINING PROJECTION MATRIX view_matrix = getCorrectPPM(rvec,tvec) # view_matrix = tryThis(p_k) ############################### lightfv = ctypes.c_float * 4 glDisable(GL_TEXTURE_2D) glEnable(GL_DEPTH_TEST) glLightfv(GL_LIGHT0, GL_POSITION, lightfv(-1.0, 1.0, 1.0, 0.0)) glEnable(GL_LIGHT0) glMatrixMode(GL_MODELVIEW) glPushMatrix() glLoadMatrixd(view_matrix) glTranslatef(0.0, 0.0, -100.0) # glTranslatef(-center[0], center[1], 0.0) # glRotatef(-90, 1.0, 0.0, 0.0) glEnable(GL_LIGHTING) glCallList(chess_piece.gl_list) glColor3f(1.0, 1.0, 1.0) glPopMatrix() glEnable(GL_TEXTURE_2D) glDisable(GL_DEPTH_TEST) glDisable(GL_LIGHTING) def display(): global chess_piece, camera_matrix, dist_coefs glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) glLoadIdentity() image = getNextFrame() rvec, tvec, frame, center, p_k = detect(image, camera_matrix, dist_coefs, chess_piece) draw_background(frame) if len(rvec) != 0 and len(tvec) != 0: draw_obj(rvec, tvec, center, p_k) glutSwapBuffers() def reshape(): if height == 0: height = 1 glViewport(0, 0, width, height) glMatrixMode(GL_PROJECTION) glLoadIdentity() if width <= height: glOrtho(-nRange, nRange, -nRange*h/w, nRange*h/w, -nRange, nRange) else: glOrtho(-nRange*w/h, nRange*w/h, -nRange, nRange, -nRange, nRange) glMatrixMode(GL_MODELVIEW) glLoadIdentity() ############################### APPLICATION def init_application(): print("Initializing app") glutInit() glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH) glutInitWindowSize(width, height) glutInitWindowPosition(0, 0) glutCreateWindow("AR Chess") glutDisplayFunc(display) glutIdleFunc(display) # glutReshapeFunc(reshape) init_gl() print("Inizialization completed") glutMainLoop() init_application() 

The function to get the „corrected ppm“ is:

def getOpenglProjectionMatrix(camera_matrix, near, far, width, height): K00 = camera_matrix[0][0] K11 = camera_matrix[1][1] K01 = camera_matrix[0][1] K02 = camera_matrix[0][2] K12 = camera_matrix[1][2] height = 720 width = 1280 fx = K00 fy = K11 fovy = 2*np.arctan(0.5*height/fy)*180/np.pi aspect = (width*fy)/(height*fx) near = 0.1 far = 100.0 print("FOVY:",fovy) print("ASPECT:",aspect) result00 = 2*K00 / width # result[0,1] = -2*K01 / width result01 = 0 result02 = (width - 2*K02 + 2*0) / width result03 = 0 result10 = 0 result11 = -2*K11 / height result12 = (height - 2*K12 + 2*0) / height result13 = 0 result20 = 0 result21 = 0 result22 = (-far - near) / (far - near) result23 = -2*far*near/(far-near) result30 = 0 result31 = 0 result32 = -1 result33 = 0 result = np.array([[result00,result01,result02,result03], [result10,result11,result12,result13], [result20,result21,result22,result23], [result30,result31,result32,result33]]) print(result) print(type(result)) m = map(float,result.T.flat) m = (GLfloat * 16)(*m) return m 

submitted by /u/Dokuni
[link] [comments] Augmented Reality & /u/Dokuni

Hi everybody,

i'm developing an AR projection, but i run out into a problem. I can't make my object stay exactly on the marker, it seems to travel all around the image...

I've used ARuco and OpenCV for detection of rvec and tvec and OpenGL for rendering. I know that a change of prospective matrix is needeed due to opengl different coordinate system but i can't find the right one and how to apply it.

Can you help me?

Here's my code

############################### # GLOBAL VARIABLES width = 1280 height = 720 texture_background = None chess_piece = None webcam = cv2.VideoCapture(0) camera_matrix, dist_coefs, rvecs, tvecs = calibrate() znear = 10 zfar = 10000.0 ############################### ############################### INIT OPENGL def init_gl(): global texture_background, chess_piece glClearColor(0.0, 0.0, 0.0, 0.0) glClearDepth(1.0) # glDepthFunc(GL_LESS) glDepthFunc(GL_LEQUAL) glEnable(GL_DEPTH_TEST) glShadeModel(GL_SMOOTH) glMatrixMode(GL_PROJECTION) glLoadIdentity() gluPerspective(40.34, 1.78, znear, zfar) #Cambiare all'occorrenza glViewport(0,0,width,height) ppm = getOpenglProjectionMatrix(camera_matrix, znear, zfar, width, height) # # glMatrixMode(GL_PROJECTION) # # glPushMatrix() glLoadMatrixf(ppm) glMatrixMode(GL_MODELVIEW) # assign shapes chess_piece = OBJ(obj_path, True) # assign texture glEnable(GL_TEXTURE_2D) texture_background = glGenTextures(1) ############################### WEBCAM HANDLER def getNextFrame(): global webcam ret, frame = webcam.read() if not ret or frame is None: webcam.release() print("Released webcam") return None return frame ############################### WINDOWS FUNCTION def draw_background(image): global texture_background bg_image = cv2.flip(image, 0) bg_image = Image.fromarray(bg_image) ix = bg_image.size[0] iy = bg_image.size[1] bg_image = bg_image.tobytes("raw", "BGRX", 0, -1) # create background texture glBindTexture(GL_TEXTURE_2D, texture_background) glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST) glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST) glTexImage2D(GL_TEXTURE_2D, 0, 3, ix, iy, 0, GL_RGBA, GL_UNSIGNED_BYTE, bg_image) # draw background glBindTexture(GL_TEXTURE_2D, texture_background) glPushMatrix() glTranslatef(0.0,0.0,-10.0) glBegin(GL_QUADS) glTexCoord2f(0.0, 1.0); glVertex3f(-4.0, -3.0, 3.0) glTexCoord2f(1.0, 1.0); glVertex3f( 4.0, -3.0, 3.0) glTexCoord2f(1.0, 0.0); glVertex3f( 4.0, 3.0, 3.0) glTexCoord2f(0.0, 0.0); glVertex3f(-4.0, 3.0, 3.0) glEnd( ) glPopMatrix() def draw_obj(rvec, tvec, center, p_k): ### OBTAINING PROJECTION MATRIX view_matrix = getCorrectPPM(rvec,tvec) # view_matrix = tryThis(p_k) ############################### lightfv = ctypes.c_float * 4 glDisable(GL_TEXTURE_2D) glEnable(GL_DEPTH_TEST) glLightfv(GL_LIGHT0, GL_POSITION, lightfv(-1.0, 1.0, 1.0, 0.0)) glEnable(GL_LIGHT0) glMatrixMode(GL_MODELVIEW) glPushMatrix() glLoadMatrixd(view_matrix) glTranslatef(0.0, 0.0, -100.0) # glTranslatef(-center[0], center[1], 0.0) # glRotatef(-90, 1.0, 0.0, 0.0) glEnable(GL_LIGHTING) glCallList(chess_piece.gl_list) glColor3f(1.0, 1.0, 1.0) glPopMatrix() glEnable(GL_TEXTURE_2D) glDisable(GL_DEPTH_TEST) glDisable(GL_LIGHTING) def display(): global chess_piece, camera_matrix, dist_coefs glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) glLoadIdentity() image = getNextFrame() rvec, tvec, frame, center, p_k = detect(image, camera_matrix, dist_coefs, chess_piece) draw_background(frame) if len(rvec) != 0 and len(tvec) != 0: draw_obj(rvec, tvec, center, p_k) glutSwapBuffers() def reshape(): if height == 0: height = 1 glViewport(0, 0, width, height) glMatrixMode(GL_PROJECTION) glLoadIdentity() if width <= height: glOrtho(-nRange, nRange, -nRange*h/w, nRange*h/w, -nRange, nRange) else: glOrtho(-nRange*w/h, nRange*w/h, -nRange, nRange, -nRange, nRange) glMatrixMode(GL_MODELVIEW) glLoadIdentity() ############################### APPLICATION def init_application(): print("Initializing app") glutInit() glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH) glutInitWindowSize(width, height) glutInitWindowPosition(0, 0) glutCreateWindow("AR Chess") glutDisplayFunc(display) glutIdleFunc(display) # glutReshapeFunc(reshape) init_gl() print("Inizialization completed") glutMainLoop() init_application() 

The function to get the "corrected ppm" is:

def getOpenglProjectionMatrix(camera_matrix, near, far, width, height): K00 = camera_matrix[0][0] K11 = camera_matrix[1][1] K01 = camera_matrix[0][1] K02 = camera_matrix[0][2] K12 = camera_matrix[1][2] height = 720 width = 1280 fx = K00 fy = K11 fovy = 2*np.arctan(0.5*height/fy)*180/np.pi aspect = (width*fy)/(height*fx) near = 0.1 far = 100.0 print("FOVY:",fovy) print("ASPECT:",aspect) result00 = 2*K00 / width # result[0,1] = -2*K01 / width result01 = 0 result02 = (width - 2*K02 + 2*0) / width result03 = 0 result10 = 0 result11 = -2*K11 / height result12 = (height - 2*K12 + 2*0) / height result13 = 0 result20 = 0 result21 = 0 result22 = (-far - near) / (far - near) result23 = -2*far*near/(far-near) result30 = 0 result31 = 0 result32 = -1 result33 = 0 result = np.array([[result00,result01,result02,result03], [result10,result11,result12,result13], [result20,result21,result22,result23], [result30,result31,result32,result33]]) print(result) print(type(result)) m = map(float,result.T.flat) m = (GLfloat * 16)(*m) return m 
submitted by /u/Dokuni
[link] [comments]