Skip to content

Instantly share code, notes, and snippets.

@astraw
Created November 5, 2011 12:51
Show Gist options
  • Save astraw/1341472 to your computer and use it in GitHub Desktop.
Save astraw/1341472 to your computer and use it in GitHub Desktop.
computing the OpenGL projection matrix from intrinsic camera parameters
#!/usr/bin/env python
# stdlib imports
import os
#other imports
import numpy as np
import scipy.misc
import matplotlib.pyplot as plt
import matplotlib.cm
from calib_test_utils import generate_cyl_points, decompose, \
convert_hz_intrinsic_to_opengl_projection
R2D = 180.0/np.pi
def debug(val):
if 0:
print repr(val)
def main():
np.set_printoptions(precision=5, linewidth=200)
window_coords = 'y down'
data_dir = os.path.split(os.path.abspath(__file__))[0]
pmat = np.loadtxt( os.path.join(data_dir, 'cameramatrix.txt') )
luminance = scipy.misc.imread( os.path.join(data_dir, 'luminance.png' ) )
img_height, img_width = luminance.shape
cyl_points_3d_h = generate_cyl_points(homog=True,n_segs=50)
debug('cyl_points_3d_h')
debug(cyl_points_3d_h)
if 1:
calib = decompose(pmat)
cyl_points_3d_eye = np.dot( calib['extrinsic'], cyl_points_3d_h )
debug('cyl_points_3d_eye')
debug(cyl_points_3d_eye)
if 1:
e = np.vstack((calib['extrinsic'],[[0,0,0,1]])) # These HZ eye coords have +Z in front of camera.
coord_xform = np.eye(4)
coord_xform[1,1]=-1 # flip Y coordinate (HZ camera has +y going down, GL has +y going up)
coord_xform[2,2]=-1 # flip Z coordinate (HZ camera looks at +z, GL looks at -z)
#debug('e')
#debug(e)
e2 = np.dot( coord_xform, e)
#debug('e2')
#debug(e2)
cyl_points_3d_eye_hz_h = np.dot( e, cyl_points_3d_h )
cyl_points_3d_eye_opengl_h = np.dot( e2, cyl_points_3d_h )
debug('cyl_points_3d_eye_hz_h')
debug(cyl_points_3d_eye_hz_h)
debug('cyl_points_3d_eye_opengl_h')
debug(cyl_points_3d_eye_opengl_h)
if 1:
cyl_points_2d_h = np.dot(calib['intrinsic'], cyl_points_3d_eye)
cyl_points_2d = cyl_points_2d_h[:2]/cyl_points_2d_h[2]
else:
cyl_points_2d_h = np.dot(pmat, cyl_points_3d_h)
cyl_points_2d = cyl_points_2d_h[:2]/cyl_points_2d_h[2]
debug('cyl_points_2d')
debug(cyl_points_2d)
if 1:
if 1:
x0 = 0
y0 = 0
gl_viewport_args = x0, y0, img_width, img_height
proj = convert_hz_intrinsic_to_opengl_projection(calib['intrinsic'],
x0,y0,img_width,img_height, 0.1, 1000.0,
window_coords=window_coords)
clip = np.dot(proj,cyl_points_3d_eye_opengl_h)
debug('clip')
debug(clip)
ndc = clip[:3,:]/clip[3,:]
debug('ndc')
debug(ndc)
window_x = gl_viewport_args[2]/2.0 * (ndc[0,:] + 1)+int(gl_viewport_args[0])
window_y = gl_viewport_args[3]/2.0 * (ndc[1,:] + 1)+int(gl_viewport_args[1])
window_gl = np.vstack((window_x,window_y))
debug('window_gl')
debug(window_gl)
if 1:
if window_coords=='y down':
luminance = luminance[::-1]
yc = img_height-cyl_points_2d[1,:]
else:
assert window_coords=='y up'
yc = cyl_points_2d[1,:]
plt.imshow(luminance, interpolation='nearest', origin='lower', cmap=matplotlib.cm.gray)
plt.plot( cyl_points_2d[0,:], yc, 'b+', ms=15.0 )
plt.plot( window_x, window_y, 'r.' )
plt.show()
if __name__=='__main__':
main()
#!/usr/bin/env python
# stdlib imports
import os
from contextlib import contextmanager
#other imports
import numpy as np
import pyglet
import pyglet.gl as gl
from calib_test_utils import generate_cyl_points, decompose, \
convert_hz_intrinsic_to_opengl_projection
R2D = 180.0/np.pi
class PointCylinder(object):
def __init__(self):
pts = generate_cyl_points().T
colors = np.zeros_like(pts)
colors[:,1]=1.0 # all green
n_pts = len(pts)
pts = map(float,pts.flat) # convert to flat list of floats
colors = map(float, colors.flat)
# Create ctypes arrays of the lists
vertices = (gl.GLfloat * (n_pts*3))(*pts)
colors = (gl.GLfloat * (n_pts*3))(*colors)
# Create a list of triangle indices.
indices = range(n_pts)
indices = (gl.GLuint * n_pts)(*indices)
# Compile a display list
self.list = gl.glGenLists(1)
gl.glNewList(self.list, gl.GL_COMPILE)
gl.glPushClientAttrib(gl.GL_CLIENT_VERTEX_ARRAY_BIT)
gl.glEnableClientState(gl.GL_VERTEX_ARRAY)
gl.glVertexPointer(3, gl.GL_FLOAT, 0, vertices)
gl.glEnableClientState(gl.GL_COLOR_ARRAY)
gl.glColorPointer(3, gl.GL_FLOAT, 0, colors)
gl.glDrawElements(gl.GL_POINTS, len(indices), gl.GL_UNSIGNED_INT, indices)
gl.glPopClientAttrib()
gl.glEndList()
def draw(self):
gl.glPointSize(5.0)
gl.glCallList(self.list)
gl.glColor3f(1.0, 1.0, 1.0)
class MyAppWindow(pyglet.window.Window):
def __init__(self,image_fname=None,pmat=None,window_coords=None,**kwargs):
if window_coords is None:
# set default value
window_coords = 'y down'
super(MyAppWindow, self).__init__(**kwargs)
self.calib = decompose(pmat)
self.window_coords = window_coords
self.img = pyglet.image.load(image_fname).get_texture(rectangle=True)
if self.window_coords=='y up':
self.img = self.img.get_transform(flip_y=True)
self.img.anchor_x = self.img.anchor_y = 0
self.width = self.img.width
self.height = self.img.height
checks = pyglet.image.create(32, 32, pyglet.image.CheckerImagePattern())
self.background = pyglet.image.TileableTexture.create_for_image(checks)
# Enable alpha blending, required for image.blit.
gl.glEnable(gl.GL_BLEND)
gl.glBlendFunc(gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA)
self.cyl = PointCylinder()
# set modelview matrix to camera extrinsic parameters
# Create ctypes arrays of the lists
e = np.vstack((self.calib['extrinsic'],[[0,0,0,1]])) # These HZ eye coords have +Z in front of camera.
coord_xform = np.eye(4)
coord_xform[1,1]=-1 # flip Y coordinate in eye space (OpenGL has +Y as up, HZ has -Y)
coord_xform[2,2]=-1 # flip Z coordinate in eye space (OpenGL has -Z in front of camera, HZ has +Z)
e2 = np.dot( coord_xform, e)
extrinsic = map(float,e2.T.flat)
extrinsic = (gl.GLfloat * 16)(*extrinsic)
gl.glMatrixMode(gl.GL_MODELVIEW)
gl.glLoadMatrixf(extrinsic)
gl.glDisable(gl.GL_DEPTH_TEST)
def on_draw(self):
self.clear()
with self.window_coordinates_ydown():
self.background.blit_tiled(0, 0, 0, self.width, self.height)
self.img.blit(0,0,0)
self.cyl.draw()
@contextmanager
def window_coordinates_ydown(self):
# These are screen coords. Y increases downward, with 0 at top
# of initial window. (Standard OpenGL has 0 at bottom and Y
# increasing upward.)
gl.glPushMatrix()
gl.glLoadIdentity()
gl.glMatrixMode(gl.GL_PROJECTION)
gl.glPushMatrix()
gl.glLoadIdentity()
gl.glOrtho(0, self.width, 0, self.height, -1, 1)
gl.glViewport(0, 0, self.width, self.height)
yield # perform drawing
gl.glViewport(*self.gl_viewport_args)
gl.glPopMatrix()
gl.glMatrixMode(gl.GL_MODELVIEW)
gl.glPopMatrix()
def on_resize(self, width, height):
# load HZ matrix into OpenGL equivalent
x0 = 0
y0 = 0
self.gl_viewport_args = x0, y0, self.img.width, self.img.height
gl.glViewport(*self.gl_viewport_args)
znear, zfar = .1, 1000.
proj = convert_hz_intrinsic_to_opengl_projection(self.calib['intrinsic'],
x0,y0,self.img.width,self.img.height, znear, zfar,
window_coords=self.window_coords)
m = map(float,proj.T.flat)
m = (gl.GLfloat * 16)(*m)
gl.glMatrixMode(gl.GL_PROJECTION)
gl.glLoadMatrixf(m)
gl.glMatrixMode(gl.GL_MODELVIEW)
return pyglet.event.EVENT_HANDLED
def main():
data_dir = os.path.split(os.path.abspath(__file__))[0]
pmat = np.loadtxt( os.path.join(data_dir, 'cameramatrix.txt') )
image_fname = os.path.join(data_dir, 'luminance.png' )
window = MyAppWindow(pmat=pmat,image_fname=image_fname,resizable=True)
pyglet.app.run()
if __name__=='__main__':
main()
import scipy.linalg
import numpy as np
def my_rq(M):
"""RQ decomposition, ensures diagonal of R is positive"""
R,K = scipy.linalg.rq(M)
n = R.shape[0]
for i in range(n):
if R[i,i]<0:
R[:,i] = -R[:,i]
K[i,:] = -K[i,:]
return R,K
def pmat2cam_center(P):
"""
See Hartley & Zisserman (2003) p. 163
"""
assert P.shape == (3,4)
determinant = np.linalg.det
# camera center
X = determinant( [ P[:,1], P[:,2], P[:,3] ] )
Y = -determinant( [ P[:,0], P[:,2], P[:,3] ] )
Z = determinant( [ P[:,0], P[:,1], P[:,3] ] )
T = -determinant( [ P[:,0], P[:,1], P[:,2] ] )
C_ = np.transpose(np.array( [[ X/T, Y/T, Z/T ]] ))
return C_
def decompose(pmat):
M = pmat[:,:3]
K,R = my_rq(M)
K = K/K[2,2] # normalize intrinsic parameter matrix
C_ = pmat2cam_center(pmat)
t = np.dot( -R, C_)
Rt = np.hstack((R, t ))
return dict( intrinsic=K,
rotation=R,
cam_center=C_,
t=t,
extrinsic=Rt,
)
def generate_cyl_points(homog=False,n_segs=30):
r = 0.5
h = 1.0
z0 = 0.0
theta0 = 0
theta = np.linspace( 0, 2*np.pi, num=n_segs, endpoint=False)+theta0
z = np.linspace( 0, h, num=2)+z0
points = []
for zi in z:
xx = r*np.cos(theta)
yy = r*np.sin(theta)
zz = zi*np.ones_like(xx)
if homog:
ww = np.ones_like(xx)
points.extend([(xx[i], yy[i], zz[i],ww[i]) for i in range(theta.shape[0])])
else:
points.extend([(xx[i], yy[i], zz[i]) for i in range(theta.shape[0])])
points = np.array(points).T
return points
def convert_hz_intrinsic_to_opengl_projection(K,x0,y0,width,height,znear,zfar, window_coords=None):
znear = float(znear)
zfar = float(zfar)
depth = zfar - znear
q = -(zfar + znear) / depth
qn = -2 * (zfar * znear) / depth
if window_coords=='y up':
proj = np.array([[ 2*K[0,0]/width, -2*K[0,1]/width, (-2*K[0,2]+width+2*x0)/width, 0 ],
[ 0, -2*K[1,1]/height,(-2*K[1,2]+height+2*y0)/height, 0],
[0,0,q,qn], # This row is standard glPerspective and sets near and far planes.
[0,0,-1,0]]) # This row is also standard glPerspective.
else:
assert window_coords=='y down'
proj = np.array([[ 2*K[0,0]/width, -2*K[0,1]/width, (-2*K[0,2]+width+2*x0)/width, 0 ],
[ 0, 2*K[1,1]/height,( 2*K[1,2]-height+2*y0)/height, 0],
[0,0,q,qn], # This row is standard glPerspective and sets near and far planes.
[0,0,-1,0]]) # This row is also standard glPerspective.
return proj
-189.758587117 276.29138676 -96.3787988728 383.130606338
111.508897788 222.066098541 192.868335852 142.494257376
0.212969663417 0.383074601861 -0.234486761804 1.0
from __future__ import division
import sympy
from sympy import Symbol, Matrix, sqrt, latex, lambdify
from sympy.utilities.lambdify import lambdastr
from sympy.solvers import solve
import numpy as np
def eye(sz):
rows = []
for i in range(sz):
row = []
for j in range(sz):
if i==j:
row.append( 1 )
else:
row.append( 0 )
rows.append(row)
return Matrix(rows)
for window_coords in ['y up','y down']:
width = Symbol('width')
height = Symbol('height')
# We start with "eye coordinates" (the coordinates of objects in
# the camera's coordinate system).
x_e = Symbol('x_e')
y_e = Symbol('y_e')
z_e = Symbol('z_e')
w_e = Symbol('w_e')
eye_hz = Matrix([[x_e],
[y_e],
[z_e],
[w_e]])
# HZ and OpenGL have different coordinate systems. We deal with
# that in our eye coordinates. This way, an object viewed with a
# standard HZ camera with +Y going down at looking at +Z will have
# different eye coordinates as an object in OpenGL, but it will
# "look" the same in the sense that when the camera is pointed at
# the object, it will be up on the image plane will be up in both
# cases. ("Pointing" and "up" meaning different things, as
# specified by the coordinate system.)
coord_xform = eye(4)
coord_xform[1,1]=-1 # flip Y coordinate (HZ camera has +y going down, GL has +y going up)
coord_xform[2,2]=-1 # flip Z coordinate (HZ camera looks at +z, GL looks at -z)
eye_gl = coord_xform*eye_hz
# Create a sympy model of the OpenGL pipeline.
if 1:
# glp = the GL Projection matrix
glp00 = Symbol('glp00')
glp01 = Symbol('glp01')
glp02 = Symbol('glp02')
glp11 = Symbol('glp11')
glp12 = Symbol('glp12')
znear = Symbol('znear')
zfar = Symbol('zfar')
depth = zfar - znear
q = -(zfar + znear) / depth
qn = -2 * (zfar * znear) / depth
# We define the matrix with zeros where we don't need
# values. This simplification allows us to solve analytically
# for the remaining values.
glp = Matrix([[glp00, glp01, glp02, 0 ],
[ 0, glp11, glp12, 0 ],
[ 0, 0, q, qn ], # This row is standard glPerspective and sets near and far planes.
[ 0, 0, -1, 0 ]]) # This row is also standard glPerspective.
if 1:
# Take the eye coordinates and create clip coordinates from
# them.
clip = glp*eye_gl
if 1:
# Now take the clip coordinates and create normalized device
# coordinates.
NDC = Matrix([[ clip[0,0] / clip[3,0] ],
[ clip[1,0] / clip[3,0] ],
[ clip[2,0] / clip[3,0] ]])
if 1:
# Finally, model the glViewport transformation.
if 1:
x0 = Symbol('x0')
y0 = Symbol('y0')
else:
x0 = 0
y0 = 0
window_gl = Matrix([[ (NDC[0,0] + 1)*(width/2)+x0 ],
[ (NDC[1,0] + 1)*(height/2)+y0 ],
# TODO: there must be something for Z
])
# The HZ pipeline is much simpler - a single matrix
# multiplication.
if 1:
# intrinsic matrix (upper triangular with last entry 1)
K00 = Symbol('K00')
K01 = Symbol('K01')
K02 = Symbol('K02')
K11 = Symbol('K11')
K12 = Symbol('K12')
K = Matrix([[K00, K01, K02],
[ 0, K11, K12],
[ 0, 0, 1]])
if 1:
eye3=eye_hz[:3,0]
window_hz_h = K*eye3
if window_coords=='y up':
window_hz = Matrix([[ window_hz_h[0,0]/window_hz_h[2,0] ],
[ window_hz_h[1,0]/window_hz_h[2,0] ]])
else:
assert window_coords=='y down'
window_hz = Matrix([[ window_hz_h[0,0]/window_hz_h[2,0] ],
[ height - window_hz_h[1,0]/window_hz_h[2,0] ]])
# Now, using all the above, solve for the entries in the GL
# projection matrix. (If I knew more sympy, this could doubtless
# be done in a single solve step with multiple equations instead
# of the solve and substitute approach I'm taking here.)
# sympy solves expressions by creating an equation where the left
# hand side is your expression and the right hand size is zero.
# The expression to solve for "window_gl[0,0] == window_hz[0,0]"
# is thus:
expr = window_gl[0,0] - window_hz[0,0]
e2 = expr.subs( {x_e: 0, y_e: 0, z_e: 1})
glp02_expr = solve(e2, glp02)
assert len(glp02_expr)==1
glp02_expr = glp02_expr[0]
e2 = expr.subs( {x_e: 0, y_e: 1, z_e: 1, glp02: glp02_expr})
glp01_expr = solve(e2, glp01)
assert len(glp01_expr)==1
glp01_expr = glp01_expr[0]
e2 = expr.subs( {x_e: 1, y_e: 0, z_e: 1, glp01: glp01_expr, glp02: glp02_expr})
glp00_expr = solve(e2, glp00)
assert len(glp00_expr)==1
glp00_expr = glp00_expr[0]
expr = window_gl[1,0] - window_hz[1,0]
e2 = expr.subs( {x_e: 0, y_e: 0, z_e: 1})
glp12_expr = solve(e2, glp12)
assert len(glp12_expr)==1
glp12_expr = glp12_expr[0]
e2 = expr.subs( {x_e: 0, y_e: 1, z_e: 1, glp12: glp12_expr})
glp11_expr = solve(e2, glp11)
assert len(glp11_expr)==1
glp11_expr = glp11_expr[0]
GLP = Matrix([[ glp00_expr, glp01_expr, glp02_expr, 0],
[ 0, glp11_expr, glp12_expr, 0],
[ 0, 0, q, qn ], # This row is standard glPerspective and sets near and far planes.
[ 0, 0, -1, 0 ]]) # This row is also standard glPerspective.
print 'window_coords=',repr(window_coords)
print GLP
@astraw
Copy link
Author

astraw commented Nov 5, 2011

See my blog post for a full description.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment