feat: calibration in opengl
This commit is contained in:
parent
fd5c9cf1cf
commit
1884e5f342
GPG Key ID:
569349933C77A854
108
objloader.py
Normal file
108
objloader.py
Normal file
@ -0,0 +1,108 @@
|
||||
import pygame
|
||||
from OpenGL.GL import *
|
||||
|
||||
def MTL(filename):
|
||||
dir_name = '/'.join(filename.split('/')[:-1])
|
||||
contents = {}
|
||||
mtl = None
|
||||
for line in open(filename, "r"):
|
||||
if line.startswith('#'): continue
|
||||
values = line.split()
|
||||
if not values: continue
|
||||
if values[0] == 'newmtl':
|
||||
mtl = contents[values[1]] = {}
|
||||
elif mtl is None:
|
||||
raise ValueError("mtl file doesn't start with newmtl stmt")
|
||||
elif values[0] == 'map_Kd':
|
||||
# load the texture referred to by this declaration
|
||||
mtl[values[0]] = values[1]
|
||||
surf = pygame.image.load(f"{dir_name}/{mtl['map_Kd']}")
|
||||
image = pygame.image.tostring(surf, 'RGBA', 1)
|
||||
ix, iy = surf.get_rect().size
|
||||
texid = mtl['texture_Kd'] = glGenTextures(1)
|
||||
glBindTexture(GL_TEXTURE_2D, texid)
|
||||
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,
|
||||
GL_LINEAR)
|
||||
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER,
|
||||
GL_LINEAR)
|
||||
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, ix, iy, 0, GL_RGBA,
|
||||
GL_UNSIGNED_BYTE, image)
|
||||
else:
|
||||
mtl[values[0]] = map(float, values[1:])
|
||||
return contents
|
||||
|
||||
class OBJ:
|
||||
def __init__(self, filename, swapyz=False):
|
||||
"""Loads a Wavefront OBJ file. """
|
||||
self.vertices = []
|
||||
self.normals = []
|
||||
self.texcoords = []
|
||||
self.faces = []
|
||||
|
||||
dir_name = '/'.join(filename.split('/')[:-1])
|
||||
|
||||
material = None
|
||||
for line in open(filename, "r"):
|
||||
if line.startswith('#'): continue
|
||||
values = line.split()
|
||||
if not values: continue
|
||||
if values[0] == 'v':
|
||||
v = list(map(float, values[1:4]))
|
||||
if swapyz:
|
||||
v = v[0], v[2], v[1]
|
||||
self.vertices.append(v)
|
||||
elif values[0] == 'vn':
|
||||
v = list(map(float, values[1:4]))
|
||||
if swapyz:
|
||||
v = v[0], v[2], v[1]
|
||||
self.normals.append(v)
|
||||
elif values[0] == 'vt':
|
||||
self.texcoords.append(list(map(float, values[1:3])))
|
||||
elif values[0] in ('usemtl', 'usemat'):
|
||||
material = values[1]
|
||||
elif values[0] == 'mtllib':
|
||||
print("mtllib: ", values[1])
|
||||
self.mtl = MTL(f"{dir_name}/{values[1]}")
|
||||
elif values[0] == 'f':
|
||||
face = []
|
||||
texcoords = []
|
||||
norms = []
|
||||
for v in values[1:]:
|
||||
w = v.split('/')
|
||||
face.append(int(w[0]))
|
||||
if len(w) >= 2 and len(w[1]) > 0:
|
||||
texcoords.append(int(w[1]))
|
||||
else:
|
||||
texcoords.append(0)
|
||||
if len(w) >= 3 and len(w[2]) > 0:
|
||||
norms.append(int(w[2]))
|
||||
else:
|
||||
norms.append(0)
|
||||
self.faces.append((face, norms, texcoords, material))
|
||||
|
||||
self.gl_list = glGenLists(1)
|
||||
glNewList(self.gl_list, GL_COMPILE)
|
||||
glEnable(GL_TEXTURE_2D)
|
||||
glFrontFace(GL_CCW)
|
||||
for face in self.faces:
|
||||
vertices, normals, texture_coords, material = face
|
||||
|
||||
mtl = self.mtl[material]
|
||||
if 'texture_Kd' in mtl:
|
||||
# use diffuse texmap
|
||||
glBindTexture(GL_TEXTURE_2D, mtl['texture_Kd'])
|
||||
else:
|
||||
# just use diffuse colour
|
||||
glColor(*mtl['Kd'])
|
||||
|
||||
glBegin(GL_POLYGON)
|
||||
for i in range(len(vertices)):
|
||||
if normals[i] > 0:
|
||||
glNormal3fv(self.normals[normals[i] - 1])
|
||||
if texture_coords[i] > 0:
|
||||
glTexCoord2fv(self.texcoords[texture_coords[i] - 1])
|
||||
glVertex3fv(self.vertices[vertices[i] - 1])
|
||||
glEnd()
|
||||
glDisable(GL_TEXTURE_2D)
|
||||
glEndList()
|
||||
|
||||
43
projection_matrix.py
Normal file
43
projection_matrix.py
Normal file
@ -0,0 +1,43 @@
|
||||
import numpy as np
|
||||
from numpy.core.multiarray import dtype
|
||||
|
||||
CAMERA_MATRIX = np.load('camera_parameters.npy', allow_pickle=True).item()['K']
|
||||
|
||||
W, H = 1280, 720
|
||||
FAR, NEAR = 100, 1
|
||||
|
||||
def build_opengl_projection_for_intrinsics(intrinsics, w, h, far, near):
|
||||
left = 0
|
||||
right = w
|
||||
bottom = h
|
||||
top = 0
|
||||
print("Instrinsics")
|
||||
print(intrinsics)
|
||||
print()
|
||||
|
||||
ortho = np.array([
|
||||
[ 2/(right-left), 0, 0, -(right+left)/(right-left)],
|
||||
[ 0, 2/(top-bottom), 0, -(top+bottom)/(top-bottom)],
|
||||
[ 0, 0, -2/(far-near), -(far+near)/(far-near)],
|
||||
[ 0, 0, 0, 1],
|
||||
], dtype=np.float32)
|
||||
print("Ortho")
|
||||
print(ortho)
|
||||
print()
|
||||
|
||||
persp = np.array([
|
||||
[ intrinsics[0][0], intrinsics[0][1], -intrinsics[0][2], 0],
|
||||
[ 0, intrinsics[1][1], -intrinsics[1][2], 0],
|
||||
[ 0, 0, (near+far), (near*far)],
|
||||
[ 0, 0, -1, 0],
|
||||
], dtype=np.float32)
|
||||
print("Persp")
|
||||
print(persp)
|
||||
print()
|
||||
|
||||
print("ANS")
|
||||
print(ortho @ persp)
|
||||
print()
|
||||
return ortho @ persp
|
||||
|
||||
|
||||
105
test.py
Normal file
105
test.py
Normal file
@ -0,0 +1,105 @@
|
||||
# Basic OBJ file viewer. needs objloader from:
|
||||
# http://www.pygame.org/wiki/OBJFileLoader
|
||||
# LMB + move: rotate
|
||||
# RMB + move: pan
|
||||
# Scroll wheel: zoom in/out
|
||||
import sys, pygame
|
||||
from pygame.locals import *
|
||||
from pygame.constants import *
|
||||
from OpenGL.GL import *
|
||||
from OpenGL.GLU import *
|
||||
|
||||
# IMPORT OBJECT LOADER
|
||||
from objloader import *
|
||||
from projection_matrix import build_opengl_projection_for_intrinsics
|
||||
import numpy as np
|
||||
|
||||
CAMERA_MATRIX = np.load('camera_parameters.npy', allow_pickle=True).item()['K']
|
||||
EXTRINSIC_MATRIX = np.array([[0.7227021503496043, -0.3686776760226368, 0.5846181429639397, -16.218840163220325], [0.44397563400870765, 0.8958935447266068, 0.016136694945417585, -1.2057431225855768], [-0.5297048596026128, 0.24789418653881962, 0.8111480962152553, 49.33418176483625], [0.0, 0.0, 0.0, 1.0]])
|
||||
W, H = 1280, 720
|
||||
FAR, NEAR = 10000.0, 1.0
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
proj = build_opengl_projection_for_intrinsics(CAMERA_MATRIX, W, H, FAR, NEAR)
|
||||
|
||||
pygame.init()
|
||||
viewport = (W, H)
|
||||
hx = viewport[0]/2
|
||||
hy = viewport[1]/2
|
||||
srf = pygame.display.set_mode(viewport, OPENGL | DOUBLEBUF)
|
||||
|
||||
# glLightfv(GL_LIGHT0, GL_POSITION, (-40, 200, 100, 0.0))
|
||||
# glLightfv(GL_LIGHT0, GL_AMBIENT, (0.2, 0.2, 0.2, 1.0))
|
||||
# glLightfv(GL_LIGHT0, GL_DIFFUSE, (0.5, 0.5, 0.5, 1.0))
|
||||
glEnable(GL_LIGHT0)
|
||||
glEnable(GL_LIGHTING)
|
||||
glEnable(GL_COLOR_MATERIAL)
|
||||
glEnable(GL_DEPTH_TEST)
|
||||
glShadeModel(GL_SMOOTH) # most obj files expect to be smooth-shaded
|
||||
|
||||
# LOAD OBJECT AFTER PYGAME INIT
|
||||
obj = OBJ(sys.argv[1], swapyz=True)
|
||||
|
||||
clock = pygame.time.Clock()
|
||||
|
||||
# set projection matrix
|
||||
# use intrinsics to build for OpenGL
|
||||
glMatrixMode(GL_PROJECTION)
|
||||
glLoadIdentity()
|
||||
# width, height = viewport
|
||||
# gluPerspective(90.0, width/float(height), 1, 100.0)
|
||||
print(proj)
|
||||
glLoadMatrixf(proj)
|
||||
glEnable(GL_DEPTH_TEST)
|
||||
|
||||
|
||||
# set model view matrix
|
||||
# use extrinsics
|
||||
glMatrixMode(GL_MODELVIEW)
|
||||
|
||||
rx, ry = (0,0)
|
||||
tx, ty = (0,0)
|
||||
zpos = 5
|
||||
rotate = move = False
|
||||
while 1:
|
||||
clock.tick(30)
|
||||
for e in pygame.event.get():
|
||||
if e.type == QUIT:
|
||||
sys.exit()
|
||||
elif e.type == KEYDOWN and e.key == K_ESCAPE:
|
||||
sys.exit()
|
||||
elif e.type == MOUSEBUTTONDOWN:
|
||||
if e.button == 4: zpos = max(1, zpos-1)
|
||||
elif e.button == 5: zpos += 1
|
||||
elif e.button == 1: rotate = True
|
||||
elif e.button == 3: move = True
|
||||
elif e.type == MOUSEBUTTONUP:
|
||||
if e.button == 1: rotate = False
|
||||
elif e.button == 3: move = False
|
||||
elif e.type == MOUSEMOTION:
|
||||
i, j = e.rel
|
||||
if rotate:
|
||||
rx += i
|
||||
ry += j
|
||||
if move:
|
||||
tx += i
|
||||
ty -= j
|
||||
|
||||
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
|
||||
glMatrixMode(GL_MODELVIEW)
|
||||
glLoadIdentity()
|
||||
glLoadMatrixf(EXTRINSIC_MATRIX)
|
||||
|
||||
# RENDER OBJECT
|
||||
'''
|
||||
glTranslate(tx/20., ty/20., - zpos)
|
||||
size = 3
|
||||
glScale(size, size, size)
|
||||
glRotate(ry, 1, 0, 0)
|
||||
glRotate(rx, 0, 1, 0)
|
||||
'''
|
||||
glCallList(obj.gl_list)
|
||||
|
||||
pygame.display.flip()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user