def viewpoint_projection(self, eye, plane_index):
ref = plane_index+1
plane,_ = self.surfaces[plane_index]
with opengl_state():
glEnable(GL_STENCIL_TEST)
glStencilFunc(GL_EQUAL, ref, 0xFF)
glStencilOp(GL_KEEP, GL_KEEP, GL_KEEP)
with self.viewpoint_program.projection(self, eye, np.array(plane)):
yield
def draw_sights(eye):
rot = np.eye(4, dtype='f')
mag = lambda x : np.sqrt((x*x).sum())
norm = lambda x : x / mag(x)
rot[:3,2] = norm(-eye)
rot[:3,0] = norm(np.cross([0,1,0], rot[:3,2]))
rot[:3,1] = norm(np.cross(rot[:3,2], rot[:3,0]))
def circle(rad, N=20):
glBegin(GL_LINE_LOOP)
np.tau = np.pi*2
for h in np.arange(0, np.tau, np.tau/N):
glVertex(rad*np.cos(h), rad*np.sin(h))
glEnd()
def cross(rad):
glRotate(45,0,0,1)
glScale(rad, rad, rad)
glBegin(GL_LINES)
glVertex(-1,0,0); glVertex(1,0,0)
glVertex(0,-1,0); glVertex(0,1,0)
glEnd()
rad = 0.05
with opengl_state():
glColor(1,0,0)
glTranslate(rad,0,0)
glMultMatrixf(rot.transpose())
cross(rad*0.6)
rad = 0.05
with opengl_state():
glColor(1,0,0)
glTranslate(-rad,0,0)
glMultMatrixf(rot.transpose())
cross(rad*0.6)
with opengl_state():
glColor(1,0,0)
glMultMatrixf(rot.transpose())
circle(rad)
glTranslate(0, 0, -mag(eye)/2)
circle(rad)
def draw(self):
w,h = window.Size
with opengl_state():
glEnableClientState(GL_VERTEX_ARRAY)
glVertexPointerf(self.vertices)
glEnableClientState(GL_COLOR_ARRAY)
glColorPointerub(self.colors)
glTranslate(-1,-1,1)
glScale(2./w,2./h,1)
glDrawElementsui(GL_QUADS, self.quad_inds)
def render(mode):
glDrawBuffer(dict(left=GL_BACK_LEFT, right=GL_BACK_RIGHT, center=GL_BACK)[mode])
glClearColor(0, 0, 0, 0)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
proj_matrix, model_matrix = make_projection(camera, mode)
glMatrixMode(GL_PROJECTION)
glLoadMatrixf(proj_matrix.transpose())
glMatrixMode(GL_MODELVIEW)
glLoadMatrixf(model_matrix.transpose())
with opengl_state():
draw_thing()
def render_frustum(RT):
assert RT.shape == (4,4)
assert RT.dtype == np.float32
with opengl_state():
glMultMatrixf(RT.transpose())
glScale(0.2, 0.2, 0.2)
# Draw a little box, that's all
glBegin(GL_LINES)
glColor(1,0,0); glVertex(0,0,0); glVertex(1,0,0)
glColor(0,1,0); glVertex(0,0,0); glVertex(0,1,0)
glColor(0,0,1); glVertex(0,0,0); glVertex(0,0,1)
glColor(1,1,1); glVertex(0,0,0); glVertex(0,0,-1)
glEnd()
def post_draw():
with opengl_state():
B = np.eye(4); B[:3,:3] = basis
glMultMatrixf(np.linalg.inv(B).transpose())
glScale(1.3, 1.3, 1.3)
glEnable(GL_LINE_STIPPLE)
glLineStipple(3, 0xAAAA)
glBegin(GL_LINES)
glColor(1,0,0); glVertex(0,0,0); glVertex(1,0,0)
glColor(0,1,0); glVertex(0,0,0); glVertex(0,1,0)
glColor(0,0,1); glVertex(0,0,0); glVertex(0,0,1)
glEnd()
with opengl_state():
B = np.eye(4); B[:3,:3] = estimate
glMultMatrixf(np.linalg.inv(B).transpose())
glLineWidth(2)
glScale(1.3, 1.3, 1.3)
glBegin(GL_LINES)
glColor(1,0,0); glVertex(0,0,0); glVertex(1,0,0)
glColor(0,1,0); glVertex(0,0,0); glVertex(0,1,0)
glColor(0,0,1); glVertex(0,0,0); glVertex(0,0,1)
glEnd()
def post_draw():
with opengl_state():
B = np.eye(4)
B[:3, :3] = basis
glMultMatrixf(np.linalg.inv(B).transpose())
glScale(1.3, 1.3, 1.3)
glEnable(GL_LINE_STIPPLE)
glLineStipple(3, 0xAAAA)
glBegin(GL_LINES)
glColor(1, 0, 0)
glVertex(0, 0, 0)
glVertex(1, 0, 0)
glColor(0, 1, 0)
glVertex(0, 0, 0)
glVertex(0, 1, 0)
glColor(0, 0, 1)
glVertex(0, 0, 0)
glVertex(0, 0, 1)
glEnd()
with opengl_state():
B = np.eye(4)
B[:3, :3] = estimate
glMultMatrixf(np.linalg.inv(B).transpose())
glLineWidth(2)
glScale(1.3, 1.3, 1.3)
glBegin(GL_LINES)
glColor(1, 0, 0)
glVertex(0, 0, 0)
glVertex(1, 0, 0)
glColor(0, 1, 0)
glVertex(0, 0, 0)
glVertex(0, 1, 0)
glColor(0, 0, 1)
glVertex(0, 0, 0)
glVertex(0, 0, 1)
glEnd()
def draw_objects(eye):
vertices = [[0,0,0],[0,0,1],[0,1,1],[0,1,0],
[1,1,0],[1,1,1],[1,0,1],[1,0,0]]
cube_inds = [0,1, 1,2, 2,3, 3,4, 4,5, 5,6, 6,7, 0,3, 4,7, 1,6, 2,5]
Y_inds = [0,1, 1,6, 6,7, 7,0]
Z_inds = [0,3, 3,4, 4,7, 7,0]
draw_eye(eye)
draw_sights(eye)
with opengl_state():
glScale(.05,.05,.05)
glTranslate(-0.5, 0, 0)
glBegin(GL_LINES)
glColor(0,1,0)
for i in cube_inds: glVertex(*vertices[i])
glEnd()
if 'obj' in globals():
with opengl_state():
glScale(.1,.1,.1)
glRotate(rot_angle, 0, 1, 0)
glScale(-1,1,1)
obj.draw()
def on_draw():
glClearColor(0,0,0,1)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glEnable(GL_DEPTH_TEST)
global projector
if not 'projector' in globals(): return
# Project decals directly to the projector, no eye needed
with opengl_state():
projector.setup_projection_matrix()
# draw_decals() # Depth is inconsistent between decals and viewpoint
# For each projection surface plane, we need to do a render
for plane_index in [0, 1]:
with projector.viewpoint_projection(eye, plane_index):
draw_decals()
draw_objects(eye)
def cv2opengl(KK_CV, size, near=0.01, far=10.0):
W,H = size
assert KK_CV[0,0] == KK_CV[1,1]
F = KK_CV[0,0]
cx = KK_CV[0,2]
cy = KK_CV[1,2]
left = near*(0-cx)/F
right = near*(W-cx)/F
bottom = near*(cy-H)/F
top = near*(cy-0)/F
with opengl_state():
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
glFrustum(left, right, bottom, top, near, far)
GL_KK = glGetFloatv(GL_PROJECTION_MATRIX).transpose()
return GL_KK
def draw_blocks():
with opengl_state():
glMultMatrixf(np.linalg.inv(preview.modelmat).transpose())
LH,LW = config.LH, config.LW
glScale(LW,LH,LW)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
glPushMatrix()
glTranslate(*config.bounds[0])
blockdraw.draw()
glPopMatrix()
# Draw the axes for the model coordinate space
glLineWidth(3)
glBegin(GL_LINES)
glColor3f(1,0,0); glVertex3f(0,0,0); glVertex3f(1,0,0)
glColor3f(0,0,1); glVertex3f(0,0,0); glVertex3f(0,0,1)
glEnd()
def draw_decals():
# Only draw things that are known to be on the projection surfaces
w, h = (0.2160, 0.2794)
obj_points = np.array([[-w/2, h/2, 0], [w/2, h/2, 0],
[-w/2, 0, 0], [w/2, 0, 0],
[-w/2, 0, h/2], [w/2, 0, h/2]])
glBegin(GL_LINES)
glColor(1,1,1)
for i in (0,1, 2,3, 4,5, 0,2, 2,4, 1,3, 3,5):
glVertex(*obj_points[i])
glEnd()
with opengl_state():
glScale(.05,.5,.05)
glBegin(GL_LINES)
glColor(1,0,0); glVertex(0,0,0); glVertex(1,0,0)
glColor(0,1,0); glVertex(0,0,0); glVertex(0,1,0)
glColor(0,0,1); glVertex(0,0,0); glVertex(0,0,1)
glEnd()
def render_frustum(self):
with opengl_state():
glMultMatrixf(self.RT.transpose())
glScale(0.2, 0.2, 0.2)
# Draw a little box, that's all
glBegin(GL_LINES)
glColor(1, 0, 0)
glVertex(0, 0, 0)
glVertex(1, 0, 0)
glColor(0, 1, 0)
glVertex(0, 0, 0)
glVertex(0, 1, 0)
glColor(0, 0, 1)
glVertex(0, 0, 0)
glVertex(0, 0, 1)
glColor(1, 1, 1)
glVertex(0, 0, 0)
glVertex(0, 0, -1)
glEnd()
def prepare_stencil(self):
# Deal with each of the surfaces
glClearStencil(0)
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT)
with opengl_state():
glColorMask(0,0,0,0)
glEnable(GL_STENCIL_TEST)
glEnable(GL_DEPTH_TEST)
#glColorMask(0,0,0,0) # Only care about stencil buffer
self.setup_projection_matrix()
for i,(plane,quads) in enumerate(self.surfaces):
ref = i+1
glStencilFunc(GL_ALWAYS, ref, ref)
glStencilOp(GL_KEEP, GL_KEEP, GL_REPLACE)
glBegin(GL_QUADS)
colors = [[1,0,0],[0,1,0],[0,0,1]]
glColor3f(*colors[ref])
for quad in quads:
for q in quad: glVertex(*q)
glEnd()
def make_projection(camera, mode):
assert mode in ("left", "right", "center")
# Copy the matrix projection mode given camera parameters
eyesep = 0.063 # Average adult eyes are 60mm apart
width = 0.736 # Width of the projection image (m)
focal_length = 1.5 # Distance to the projection image (m)
ratio = 1920 / 1080.0 # Width / height
# ratio = 1024 / 768. # Width / height
far = 10
near = 0.5 # Near plane is half meter in front of eyes
tan_ap = 0.5 * width / focal_length
wd2 = near * tan_ap
ndfl = near / focal_length
offsetx = dict(left=-1, right=1, center=0)[mode] * 0.5 * eyesep
left = -ratio * wd2 + offsetx * ndfl
right = ratio * wd2 + offsetx * ndfl
top = wd2
bottom = -wd2
with opengl_state():
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
glFrustum(left, right, bottom, top, near, far)
projection = glGetFloatv(GL_PROJECTION_MATRIX).transpose()
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glTranslate(offsetx, 0, -focal_length)
glRotate(180, 0, 1, 0)
modelview = glGetFloatv(GL_MODELVIEW_MATRIX).transpose()
return projection, modelview
def projection(self, projector, eye, plane):
KKp = projector.KK
RTp = projector.RT
# Construct a projection matrix that projects onto the plane.
# Additionally the scale should be set so w is the actual depth.
# Then (A*w + B)/w is the clipped depth for eye coordinates
F = np.dot(np.concatenate((eye,[1])), plane.transpose())
if F < 0: F *= -1
KKe = np.array([[F, 0, 0, 0],
[0, F, 0, 0],
[0, 0, F, 0],
[0, 0,-1, 0]]) # Note - a projection, not invertible
near = 0.01
far = 5.0
A = -(far + near) / (far - near)
B = -(2*far*near) / (far - near)
# Sanity checks
within_epsilon = lambda a, b: (np.abs(a-b) < 1E-5).all()
near_zero = lambda x: within_epsilon(x, np.float32(0))
def check():
#print near, far
#print A, B
divide = lambda w: (A * w + B) / w
assert within_epsilon(divide(-near),1)
assert within_epsilon(divide(-far),-1)
check()
# Construct the Eye view matrix from the plane and the projector
RTe = np.eye(4, dtype='f')
magnitude = lambda x: np.sqrt(np.sum(x*x))
normalize = lambda x: x / magnitude(x)
RTe[:3,3] = eye # Last column is the actual eye position
RTe[:3,2] = normalize(plane[:3]) # Take Z to be the plane normal
RTe[:3,0] = normalize(np.cross(RTp[:3,1], RTe[:3,2]))
RTe[:3,1] = normalize(np.cross(RTe[:3,2], RTe[:3,0]))
# Pack everything up and configure the shader
KKe_invRTe = np.dot(KKe, np.linalg.inv(RTe))
KKp_invRTp_RTe = np.dot(np.dot(KKp, np.linalg.inv(RTp)), RTe)
homo_divide = lambda x: (x[0]/x[3], x[1]/x[3], x[2]/x[3])
if 0:
print 'KKe_invRTe'
print KKe_invRTe
print
print 'KKe'
print KKe
print
print 'KKp_invRTp_RTe'
print KKp_invRTp_RTe
print
print homo_divide(np.dot(KKe_invRTe, [0,0,0,1]))
self.RTe = RTe
self.KKe = KKe
toplane = np.dot(RTe, KKe_invRTe)
KKp_invRTp = np.dot(KKp, np.linalg.inv(RTp))
full = np.dot(KKp_invRTp, toplane)
# Some sanity checks to demonstrate how this works
within_epsilon = lambda a, b: (abs(a-b) < 1E-5).all()
near_zero = lambda x: within_epsilon(x, np.float32(0))
assert within_epsilon(full, np.dot(KKp_invRTp_RTe, KKe_invRTe))
# For any point on the plane,
# (RTe) (KKe)(RTe^-1) P should also be on the plane
def check_plane(P):
assert near_zero(np.dot(P, plane))
RTe_KKe_invRTe = np.dot(RTe, KKe_invRTe)
assert near_zero(np.dot(np.dot(RTe_KKe_invRTe, P), plane))
return homo_divide(np.dot(RTe_KKe_invRTe, P))
assert near_zero(check_plane([0,0,0,1])) # Origin is untouched
if within_epsilon(plane, [0,1,0,0]):
check_plane([1,0,1,1])
check_plane([-1,0,-1,1])
if within_epsilon(plane, [0,0,1,0]):
check_plane([1,1,0,1])
check_plane([-1,-1,0,1])
def point_journey(P=[0,0,0,1]):
toplane = np.dot(RTe, KKe_invRTe)
full = np.dot(KKp_invRTp, toplane)
print 'P: ', P
print '(invRTe)P: ', homo_divide(np.dot(np.linalg.inv(RTe), P))
print '(KKe_invRTe)P: ', homo_divide(np.dot(KKe_invRTe, P))
print '(RTe_KKe_invRTe)P: (plane)', homo_divide(np.dot(toplane, P))
print '(full)P: ', homo_divide(np.dot(full, P))
#point_journey()
p = self.program
loc = lambda x: glGetUniformLocation(p, x)
matrix = lambda k, f: glUniformMatrix4fv(k, 1, True, f.astype('f'))
assert glIsProgram(p)
with opengl_state():
"""Load the appropriate matrices for projection and view
KK_e * RTe^-1 * P
where P is a point in world coordinates
"""
# Set up GL_MODELVIEW with identity so that
# consumers can use world (model) coordinates
try:
glUseProgram(self.program)
matrix(loc("KKe_invRTe"), KKe_invRTe)
matrix(loc("KKp_invRTp_RTe"), KKp_invRTp_RTe)
glUniform1f(loc("A"), A)
glUniform1f(loc("B"), B)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
yield
finally:
glUseProgram(0)
def draw_eye(eye):
with opengl_state():
glTranslate(*eye)
glColor(1,1,0)
glutSolidSphere(.02, 10, 10)
def post_draw():
glClearColor(0,0,0,1)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
# Draw a frame
glBegin(GL_LINES)
glColor(1,1,1)
for i in (0,1, 2,3, 4,5, 0,2, 2,4, 1,3, 3,5):
glVertex(*obj_points[i])
glEnd()
# Draw the target points and imaged finger points
for p in target_points:
with opengl_state():
glTranslate(*p)
glColor(1,0,1)
glutSolidSphere(.01, 10, 10)
glBegin(GL_LINES)
glVertex(*eye); glVertex(*p)
glEnd()
for p in finger_img:
with opengl_state():
glMultMatrixf(camera.transpose())
glTranslate(*p)
glColor(0,1,1)
glutSolidSphere(.01, 10, 10)
# Draw the eye
with opengl_state():
glMultMatrixf(camera.transpose())
glTranslate(*eye_img)
glColor(1,1,0)
glutSolidSphere(.02, 10, 10)
# Draw coordinate axes
with opengl_state():
glScale(.05,.05,.05)
glBegin(GL_LINES)
glColor(1,0,0); glVertex(0,0,0); glVertex(1,0,0)
glColor(0,1,0); glVertex(0,0,0); glVertex(0,1,0)
glColor(0,0,1); glVertex(0,0,0); glVertex(0,0,1)
glEnd()
# Draw coordinate axes for the camera
with opengl_state():
glMultMatrixf(camera.transpose())
glScale(0.2, 0.2, 0.2)
glBegin(GL_LINES)
glColor(1,0,0); glVertex(0,0,0); glVertex(1,0,0)
glColor(0,1,0); glVertex(0,0,0); glVertex(0,1,0)
glColor(0,0,1); glVertex(0,0,0); glVertex(0,0,1)
glColor(1,1,1); glVertex(0,0,0); glVertex(0,0,-1)
glEnd()
# Draw coordinate axes for the camera
with opengl_state():
glMultMatrixf(camera_est.transpose())
glScale(0.2, 0.2, 0.2)
glLineWidth(2)
glBegin(GL_LINES)
glColor(1,0,0); glVertex(0,0,0); glVertex(1,0,0)
glColor(0,1,0); glVertex(0,0,0); glVertex(0,1,0)
glColor(0,0,1); glVertex(0,0,0); glVertex(0,0,1)
glColor(1,1,1); glVertex(0,0,0); glVertex(0,0,-1)
glEnd()
