Perspective projection / camera model

from pylab import *
from quaternion import Quat

## 3D coordinates of our points in the "world" reference frame.

## This is a cube, plus the origin
r = array([
    [-1,-1,-1],[-1,-1, 1],[-1, 1,-1],[-1, 1, 1],
    [ 1,-1,-1],[ 1,-1, 1],[ 1, 1,-1],[ 1, 1, 1],
    [0,0,0]])

## The camera position (translation vector)
t = array([1.5,0.5,-4.0])
## The camera orientation, as a quaternion. We caalculate is so it
## points to the origin, and is "upright"
psi = Quat(0,sin(arctan2(t[0],t[2])/2),0) * \
      Quat(sin(arctan2(t[1],sqrt(t[0]**2+t[2]**2))/2),0,0)
## Rotation matrix from the quaternion
R = psi.rot()

## Pontos no referencial da camera
rc = dot(r-t, R)

## Intrinsic parameters
oc = array([320,240]) ## Projection and distortion center.
fd = 600          ## focal distance

## Apply perspective distortion
q = rc[:,:2] * fd / rc[:,[2,2]]

## Calculate final image coordinates,
rho2 = (q[:,0]**2 + q[:,1]**2)
kappa = 0.0
p = q / c_[2*[1.0/sqrt(1+kappa*rho2)]].T + oc

ion()
subplot(1,2,1)
title('Points in camera reference frame')
plot(rc[:,0], rc[:,1], 'b+')
axis('equal')
grid()

## Plot projected points
subplot(1,2,2)
title('Projected coordinates on the image plane')
plot(p[:,0], p[:,1], 'b+')
## Plot the cube edges
conn = [[0,1],[0,2],[0,4],[1,3],[1,5],[2,3],
        [2,6],[3,7],[4,5],[4,6],[5,7],[6,7]]
for cc in conn:
    plot(p[cc,0], p[cc,1], 'b-')
axis('equal')
grid()