In [1]:

    

from numpy import *
from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
import pygame, pygame.image
from pygame.locals import *
import pickle


    

In [2]:

    

width, height = 1000, 747


    

In [3]:

    

def set_projection_from_camera(K, p=[0, 0, 0]):
    """ Set view from a camera calibration matrix. """
    
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    
    fx = K[0, 0]
    fy = K[1, 1]
    fovy = 2*arctan(0.5*height/fy)*180/pi
#    aspect= float(width*fy)/(height*fx)
    aspect= (width*fy)/(height*fx)
    
    # define the near and far clipping planes
    near = 0.1
    far = 100.0
    
    # set perspective
    gluPerspective(fovy, aspect, near, far)
    glViewport(p[0], p[1], width, height)


    

In [11]:

    

def set_modelview_from_camera(Rt):
    """ Define the camera modelview matrix from camera postion """
    
    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()
    
    # rotate teapot 90 deg around x-axis so that z-axis is up
    Rx = array([[1,0,0],[0,0, -1],[0,1,0]])
    
    # set rotation to best approximation
    R = Rt[:, :3]
    U, S, V = linalg.svd(R)
    R = dot(U, V)
    R[0, :] = -R[0,:] # change sign of x-axis
    
    # set translation
    t = Rt[:, 3]
    
    # setup 4*4 matrix view matrix
    M = eye(4)
    M[:3, :3] = dot(R, Rx)
    M[:3, 3] = t
    
    # transpose and flatten to get column order
    M = M.T
    m = M.flatten()
    
    # replace model view with the new matrix
    glLoadMatrixf(m)


    

In [5]:

    

def draw_background(imname):
    """ Draw background image using a quad.  """
    
    # load background image (should be .bmp) to OpenGL texture
    bg_image = pygame.image.load(imname).convert()
    bg_data = pygame.image.tostring(bg_image,"RGBX",1)
    
    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
    
    # bind the texture
    glEnable(GL_TEXTURE_2D)
    glBindTexture(GL_TEXTURE_2D, glGenTextures(1))
    glTexImage2D(GL_TEXTURE_2D,0,GL_RGBA,width,height,0,GL_RGBA,GL_UNSIGNED_BYTE,bg_data)
    glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_NEAREST)
    glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_NEAREST)
    
    # create quad to fill the whole window
    glBegin(GL_QUADS)
    glTexCoord2f(0.0, 0.0); glVertex3f(-1.0, -1.0, -1.0)
    glTexCoord2f(1.0, 0.0); glVertex3f( 1.0, -1.0, -1.0)
    glTexCoord2f(1.0, 1.0); glVertex3f( 1.0,  1.0, -1.0)
    glTexCoord2f(0.0, 1.0); glVertex3f(-1.0,  1.0, -1.0)
    glEnd()
    
    # clear the texture
    glDeleteTextures(1)


    

In [6]:

    

def draw_teapot(size):
    """ Draw a red teapot at the origin. """
    glEnable(GL_LIGHTING)
    glEnable(GL_LIGHT0)
    glEnable(GL_DEPTH_TEST)
    glClear(GL_DEPTH_BUFFER_BIT)
    
    #draw red teapot
    glMaterialfv(GL_FRONT, GL_AMBIENT, [0, 0, 0, 0])
    glMaterialfv(GL_FRONT, GL_DIFFUSE, [0.5, 0.0, 0.0, 0.0])
    glMaterialfv(GL_FRONT, GL_SPECULAR, [0.7, 0.6, 0.6, 0.0])
    glMaterialf(GL_FRONT, GL_SHININESS, 0.25*128.0)
    glutInit()
    glutSolidTeapot(size)


    

In [7]:

    

def setup():
    """ Setup window and pygame environment. """
    
    pygame.init()
    pygame.display.set_mode((width, height), OPENGL | DOUBLEBUF)
    pygame.display.set_caption('OpenGL AR demo')


    

In [8]:

    

def cube_points(c, wid):
    """ Creates a list of points for plotting
    a cube with plot. (the first 5 points are
    the bottom square, some sides repeated). """
    
    p=[]
    #bottom
    p.append([c[0]-wid,c[1]-wid,c[2]-wid])
    p.append([c[0]-wid,c[1]+wid,c[2]-wid])
    p.append([c[0]+wid,c[1]+wid,c[2]-wid])
    p.append([c[0]+wid,c[1]-wid,c[2]-wid])
    p.append([c[0]-wid,c[1]-wid,c[2]-wid]) # same as first to close plot

    #top
    p.append([c[0]-wid,c[1]-wid,c[2]+wid])
    p.append([c[0]-wid,c[1]+wid,c[2]+wid])
    p.append([c[0]+wid,c[1]+wid,c[2]+wid])
    p.append([c[0]+wid,c[1]-wid,c[2]+wid])
    p.append([c[0]-wid,c[1]-wid,c[2]+wid]) # same as first to close plot
    
    #vertical sides
    p.append([c[0]-wid,c[1]-wid,c[2]+wid])
    p.append([c[0]-wid,c[1]+wid,c[2]+wid])
    p.append([c[0]-wid,c[1]+wid,c[2]-wid])
    p.append([c[0]+wid,c[1]+wid,c[2]-wid])
    p.append([c[0]+wid,c[1]+wid,c[2]+wid])
    p.append([c[0]+wid,c[1]-wid,c[2]+wid])
    p.append([c[0]+wid,c[1]-wid,c[2]-wid])
    
    return array(p).T


    

In [13]:

    

#load camera data
with open('ar_camera.pkl', 'r') as f:
    K = pickle.load(f)
    Rt = pickle.load(f)

import camera
import homography
from pylab import *

setup()
draw_background('book_perspective.bmp')
set_projection_from_camera(K)

points = array([[xx, yy, 0, 1] for xx in arange(-0.25, 0.26, 0.25) for yy in arange(-0.25, 0.26, 0.25)])
#print points
offsets = dot(Rt, points.T)
#print offsets.T
#print 'Rt  : ', Rt
for offset in offsets.T:
    Rnew = Rt.copy()
    Rnew[0][3]  = -offset[0] # x-axis sign is opposite for OpenGL
    Rnew[1][3]  = offset[1]
    Rnew[2][3]  = offset[2]
    set_modelview_from_camera(Rnew)
    draw_teapot(0.05)

while True:
    event = pygame.event.poll()
    if event.type in (QUIT, KEYDOWN):
        break
    pygame.display.flip() 
pygame.display.quit()


    

In [ ]: