computing the OpenGL projection matrix from intrinsic camera parameters

calib_test_numpy.py

#!/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()

calib_test_pyglet.py

#!/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()

calib_test_utils.py

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

cameramatrix.txt

-189.758587117 276.29138676 -96.3787988728 383.130606338
111.508897788 222.066098541 192.868335852 142.494257376
0.212969663417 0.383074601861 -0.234486761804 1.0

projection_math.py

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

See my blog post for a full description.